bbiwarg-original/bbiwarg/Utility/KalmanFilter.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

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

namespace bbiwarg.Utility
{
    class KalmanFilter
    {
        public KalmanFilter()
        {
            Kalman kalman = new Kalman(4, 2, 0);

            // initial state
            Matrix<float> initialState = new Matrix<float>(new float[] { 0.0f, 0.0f, 0.0f, 0.0f});
            kalman.CorrectedState = initialState;

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

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

            // measurement noise covariance matrix
            Matrix<float> measurementNoiseCovarianceMatrix = new Matrix<float>(2, 2);
            measurementNoiseCovarianceMatrix.SetIdentity(new MCvScalar(1.0e-1));
            kalman.MeasurementNoiseCovariance = measurementNoiseCovarianceMatrix;

            // process noise covariance matrix
            Matrix<float> processNoiseCovarianceMatrix = new Matrix<float>(4, 4);
            processNoiseCovarianceMatrix.SetIdentity(new MCvScalar(1.0e-4));
            kalman.ProcessNoiseCovariance = processNoiseCovarianceMatrix;

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


            Random random = new Random();
            int[] randomData = new int[70];

            for (int i = 0; i < 70; ++i)
            {
                int y = random.Next() % 21 - 10;
                randomData[i] = y;
            }

            int[] filteredData = new int[70];
            for (int i = 0; i < 70; ++i)
            {
                // random data (could be touch position)
                Matrix<float> data = new Matrix<float>(new float[,]
                { {0.0f},
                  {randomData[i]}});

                // prediction according to model
                Matrix<float> prediction = kalman.Predict();
            
                // estimated point
                Matrix<float> estimate = kalman.Correct(data);
                filteredData[i] = (int) estimate[1, 0];
            }


            Console.WriteLine("unfiltered: ");
            for (int i = 0; i < 21; ++i)
            {
                int val = 10 - i;
                for (int j = 0; j < 70; ++j)
                {
                    if (randomData[j] >= val)
                        Console.Write("#");
                    else
                        Console.Write(' ');
                }
                Console.WriteLine();
            }

            Console.WriteLine("filtered: ");
            for (int i = 0; i < 21; ++i)
            {
                int val = 10 - i;
                for (int j = 0; j < 70; ++j)
                {
                    if (filteredData[j] >= val)
                        Console.Write("#");
                    else
                        Console.Write(' ');
                }
                Console.WriteLine();
            }

            Console.Read();
        }
    }
}