EMGU Kalman Filter

Source Code [SourceForge]

Mouse Live Data Example

• x86 Source Code
• x64 Source Code

Calculated Data Example

• x86 Source Code
• x64 Source Code

Introduction

The following tutorial implements a simple Kalman Filter. The code is derived originally from and article witten by Roy on morethantechnical.com. It has been very kindly translated to C# EMGU by Usman Ashraf and Kevin Chow.

This article is follow up article in the explaination of the use of the EMGU image processing wrapper. For more information on the EMGU wrapper please visit the EMGU website . If you are new to this wrapper see the Creating Your First EMGU Image Processing Project article. As you will start with 3 warnings for the references not being found. Expand the References folder within the solution explorer delete the 3 with yellow warning icons and Add fresh references to them.

The Kalman filter is an algorithm which operates recursively on streams of noisy input data to produce a statistically optimal estimate of the underlying system state (Original Paper). The filter is named for Rudolf (Rudy) E. Kálmán, one of the primary developers of its theory. More information is available at Wikipedia, the Kalmn Filter was derived to solve the Wiener filter problem. The Wiener filter problem is to reduce the amount of noise present in a signal by comparison with an estimation of the desired noiseless signal. The discrete-time equivalent of Wiener's work was derived independently by Kolmogorov and published in 1941. Hence the theory is often called the Wiener-Kolmogorov filtering theory.

In this example the signal processes is the movement of the mouse. While a simple application the algorithm can have numerous applications including image smoothing, edge tracking and optical flow to name a few. This tutorial is designed to give a rather basic introduction to the filter design. The left screen shot is the basic mouse tracking example. The right screen shot is the data processing example.

Assumed Knowledge

The following tutorial is of an intermediate level. It is assumed that you can set up your own project and reference the appropriate files accordingly. For reference to errors or if you are a beginner then a good starting point is here: Creating Your First EMGU Image Processing Project article.

The Basics of the Kalman Filter

This explanation is taken from this video. It explains the Kalman filter in a simple way and this following section transcribes this to this particular application.

Imagine in our case the mouse pointer. It has a known current position denoted by Χ_τ-1, and it’s position is going to change by an unknown factor µ. The mouse will move to a next state of ̅Χ_τ, however we will incorporate some form of tracking information. In this case we assume that where the mouse thinks it is, actually, isn’t correct. In fact the location the computer says it is, is at the point Χ_Comp. It is safe to assume that the mouse is located somewhere at a vector point between ̅Χ_τ, and Χ_Comp . This is called our estimate position and is what where interested in Χ_EST.

This is what the Kalman filter works out for us in a very efficient way. There are a few states to this, the first is the State Prediction. We are trying to calculate the next state of the mouse ̅Χ_τ now we know the next state will be a function of the previous state (Χ_τ-1) and the movement (µ). The first assumption of the Kalman filter is that this change is linear producing the equation bellow.

̅Χ_τ = ΑΧ_τ-1 + Βµ

We also have state error estimation which we must take into account this error is assumed to have a Gaussian distribution (ε_τ). These assumption are not always true for a number of systems but similarly they are true for many systems as well. The resulting equation is therefore:

̅Χ_τ = ΑΧ_τ-1 + Βµ + ε_τ

The second state is the sensor prediction. As we have some form of prediction for where the mouse thinks it is going to be, it stands to reason that we should have some form of prediction to where the computer is going to think the mouse position will be. This will be ̅Ζ_τ. This is going to be some function of our state prediction and will also have some error which again is assumed to have a Gaussian distribution.

̅Ζ_τ = C̅Χ_τ +ε_τ

So the whole idea of the Kalman filter boils down to the estimated state (Χ_EST) that needs to be calculated, being a linear function of the predicted state (̅Χ_τ) . In addition to the difference between the actual measurement that the computer states the mouse pointer is at (Χ_Comp) and the predicted measurement of where the computer was going to track the mouse position to (̅Ζ_τ) multiplied by some gain factor called the Kalman gain (κ). This leaves us with our final equation bellow

Χ_EST = ̅Χ_τ + κ( Χ_Comp - ̅Ζ_τ)

So what does all this mean, well if the prediction of mouse position according to the computer is accurate then Χ_Comp - ̅Ζ_τ = 0, and the prediction of the estimated position is ideal. But if the prediction of the mouse position according to the computer is wrong an error may have been made (i.e. noise). The prediction for the estimated position is then corrected by some factor according to the Kalman Gain allowing for a more accurate estimate. The Kalman Gain is therefore considered a correction term

The Code

Setting Up the Kalman Filter

In the source provided the Kalman filter is initialised after the Form1 InitializeComponent() method is called. The method were interested in is KalmanFilter()

public void KalmanFilter()
{
      mousePoints = new List<pointf>();
      kalmanPoints = new List<pointf>();
      kal = new Kalman(4, 2, 0);
      syntheticData = new SyntheticData();
      Matrix<float> state = new Matrix<float>(new float[]
      {
            0.0f, 0.0f, 0.0f, 0.0f
      });
      kal.CorrectedState = state;
      kal.TransitionMatrix = syntheticData.transitionMatrix;
      kal.MeasurementNoiseCovariance = syntheticData.measurementNoise;
      kal.ProcessNoiseCovariance = syntheticData.processNoise;
      kal.ErrorCovariancePost = syntheticData.errorCovariancePost;
      kal.MeasurementMatrix = syntheticData.measurementMatrix;
}

The method above simply provides the Kalman filter with details of the quantity of variables it will be tracking and while “kal = new Kalman(4, 2, 0);” is important it simply states that 4 dynamic variables are tracked with 2 Measurement Variables. The “syntheticData” variable is a collection of Matrices used to calculate the noise present within the data and process the noise out of the data. SyntheticData variables are held with a class SyntheticData.cs. It is within this class that the Kalman filter is set up with 4 Dynamic parameters denoted by “state” with 2 measurement parameters. All the matrices of the synthetic data are passed to the Kalman filter so it can reference them in predicting next states. 

To fully understand the implementation and adjust the Kalman Filter SyntheticData.cs must be explored. The first thing to inspect is the initialisation of the SyntheticData,

public SyntheticData()
{
      state = new Matrix<float>(4, 1);
      state[0, 0] = 0f; // x-pos
      state[1, 0] = 0f; // y-pos
      state[2, 0] = 0f; // x-velocity
      state[3, 0] = 0f; // y-velocity
      transitionMatrix = new Matrix<float>(new float[,]
      {
            {1, 0, 1, 0},
            {0, 1, 0, 1},
            {0, 0, 1, 0},
            {0, 0, 0, 1}
      });
      measurementMatrix = new Matrix<float>(new float[,]
      {
            { 1, 0, 0, 0 },
            { 0, 1, 0, 0 }
      });
      measurementMatrix.SetIdentity();
      processNoise = new Matrix<float>(4, 4);
      processNoise.SetIdentity(new MCvScalar(1.0e-4));
      measurementNoise = new Matrix<float>(2, 2);
      measurementNoise.SetIdentity(new MCvScalar(1.0e-1));
      errorCovariancePost = new Matrix<float>(4, 4);
      errorCovariancePost.SetIdentity();
}

As can be observed a variable state is also allocated giving the four dimensions to be tracked. The X position of the mouse, the Y position of the mouse, the velocity change along the X access, and the velocity change along the Y access These four variables are usually enough for a majority of situations however the more variable that are tracked the potential for better suppression of noise.

The transition matrix provides the Kalman filter with the expected combinations of change between all variables. As we are tracking four variables it’s a 4x4 matrix. Each row of the matrix from left to right corresponds to X position, Y position, X velocity, Y velocity. In the design provided expected changes in position and velocity are an increase or decrease of 1. We can improve the Kalman filters noise suppression by reducing the values try replacing the transition matrix with the one below and you will see a higher resilience to noise.

transitionMatrix = new Matrix<float>(new float[,]
{
      {0.1F, 0,    1, 0},  // x-pos, y-pos, x-velocity, y-velocity
      {0,    0.1F, 0, 1},
      {0,    0,    1, 0},
      {0,    0,    0, 1}
}); 

Similarly you will start to observe a lower response to the velocity change in tracking the mouse. Altering the X and Y velocity figures further extenuates this. Using higher values produces a highly unstable system that fails to predict the correct values or suppress noise.

The variables set up in the constructer mainly deal with the design and only need to be adjusted when extra variables are added for tracking. One variable that is very useful is the noise identity value.

processNoise.SetIdentity(new MCvScalar(1.0e-4));

Adjusting these values can also effect the Kalman filters ability to suppress noise. The smaller value “processNoise.SetIdentity” is presented with the more resilient to noise the Kalman filter becomes over time.

Applying the Filter to Data

The data, in this application X and Y of the mouse position is applied to the Kalman filter using the “filterPoints” method call. This is called from the “KalmanFilterRunner” method which is a tick event for one of the timers. The “KalmanFilterRunner” method is responsible for calculating the Kalman Filter Output and also displaying the results. The “filterpoints” method allows faster porting of the application to other applications. The main method is essentially simple:

public PointF[] filterPoints(PointF pt)
{
      syntheticData.state[0, 0] = pt.X;
      syntheticData.state[1, 0] = pt.Y;
      Matrix<float> prediction = kal.Predict();
      PointF predictPoint = new PointF(prediction[0, 0], prediction[1, 0]);
      PointF measurePoint = new PointF(syntheticData.GetMeasurement()[0, 0],
      syntheticData.GetMeasurement()[1, 0]);
      Matrix<float> estimated = kal.Correct(syntheticData.GetMeasurement());
      PointF estimatedPoint = new PointF(estimated[0, 0], estimated[1, 0]);
      syntheticData.GoToNextState();
      PointF[] results = new PointF[2];
      results[0] = predictPoint;
      results[1] = estimatedPoint;
      px = predictPoint.X;
      py = predictPoint.Y;
      cx = estimatedPoint.X;
      cy = estimatedPoint.Y;
      return results;
}

The syntheticData current state is updated with the recorded position of the mouse pointer. The Kalman Filter is then called upon to predict the next state that the mouse is expected to be in and generate the Kalman Gain. From the set up of the filter the synthetic data matrices have already been referenced.

The “predictedPoint” is then the position prediction to where the computer is going to think the mouse position will be. The “mesurePoint” is used to generate a prediction for where the mouse thinks it is going to be a small amount of noise is added at this stage as there is a we expect that the actual position of the mouse and the predicted position are not identical. There would be no point of using the filter if this was true since there would be no noise to eliminate. Finally the Kalman Filter is used to Correct the points and produce the accurate ?_EST position.

The “syntheticData.GoToNextState()” is used to generate a next prediction state with noise for the Kalman filter to work from and devlop it’s noise suppression gain and correction from. This is the reason why the Kalman filter improves after a the first few measurements . With each pass the Kalman filter has an increase knowledge of the noise it’s dealing with.

Obtaining The mouse Co-Ordinates and up Setting the Data

The mouse co-ordinates are obtained using the MouseMove method available to Picturebox object. Alternative objects can be used as long as the mouse co-ordinates are available to the method call. Each time the mouse move over the picture box the global variables “ax” and “ay” are updated. These are the actual x co-ordinates (ax) recorded and the actual y co-ordinates recorded (ay). In the Kalman filter this translates as the position where the computer thinks the mouse is going to be.

The variables “ax” and “ay” are global variables and are only fed into the Kalman filter when the timer methods are called. The timers are an essential for viewing different filter effects when using live data.

Use of Timers

In this example timers are used to space the recording of the mouse position and the predict Kalman points. They are not required when analysing pre-recorded data, however in order to evaluate different filter design they are essential. The timer setup has been brought under its own method:

private void InitialiseTimers(int Timer_Interval = 1000)

The default timing is 1 second however this can be adjusted by altering the “Start_BTN_Click method from which the timer setup method is called bellow is an example reducing the timers to every 100 microseconds.

private void Start_BTN_Click(object sender, EventArgs e)
{
      if (Start_BTN.Text == "Start")
      {
            MouseTrackingArea.Refresh();
            InitialiseTimers(100);
            Start_BTN.Text  = "Stop";
      }
      else
      {
            StopTimers();
            Start_BTN.Text = "Start";
      }
}

The recorded position of the mouse is also only recorded with a timer. As discussed all positions of the mouse are stored within “ax” and “ay” global variables whenever the mouse moves over the Paintbox control however they are only recorded when “MousePositionRecord” timer tick method is called. This not really necessary and is simply allows comparison between the position that was fed into the Kalman filter and the prediction that the Kalman Filter produced.

Displaying the Results

The results are displayed by drawing them directly onto the Paintbox control. This does mean that if the program is minimised/maximised or losses focus the points will disappear. The positions can be drawn onto an Image directly if required by using the outpur provided when calling “public PointF[] filterPoints(PointF pt)“. This is a simple coding implementation in EMGU and is not implemented since it is not necessary.

Applying the Kalman Filter to Other Applications

Pre-Existing Data and graphical display

History

[1] 05/03/2012 Initial Article Complete with V1.0 x86 released.

[2] 07/03/2012 x64 Versions release and example included to work with alternative data.