Kalman Filter
Наследование: DisposableObject
        //Setup Kalman Filter and predict methods
        public void InitializeKalmanFilter()
        {
            _kalmanXYZ = new Kalman(6, 3, 0);
            Matrix<float> state = new Matrix<float>(new float[]
                {
                    0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f
                });
            _kalmanXYZ.CorrectedState = state;
            _kalmanXYZ.TransitionMatrix = new Matrix<float>(new float[,]
                    {
                        // x-pos, y-pos, z-pos, x-velocity, y-velocity, z-velocity
                        {1, 0, 0, 1, 0, 0},
                        {0, 1, 0, 0, 1, 0},
                        {0, 0, 1, 0, 0, 1},
                        {0, 0, 0, 1, 0, 0},
                        {0, 0, 0, 0, 1, 0},
                        {0, 0, 0, 0, 0, 1}
                    });
            _kalmanXYZ.MeasurementNoiseCovariance = new Matrix<float>(3, 3); //Fixed accordiong to input data 
            _kalmanXYZ.MeasurementNoiseCovariance.SetIdentity(new MCvScalar(1.0e-1));

            _kalmanXYZ.ProcessNoiseCovariance = new Matrix<float>(6, 6); //Linked to the size of the transition matrix
            _kalmanXYZ.ProcessNoiseCovariance.SetIdentity(new MCvScalar(1.0e-4)); //The smaller the value the more resistance to noise 

            _kalmanXYZ.ErrorCovariancePost = new Matrix<float>(6, 6); //Linked to the size of the transition matrix
            _kalmanXYZ.ErrorCovariancePost.SetIdentity();
            _kalmanXYZ.MeasurementMatrix = new Matrix<float>(new float[,]
                    {
                        { 1, 0, 0, 0, 0, 0 },
                        { 0, 1, 0, 0, 0, 0 },
                        { 0, 0, 1, 0, 0, 0 }
                    });
        }
Пример #2
0
 private void initKalman()
 {
     last = lastEst = new System.Drawing.Point();
     kf = new Kalman(kfData.state,kfData.transitionMatrix, kfData.measurementMatrix, 
         kfData.processNoise, kfData.measurementNoise);
     kf.ErrorCovariancePost = kfData.errorCovariancePost;
 }
 public Matrix<float> FilterPoints(Kalman kalman, float x, float y, float z)
 {
     Matrix<float> prediction = kalman.Predict();
     Matrix<float> estimated = kalman.Correct(new Matrix<float>(new[] { x, y, z }));
     Matrix<float> results = new Matrix<float>(new[,]
     {
         { prediction[0, 0], prediction[1, 0], prediction[2, 0] },
         { estimated[0, 0], estimated[1, 0], estimated[2, 0] }
     });
     return results;
 }
Пример #4
0
        public Ball()
        {
            m_PreviousBallPositions = new List<PointF>();

            m_KalmanFilter = new Kalman(4, 2, 0);
            m_KalmanFilter.CorrectedState = new Matrix<float>(new float[] { 0f, 0f, 0f, 0f });
            m_KalmanFilter.TransitionMatrix = new Matrix<float>(new float[,] { { 1f, 0, 1, 0 }, { 0, 1f, 0, 1 }, { 0, 0, 0.85f, 0 }, { 0, 0, 0, 0.85f } });
            m_KalmanFilter.MeasurementNoiseCovariance = new Matrix<float>(new float[,] { { 0.0006f, 0 }, { 0, 0.0006f } });
            m_KalmanFilter.ProcessNoiseCovariance = new Matrix<float>(new float[,] { { 0.1f, 0, 0, 0 }, { 0, 0.1f, 0, 0 }, { 0, 0, 0.1f, 0 }, { 0, 0, 0, 0.1f } });
            m_KalmanFilter.ErrorCovariancePost = new Matrix<float>(new float[,] { { 1f, 0, 0, 0 }, { 0, 1f, 0, 0 }, { 0, 0, 1f, 0 }, { 0, 0, 0, 1f } });
            m_KalmanFilter.MeasurementMatrix = new Matrix<float>(new float[,] { { 1f, 0, 0, 0 }, { 0, 1f, 0, 0 } });
        }
Пример #5
0
        public KalmanFilter()
        {
            dataPoints = new List<PointF>();
                kalmanPoints = new List<PointF>();
                kalFilter = new Kalman(2, 2, 0);
                syntheticData = new SyntheticData();
                Matrix<float> state = new Matrix<float>(new float[]
                     {
                          0.0f, 0.0f
                     });

                kalFilter.CorrectedState = state;
                kalFilter.TransitionMatrix = syntheticData.transitionMatrix;
                kalFilter.MeasurementNoiseCovariance = syntheticData.measurementNoise;
                kalFilter.ProcessNoiseCovariance = syntheticData.processNoise;
                kalFilter.ErrorCovariancePost = syntheticData.errorCovariancePost;
                kalFilter.MeasurementMatrix = syntheticData.measurementMatrix;
        }
Пример #6
0
        // Инициализация фильтра Кальмана
        public KalmanFilter()
        {
            m_timeLastMeasurement = clock();
            int dynam_params = 4; // x,y,dx,dy
            int measure_params = 2;
            m_pKalmanFilter = cvCreateKalman(dynam_params, measure_params);
            state = cvCreateMat( dynam_params, 1, CV_32FC1 );

            // Генератор случайных чисел
            cvRandInit( &rng, 0, 1, -1, CV_RAND_UNI );
            cvRandSetRange( &rng, 0, 1, 0 );
            rng.disttype = CV_RAND_NORMAL;
            cvRand( &rng, state );

            process_noise = cvCreateMat( dynam_params, 1, CV_32FC1 ); // (w_k)
            measurement = cvCreateMat( measure_params, 1, CV_32FC1 ); // two parameters for x,y  (z_k)
            measurement_noise = cvCreateMat( measure_params, 1, CV_32FC1 ); // two parameters for x,y (v_k)
            cvZero(measurement);

            // F matrix data
            // F is transition matrix.  It relates how the states interact
            // For single input fixed velocity the new value
            // depends on the previous value and velocity- hence 1 0 1 0
            // on top line. Новое значение скорости не зависит от предыдущего
            // значения координаты, и зависит только от предыдущей скорости- поэтому 0 1 0 1 on second row
            const float[] F = {
                1, 0, 1, 0,//x + dx
                0, 1, 0, 1,//y + dy
                0, 0, 1, 0,//dx = dx
                0, 0, 0, 1,//dy = dy
            };
            memcpy( m_pKalmanFilter->transition_matrix->data.fl, F, sizeof(F));
            cvSetIdentity( m_pKalmanFilter->measurement_matrix, cvRealScalar(1) ); // (H)
            cvSetIdentity( m_pKalmanFilter->process_noise_cov, cvRealScalar(1e-5) ); // (Q)
            cvSetIdentity( m_pKalmanFilter->measurement_noise_cov, cvRealScalar(1e-1) ); // (R)
            cvSetIdentity( m_pKalmanFilter->error_cov_post, cvRealScalar(1));

            // choose random initial state
            cvRand( &rng, m_pKalmanFilter->state_post );

            //InitializeCriticalSection(&mutexPrediction);
        }
Пример #7
0
        private bool isInitialized; // true if any data has been fed

        public KalmanFilter(int variables)
        {
            variablesCount = variables;

            int measurementVariables = variables;
            int dynamicVariables = variables * 2;

            float[] state = new float[dynamicVariables];
            for (int i = 0; i < dynamicVariables; ++i)
                state[i] = 0.0f;

            Matrix<float> transitionMatrix = new Matrix<float>(dynamicVariables, dynamicVariables);
            transitionMatrix.SetZero();
            for (int i = 0; i < dynamicVariables; ++i)
            {
                transitionMatrix[i, i] = 1.0f;
                if (i >= measurementVariables)
                    transitionMatrix[i - measurementVariables, i] = 1;
            }

            Matrix<float> measurementMatrix = new Matrix<float>(measurementVariables, dynamicVariables);
            measurementMatrix.SetZero();
            for (int i = 0; i < measurementVariables; ++i)
                measurementMatrix[i, i] = 1.0f;

            Matrix<float> processNoise = new Matrix<float>(dynamicVariables, dynamicVariables);
            processNoise.SetIdentity(new MCvScalar(1));//1.0e-4));

            Matrix<float> measurementNoise = new Matrix<float>(measurementVariables, measurementVariables);
            measurementNoise.SetIdentity(new MCvScalar(4));//1.0e-1));

            Matrix<float> errorCovariancePost = new Matrix<float>(dynamicVariables, dynamicVariables);
            errorCovariancePost.SetIdentity();

            kalman = new Kalman(dynamicVariables, measurementVariables, 0);
            kalman.CorrectedState = new Matrix<float>(state);
            kalman.TransitionMatrix = transitionMatrix;
            kalman.MeasurementNoiseCovariance = measurementNoise;
            kalman.ProcessNoiseCovariance = processNoise;
            kalman.ErrorCovariancePost = errorCovariancePost;
            kalman.MeasurementMatrix = measurementMatrix;
        }
        private void inicializarKalman()
        {
            kal = new Kalman(4, 2, 0);
            kal.TransitionMatrix = new Matrix<float>(new float[4, 4] { {1, 0, 1, 0}, {0, 1, 0, 1}, {0, 0, 1, 0}, {0, 0, 0, 1} });

            kal.MeasurementMatrix.SetIdentity();
            kal.ProcessNoiseCovariance.SetIdentity(new MCvScalar(1e-4));
            kal.MeasurementNoiseCovariance.SetIdentity(new MCvScalar(1e-4));
            kal.ErrorCovariancePost.SetIdentity(new MCvScalar(1e-4));

            medidaKalman = new Matrix<float>(2, 1);
        }
Пример #9
0
        public void TestKalman()
        {
            Image<Bgr, Byte> img = new Image<Bgr, byte>(400, 400);

             SyntheticData syntheticData = new SyntheticData();

             // state is (phi, delta_phi) - angle and angle increment
             Matrix<float> state = new Matrix<float>(new float[] { 0.0f, 0.0f}); //initial guess

             #region initialize Kalman filter
             Kalman tracker = new Kalman(2, 1, 0);
             tracker.TransitionMatrix = syntheticData.TransitionMatrix;
             tracker.MeasurementMatrix = syntheticData.MeasurementMatrix;
             tracker.ProcessNoiseCovariance = syntheticData.ProcessNoise;
             tracker.MeasurementNoiseCovariance = syntheticData.MeasurementNoise;
             tracker.ErrorCovariancePost = syntheticData.ErrorCovariancePost;
             tracker.CorrectedState = state;
             #endregion

             System.Converter<double, PointF> angleToPoint =
            delegate(double radianAngle)
            {
               return new PointF(
                  (float)(img.Width / 2 + img.Width / 3 * Math.Cos(radianAngle)),
                  (float)(img.Height / 2 - img.Width / 3 * Math.Sin(radianAngle)));
            };

             Action<PointF, Bgr> drawCross =
               delegate(PointF point, Bgr color)
               {
              img.Draw(new Cross2DF(point, 15, 15), color, 1);
               };

             ImageViewer viewer = new ImageViewer();
             System.Windows.Forms.Timer timer = new System.Windows.Forms.Timer();
             timer.Interval = 200;
             timer.Tick += new EventHandler(delegate(object sender, EventArgs e)
             {
            Matrix<float> measurement = syntheticData.GetMeasurement();
            // adjust Kalman filter state
            tracker.Correct(measurement);

            tracker.Predict();

            #region draw the state, prediction and the measurement
            PointF statePoint = angleToPoint(tracker.CorrectedState[0, 0]);
            PointF predictPoint = angleToPoint(tracker.PredictedState[0, 0]);
            PointF measurementPoint = angleToPoint(measurement[0, 0]);

            img.SetZero(); //clear the image
            drawCross(statePoint, new Bgr(Color.White)); //draw current state in White
            drawCross(measurementPoint, new Bgr(Color.Red)); //draw the measurement in Red
            drawCross(predictPoint, new Bgr(Color.Green)); //draw the prediction (the next state) in green
            img.Draw(new LineSegment2DF(statePoint, predictPoint), new Bgr(Color.Magenta), 1); //Draw a line between the current position and prediction of next position

            //Trace.WriteLine(String.Format("Velocity: {0}", tracker.CorrectedState[1, 0]));
            #endregion

            syntheticData.GoToNextState();

            viewer.Image = img;
             });
             timer.Start();
             viewer.Disposed += delegate(Object sender, EventArgs e) { timer.Stop(); };
             viewer.Text = "Actual State: White; Measurement: Red; Prediction: Green";
             viewer.ShowDialog();
        }