http://cylonrobot.blogspot.com/feeds/posts/default

Friday, October 01, 2004

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;
}
}
}