The Importance of Bookkeeping

I may have mentioned this before, but there is about 1000 w, x, y, and z in the formulas for the Kalman filter, along with all their + and -. I found a feature of Matlab (sorry, doesn't seem to be available in Octave) symbolic computation. Actually I have know about it for years, I just didn't want to use it for this project.

Anyway, I was having trouble with the filter responding to real data. I was trying to reduce the data from the calibration dance, and at the same time debug my implementation of the filter. The nasty thing is that there isn't much in the way of debugging that can be done. All you can do is re-double-check all the signs you have already re-double-checked before. In this case I was doing it with the Matlab symbolic routines, to check against the semi-manual, semi-Alpha way I had done it before. I found one (only!) problem in the signs, but it still wasn't working.

Then I found it in the measurement covariance. To follow proper form, I needed a square matrix of the sensor covariance with itself. However, I am treating the components of the measurement error as uncorrelated with each other, so it is just a diagonal matrix. So, I did something like

R=diag([4,4,4]).^2;

which makes perfect sense, until I start pulling elements out. When I called the code to get element 1, I did

R(1)

which works fine, but then for element 2 and 3 I did something like

R(2)

Do you see the problem? It took me all day to see it. It should be

R(2,2)

to get the element on the diagonal I wanted. Instead, it got one of the zero off-diagonal elements, which said that the noise on this measurement was zero. I accidentally told the filter that the measurement was perfect, and that it was to believe the measurement with all its heart and soul, and do whatever was necessary with the state to match the measurement.

What I did instead was take out diag() and therefore leave R as a vector representing the diagonal of the matrix.

With this, my IMU sitting still thinks that it is sitting still. Yay! Now let's see about rotating...