Intro4u2u

KALMAN FILTERS

The purpose of a Kalman filter is to estimate the state of a system from meaurements which contain random errors. An example is estimating the position and velocity of a satelite from radar data. There are 3 components of position and 3 of velocity so there are at least 6 variables to estimate. These variables are called state variables. With 6 state variables the resulting Kalman filter is called a 6 dimensional Kalman filter.

In this introduction, we will start with a much simpler problem. We will start with a one dimensional Kalman filter. In other words we will assume we have only one variable to estimate.

This is a much easier problem to understand than the higher dimensional Kalman filter. It is very instructive to study this simpler example because you can intuitively see the reasoning behind the Kalman filter. Later this intuitive understanding will help in grasping what is going on in the more complicated filters.

You will notice that since I am typing this with a text editor, I cannot use subscripts, superscripts, greek letters etc. If you would prefer to see these lessons in the more usual math format and notation send an e mail to:

PDJoseph@compuserve.com

I will E mail you the first the first 4 lessons typed in Word using the Microsoft equation editor.

INTRODUCTORY LESSON

The one dimensional Kalman Filter

Suppose we have a random variable x(t) whose value we want to estimate at certain times t0 ,t1, t2, t3, etc. Also, suppose we know that x(tk) satisfies a linear dynamic equation

x(tk+1) = Fx(tk) + u(k) (the dynamic equation)

In the above equation F is a known number. In order to work through a numerical example let us assume F= 0.9

Kalman assumed that u(k) is a random number selected by picking a number from a hat. Suppose the numbers in the hat are such that the mean of u(k) = 0 and the variance of u(k) is Q. For our numerical example, we will take Q to be 100.

u(k) is called white noise, which means it is not correlated with any other random variables and most especially not correlated with past values of u.

In later lessons we will extend the Kalman filter to cases where the dynamic equation is not linear and where u is not white noise. But for this lesson, the dynamic equation is linear and w is white noise with zero mean.

Now suppose that at time t0 someone came along and told you he thought x(t0) = 1000 but that he might be in error and he thinks the variance of his error is equal to P. Suppose that you had a great deal of confidence in this person and were, therefore, convinced that this was the best possible estimate of x(t0). This is the initial estimate of x. It is sometimes called the a priori estimate.

A Kalman filter needs an initial estimate to get started. It is like an automobile engine that needs a starter motor to get going. Once it gets going it doesn’t need the starter motor anymore. Same with the Kalman filter. It needs an initial estimate to get going. Then it won’t need any more estimates from outside. In later lessons we will discuss possible sources of the initial estimate but for now just assume some person came along and gave it to you.

So we have an estimate of x(t0),which we will call xe. For our example

xe = 1000.

The variance of the error in this estimate is defined by

P = E [(x(t0) -xe)2]

where E is the expected value operator. x(t0) is the actual value of x at time t0 and xe is our best estimate of x. Thus the term in the parentheses is the error in our estimate.

For the numerical example, we will take P = 40,000.

Now we would like to estimate x(t1). Remember that the first equation we wrote (the dynamic equation) was

x(tk+1) = Fx(tk) + u(k)

Therefore, for k=0 we have

x(t1) = Fx(t0) + u(0)

Dr. Kalman says our new best estimate of x(t1) is given by

newxe = Fxe (equation 1)

or in our numerical example 900.

Do you see why Dr. Kalman is right. We have no way of estimating u(0) except to use its mean value of zero. How about Fx(t0). If our initial estimate of x(t0) = 1000 was correct then Fx(t0) would be 900. If our initial estimate was high, then our new estimate will be high but we have no way of knowing whether our initial estimate was high or low (if we had some way of knowing that it was high than we would have reduced it). So 900 is the best estimate we can make. What is the variance of the error of this estimate?

newP = E [(x(t1) - newxe)2]

Substitute the above equations in for x(t1) and newxe and you get

newP = E [(Fx(t0) + u -Fxe)2]

= E[F2(x(t0) - xe)2 ]+ E u2 + 2F E (x(t0)- xe)*u]

The last term is zero because u is assumed to be uncorrelated with x(t0) and xe.

So, we are left with

newP = PF2 + Q (Equation 2)

For our example, we have

newP = 40,000 X .81 + 100 = 32,500

Now, let us assume we make a noisy measurement of x. Call the measurement y and assume y is related to x by a linear equation. (Kalman assumed that all the equations of the system are linear. This is called linear system theory. In later lessons, we will extend the Kalman filter to non-linear systems.)

y(1) = Mx(t1) + w(1)

where w is white noise. We will call the variance of w, “R”.

M is some number whose value we know. We will use for our numerical example M = 1 , R= 10,000 and y(1) = 1200

Notice that if we wanted to estimate y(1) before we look at the measured value we would use

ye = M*newxe

for our numerical example we would have ye = 900

Dr. Kalman says the new best estimate of x(t1) is given by

newerxe = newxe + K*(y(1) -M*newxe)

= newxe + K*(y(1) - ye) (equation 3)

where K is a number called the Kalman gain.

Notice that y(1) - ye is just our error in estimating y(1). For our example, this error is equal to plus 300. Part of this is due to the noise, w and part to our error in estimating x.

If all the error were due to our error in estimating x, then we would be convinced that newxe was low by 300. Setting K=1 would correct our estimate by the full 300. But since some of this error is due to w, we will make a correction of less than 300 to come up with newerxe. We will set K to some number less than one.

What value of K should we use? Before we decide, let us compute the variance of the resulting error

E (x(t1) - newerxe)2 = E [x - newxe - K(y - M newxe)]2

= E [(x - newxe - K(Mx + w - M newxe)]2

= E [{(1 - KM)(x - newxe)2 +Kw}]2

= newP(1 - KM)2 + RK2

where the cross product terms dropped out because w isassumed to be uncorrelated with x and newxe. So the newer value of the variance is now given by

newerP = newP(1 - KM) 2 + R(K2) (equation 5)

If we want to minimize the estimation error we should minimize newerP. We do that by differentiating newerP withrespect to K and setting the derivative equal to zero and then solving for K. A little algebra shows that the optimal K is given by

K = M newP/[newP(M2) + R] (Equation 4)

For our example, K = .7647

newerxe = 1129

newerP = 7647

Notice that the variance of our estimation error is decreasing.

These are the five equations of the Kalman filter. At time t2, we start again using newerxe to be the value of xe to insert in equation 1 and newerP as the value of P in equation 2. Then we calculate K from equation 4 and use that along with the new measurement, y(2), in equation 3 to get another estimate of x and we use equation 5 to get the corresponding P. And this goes on computer cycle after computer cycle.

In the multi-dimensional Kalman filter, x is a column matrix with many components. For example if we were determining the orbit of a satellite, x would have 3 components corresponding to the position of the satellite and 3 more corresponding to the velocity plus other components corresponding to other random variables. Then equations 1 through 5 would become matrix equations and the simplicity and intuitive logic of the Kalman filter becomes obscured. The remaining lessons deal with the more complex nature of the multi-dimensional Kalman filter.