A 'Simple' Example

For a 'simple' one dimensional example, we would like to use the following equations,

(8)

and

(9).

Weirdly, and sadly for us, the true acceleration is not a known quantity so equations (8) and (9) are fiction. All we can do is estimate it. The position and velocity are also quantities that are estimated by the filter and are also never really known. So how do we estimate the acceleration? In this example we will model the true acceleration as

(10).

First the accelerometer noise is observed by testing the accelerometer, how much does it vary in the output? Or how much do you trust the numbers that come out. Whichever one of those numbers is larger is the one to pick. Second we assume the accelerometer has a bias, and the Kalman Filter will estimate the bias then remove it from any measured acceleration.

There are other models of accelerometer errors, but they are more involved and the bias is one of the important error descriptions of an accelerometer. One other important type of error in inexpensive accelerometers is an overall scale factor. The scale factor would be another element of the state vector, if it were modeled. In this article it is not.

Last, the description of the position measurements is

(11).

I apologize that the noise variable looks just like the velocity, but it is common practice to use the same letter for both. The position noise is given by how accurate you think the position measurements are. Are they good to 1cm? 1mm? 5 ft? Use your brain, be honest, and take a guess. The smaller you make it the more important the filter will think the position measurements are, and if they are not that good your filter will give bad results.

Now we convert these equations to the form of equations (1) and (2). The state vector is defined

(12).

In equation (12) the v really is the velocity. From this definition of the state vector we deduce the rest of the matrices in equation (1).

, ,

, , , and (13).

In the F matrix the bias is assumed constant, i.e. its derivative is zero (This is noticeable in the F matrix also). The particular values we use for the w vector will depend on the accuracy of accelerometer and how fast we want the filter to respond and estimate the accelerometer bias. If you multiply these out, this will seem a lot less magical if you do not. If you multiply them out you correctly you will get equations (8) - (10) and the time evolution relationship between those equations. For the position element of the state vector, the time evolution equation, equation (3), multiplies out as This equation should make sense. If you multiply out the time propagation equations for the covariance matrix the results are less intuitive, but the outcome is the elements only get larger as time goes on. It takes a measurement to shrink the covariance. For equation (2) we get

, and (14).

(The v's in equation (14) are the noises from equation (2), not the velocity) The i represents the i-th measurement of the position, there will be many. The value of the position measurement noise will be determined from the accuracy of the position measurements, and I will assume the value of .5 cm, which is not horrible but not great. The real Wiimote can probably do better but this is a conservative preliminary guess.

A nice feature of this particular model is that most of the matrices and vectors are time independent so the equations can be computed beforehand, avoiding several matrix multiplies and the numerical integration of the matrices. This improves the speed and accuracy of the filter.

The last thing to do is to set up initial values forand P(t), things that we probably do not know. A good way to start the state estimate is to set all the states to zero unless there is otherwise a good reason to set it differently. One good reason to set a state vector element to a nonzero value would be if the initial value is approximately known from a preliminary high quality measurement. For example if a position measurement is available, then that might be a good value to set as the initial position. A basic way to define the covariance matrix, P(t), is to set the off diagonal elements as zero and the diagonal elements as the large enough value to accept new measurements but not too large, otherwise the time for the filter to figure out good estimates for everything will take longer. This is one of the many balancing acts of Kalman Filter design.