Last post has been months ago. I finally got the courage to continue my work on the Kalman filter. I can report some progress and another, hopefully small, draw back.

Remember that my filter didn’t work because of the compass disturbances made by metal and magnets. Last nights I’ve been working on a solution for this. In essence I want to ignore compass readings when there is a disturbance somewhere. But how do I detect disturbances? A little work I did for Xanders omniwheel robot pointed me in the right direction. 
My Kalman filter not only gives predicted values for the heading but it also knows how accurate this prediction is. Knowing these two things one can use statistics to calculate how likely a compass reading fits in the prediction. Let me explain this using an example. Suppose the filter predicts a heading of 90 degrees (West). At the same time it also knows that the true heading is for 95% certain between 87 and 93 degrees. In this case you can trust a compass reading of 92 degrees, but you wouldn’t trust a compass reading of 97 degrees. Based on this technique I added a switch to my filter. If compass readings are to be trusted the filter performs normally. But if it does not trust a compass reading then it doesn’t take this reading into account. It then skips the update phase and continues straight to the next iteration making the next prediction. This seems to work.

Here is the first graph of my filter in action. 

The red and purple lines represents the orientation of my robot. In red is what my compass reports, in purple is what the filter reports. The horizontal axis is the time axis (in msec). Let me explain what is happening in real life and how you can see the effects in the graph.
The robot is standing on an enforced concrete floor with its side against the wall. The iron in the concrete gives nasty disturbances to the compass. Initially the robot is pointing almost east (80 degrees). After some time I lift the robot and turn it half a circle and put it back against the wall with its other side. By using the wall I know that the robot made a turn of 180 degrees. Turning the robot shows up in the graph as rising (or falling) of the red and purple lines. Once the robot starts turning we can see both lines getting away from each other. This is because the compass is affected by the iron and the gyro is not. The filter detects this and start ignoring the compass readings. This is shown by the green line, when it is up the filter ignores the compass. As you can see from the red line the filter is very right to do so. We would expect it to rise to 80+180=260 degrees, but it reports around 240 degrees. This is obviously very wrong. The result of the filter on the other hand seems quite accurate, the purple line rises to the expected value of 260 degrees. Once I turn the robot back the compass readings come back in line with the filter readings. The filter then starts taking compass readings back into account. From that moment the filter can use compass readings again to filter out any accumulated errors in the filter heading.

Does this mean my filter is ready? No, There still is one problem to tackle. But I’ll explain that problem in the next episode of my ever lasting quest for better sensor readings.