Kalman Filter

Kalman Filter

The purpose of this note is to give simple non technical introduction to Kalman filter.

Suppose we need to track position of the moving object. Our measurements can give us a position of this object with accuracy R. So every time we take a measurement we have

Xm = Xtrue + R

Where Xm is measured position, Xtrue is true position and R is measurement error. If there is no other source of information about this object then we should accept Xm as an estimate of the object position.

But suppose we know something about time object, for example the estimate of its previous position and the estimate of its velocity. By assuming that object velocity does not change we can estimate its position

Xmodel = Xprev + Velosity*dt + Q

Xmodel – an estimate of object position according to our model

Xprev – estimate of previous position

Q – model estimation error (we know that our model is not perfect)

The key idea of Kalman filter is to combine measurement and the model.

Xnext = Xmodel + K*(Xm - Xmodel)

This equation is basically linear combination of model prediction and measurement. If parameter K = 1 (called Kalman gain) then Xnext=Xm (i.e. we absolutely trust our measurements and don’t trust our model).

We choose value of K that minimizes variance (i.e. uncertainty) of Xnext

D[Xnext] = D[Xmodel]*(1-K)^2 + K^2*D[Xm]

D[x] – is variance or dispersion of x, D[x] = E[(x-E[x])^2] where E[x] – expectation of x.

From equations above we have:

D[Xm] = D[R]

D[Xmodel] = D[Xprev] + D[Q]

Minimum of D[Xnext] is achieved when K = D[Xmodel] / (D[Xmodel] + D[R])

(Note that D[Xmodel] = D[Xprev]+D[Q])

This agrees with our intuition, when there is no uncertainty about measurements D[R] = 0 that K = 1 and we completely ignore model prediction in computing an estimate for the Xnext.

Substituting the value of K into the equation for variance of Xnext we find:

D[Xnext] = (1-K) * D[Xprev]

Let’s put it all together. Kalman filters measurements based on the model predictions. In order to make it work we need to specify a model as well as accuracy of the model (Q) and measurements (R).

1. Specify R, Q, Xprev and D[Xprev] (Xprev and D[Xprev] are not critical since filter will automatically update these values),
2. Compute model prediction Xmodel
3. Take a measurement Xm
4. Compute Kalman gain K = (D[Xprev]+D[Q]) / (D[Xprev]+D[Q]+ D[R])
5. Correct measurement using model prediction Xnext = Xmodel + K*(Xm - Xmodel)
6. Compute D[Xnext] = (1-K) * D[Xprev]

If R=0 then filter absolutely trusts measurements and no adjustments will be made based on model predictions.

Even when Q=0 filter will not absolutely trust model since predicted value still contains an uncertainty of previous position.

Kalman, R. E. ( 1960). “A New Approach to Filtering and Prediction Problems.”

Transaction of the ASME Journal of Basic Engineering, 82(Series D), 35–45.