# Difference between revisions of "Kalman Filter"

## 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 bojoeb.

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.

## 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: Setting up EMGU C#.

## 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 know current position denoted by Χτ-1, and it’s position is going to change by a unknown factor µ. The mouse will move to a next state of ̅Χτ, however we will incorporate some form of 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 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.

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.