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;

        }

    }

}