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.
public Double Value = 0;
public Double Variance = 0;
public Estimate Add(Double value, Double variance)
Value += value;
Variance = Math.Sqrt(Sqr(Variance) + Sqr(variance));
public Estimate Update(Double value, Double variance)
Double denom = Sqr(Variance) + Sqr(variance);
if (Value == 0)
Value = value;
Variance = variance;
Sqr(Variance) / denom * value +
Sqr(variance) / denom * Value;
Math.Sqrt(1 / (1 / Sqr(Variance) + 1 / Sqr(variance)));