### Sample code for a Kalman filter

On the SRS Mailing list, the topic of how to code up a Kalman filter came up. I said I'd post some sample code of how I implemented a simple Kalman filter.

The core concept of a Kalman filter is that each "number" consists of two parts -- the value of the number and the variance, or uncertainty / error range of the number. When you add numbers together or update a number, you have to take both the value and the variance into effect to calculate the new "number" (which also has both a value and a variance).

In my case, I made a new structure called "Estimate" which contained two Double precision numbers, Value and Variance. I added methods to this "Estimate" structure Add and Update.

Add takes two Estimates and adds them together. For instance, you might use this if you had computed your X position to be 10 m +- 1.0 m and then you moved via dead reckoning 1.0 m +- 0.1 m.

Update is used to add another observed value from the same position. For instance, if you were sitting still taking 10 GPS readings in a row to try and get a better precision guess of your current location you would use Update with each new observation.

I don't guarantee this code is correct -- it's been too many years since my college math courses to know for sure. But it's my best guess, so use as you will.

- jcb

struct Estimate

{

public Double Value = 0;

public Double Variance = 0;

public Estimate Add(Double value, Double variance)

{

Value += value;

Variance = Math.Sqrt(Sqr(Variance) + Sqr(variance));

return this;

}

public Estimate Update(Double value, Double variance)

{

Double denom = Sqr(Variance) + Sqr(variance);

if (Value == 0)

{

Value = value;

Variance = variance;

return this;

}

Value =

Sqr(Variance) / denom * value +

Sqr(variance) / denom * Value;

Variance =

Math.Sqrt(1 / (1 / Sqr(Variance) + 1 / Sqr(variance)));

return this;

}

}

}

<< Home