Correct() public method

Adjusts stochastic model state on the basis of the given measurement of the model state
The function stores adjusted state at kalman->state_post and returns it on output
public Correct ( Matrix measurement ) : Matrix
measurement Matrix The measurement data
return Matrix
Kalman Class Documentation
Exemplo n.º 1
0
 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;
 }
Exemplo n.º 2
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();
        }