Example #1
0
        // Segundo Filtro de Kalman (um pouco melhor)
        #region FiltroKalman2(double valor)
        private double FiltroKalman2(double valor)
        {
            double retorno;

            Kalman1D k = new Kalman1D();

            k.Reset(
                (double)0.1,
                (double)0.1,
                (double)0.1,
                (double)400, 0);


            // Assume we get to see every other measurement we calculated, and use
            // the others as the points to compare for estimates.
            // Run the filter, note our time unit is 1.
            double[] kalman = new double[data_array_Y.Length];
            double[] vel    = new double[data_array_Y.Length];
            double[] kGain  = new double[data_array_Y.Length];

            for (int i = 0; i < data_array_Y.Length; i += 2)
            {
                if (i == 0)
                {
                    kalman[0] = 0;
                    vel[0]    = k.Velocity;
                    kGain[0]  = k.LastGain;
                    kalman[1] = k.Predicition(1);
                    vel[1]    = k.Velocity;
                    kGain[1]  = k.LastGain;
                }
                else
                {
                    kalman[i]     = k.Update(data_array_Y[i], 1);
                    kalman[i + 1] = kalman[i];
                    vel[i]        = k.Velocity;
                    kGain[i]      = k.LastGain;
                    vel[i + 1]    = vel[i];
                    kGain[i + 1]  = kGain[i];
                    //     kalman[i + 1] = k.Predicition(1);
                }
            }

            retorno = kalman[kalman.Length - 1];

            return(retorno);
        }
Example #2
0
        private double FiltroKalman2(double valor)
        {
            double retorno;

            Kalman1D k = new Kalman1D();
            k.Reset(
                (double)0.1,
                (double)0.1,
                (double)0.1,
                (double)400, 0);

            // Assume we get to see every other measurement we calculated, and use
            // the others as the points to compare for estimates.
            // Run the filter, note our time unit is 1.
            double[] kalman = new double[data_array_Y.Length];
            double[] vel = new double[data_array_Y.Length];
            double[] kGain = new double[data_array_Y.Length];

            for (int i = 0; i < data_array_Y.Length; i += 2)
            {
                if (i == 0)
                {
                    kalman[0] = 0;
                    vel[0] = k.Velocity;
                    kGain[0] = k.LastGain;
                    kalman[1] = k.Predicition(1);
                    vel[1] = k.Velocity;
                    kGain[1] = k.LastGain;
                }
                else
                {
                    kalman[i] = k.Update(data_array_Y[i], 1);
                    kalman[i + 1] = kalman[i];
                    vel[i] = k.Velocity;
                    kGain[i] = k.LastGain;
                    vel[i + 1] = vel[i];
                    kGain[i + 1] = kGain[i];
                    //     kalman[i + 1] = k.Predicition(1);
                }

            }

            retorno = kalman[kalman.Length - 1];

            return retorno;
        }