bbiwarg-original/bbiwarg/Utility/Kalman2DPositionFilter.cs

using Emgu.CV;
using Emgu.CV.Structure;

namespace BBIWARG.Utility
{
    /// <summary>
    /// Filter used to smooth a series of 2d positions.
    /// </summary>
    internal class Kalman2DPositionFilter
    {
        /// <summary>
        /// number of measurements per second
        /// </summary>
        private float fps;

        /// <summary>
        /// the emgu kalman filter
        /// </summary>
        private Kalman kalman;

        /// <summary>
        /// xx entry for the measurement noise covariance matrix
        /// </summary>
        private float mXX;

        /// <summary>
        /// xy and yx entry for the measurement noise covariance matrix
        /// </summary>
        private float mXY;

        /// <summary>
        /// yy entry for the measurement noise covariance matrix
        /// </summary>
        private float mYY;

        /// <summary>
        /// value used for all entries in the process noise covariance matrix
        /// </summary>
        private float processNoiseFactor;

        /// <summary>
        /// true iff the kalman filter is initialized
        /// </summary>
        public bool Initialized { get; private set; }

        /// <summary>
        /// Creates a Kalman2DPositionFilter.
        /// </summary>
        /// <param name="mXX">xx entry for the measurement noise covariance matrix</param>
        /// <param name="mXY">xy and yx entry for the measurement noise covariance matrix</param>
        /// <param name="mYY">yy entry for the measurement noise covariance matrix</param>
        /// <param name="processNoiseFactor">value used for all entries in the process noise covariance matrix</param>
        /// <param name="fps">number of measurements per second</param>
        public Kalman2DPositionFilter(float mXX, float mXY, float mYY, float processNoiseFactor = 1.0e-4f, int fps = 30)
        {
            this.mXX = mXX;
            this.mXY = mXY;
            this.mYY = mYY;
            this.processNoiseFactor = processNoiseFactor;
            this.fps = fps;
            reset();
        }

        /// <summary>
        /// Computes a smoothed position for a measurement and updates the filter.
        /// </summary>
        /// <param name="rawPosition">the measurement</param>
        /// <returns>the smoothed position</returns>
        public Vector2D getCorrectedPosition(Vector2D rawPosition)
        {
            Matrix<float> rawPositionMatrix = new Matrix<float>(new float[,] { { rawPosition.X }, { rawPosition.Y } });

            // prediction according to model
            kalman.Predict();

            // corrected point
            Matrix<float> estimate = kalman.Correct(rawPositionMatrix);
            return new Vector2D(estimate[0, 0], estimate[1, 0]);
        }

        /// <summary>
        /// Computes a prediction for the next position based on the previous positions.
        /// </summary>
        /// <returns>prediction for the next position</returns>
        public Vector2D getPrediction()
        {
            Matrix<float> predicton = kalman.Predict();
            return new Vector2D(predicton[0, 0], predicton[1, 0]);
        }

        /// <summary>
        /// Resets the kalman filter.
        /// </summary>
        public void reset()
        {
            // 6 state variables and 2 measurements (0 controls)
            kalman = new Kalman(2, 2, 0);

            // time step (s)
            float t = 1 / fps;

            // transition matrix
            Matrix<float> transitionMatrix = new Matrix<float>(new float[,]
            {
                { 1.0f, 0.0f },
                { 0.0f, 1.0f }
            });
            kalman.TransitionMatrix = transitionMatrix;

            // measurement matrix
            Matrix<float> measurementMatrix = new Matrix<float>(new float[,]
            {
                { 1.0f, 0.0f }, // first measurement = x
                { 0.0f, 1.0f }  // second measurement = y
            });
            kalman.MeasurementMatrix = measurementMatrix;

            // measurement noise covariance matrix
            Matrix<float> measurementNoiseCovarianceMatrix = new Matrix<float>(2, 2);

            measurementNoiseCovarianceMatrix[0, 0] = mXX;
            measurementNoiseCovarianceMatrix[0, 1] = measurementNoiseCovarianceMatrix[1, 0] = mXY;
            measurementNoiseCovarianceMatrix[1, 1] = mYY;

            kalman.MeasurementNoiseCovariance = measurementNoiseCovarianceMatrix;

            // process noise covariance matrix
            Matrix<float> processNoiseCovarianceMatrix = new Matrix<float>(2, 2);
            processNoiseCovarianceMatrix.SetIdentity(new MCvScalar(processNoiseFactor));
            kalman.ProcessNoiseCovariance = processNoiseCovarianceMatrix;

            // error covariance post matrix (initial value)
            Matrix<float> errorCovariancePostMatrix = new Matrix<float>(2, 2);
            errorCovariancePostMatrix.SetIdentity(new MCvScalar(processNoiseFactor));
            kalman.ErrorCovariancePost = errorCovariancePostMatrix;

            Initialized = false;
        }

        /// <summary>
        /// Sets the initial position.
        /// </summary>
        /// <param name="initialPosition">initial position</param>
        public void setInitialPosition(Vector2D initialPosition)
        {
            // initial state (x, y, v_x, v_y)
            Matrix<float> initialState = new Matrix<float>(new float[] { initialPosition.X, initialPosition.Y });
            kalman.CorrectedState = initialState;

            Initialized = true;
        }
    }
}