a Kalman filter to fuse gyro and compass readings

[aswin0]

Hi,

Few days ago I finished a project that aimed at getting trustworthy heading information from a compass in an environment with lots of magnetic disturbances. I did so by combining information from a compass sensor and a gyro sensor by means of a Kalman filter. To tell you the truth, the main goal of this project was to understand the working of a Kalman filter.
Well, I think I reached both goals. I got a working program (in robotC) that outputs better heading information than I ever experienced before. I do understand how a Kalman filter works and what its limitations are. The filter itself is not a miracle, it is the vector math that makes it look difficult and it is the general way the formulas are written down that make it look realy complicated.

I have written down the steps of this project in my blog, here you can find the filter explained, read about my frustrations in getting things to work, view graphs of the behaviour of the filter. And you can view or download the final code from there. This code is not to hard to ubderstand and can be used in your programs with easy (provided you use robotc). My blog can be found here.

Have fun with it,

Aswin

[HaWe]

though I'm not able to test it - a real good job, congratulations.
I'd love to get it converted to NXC. Pathfinding is my theme as well (as you know from my navigation robot and a*) but the project came to a standstill - above all because of insufficient/missing exact bearing and odometric abilities.
I hope your KF will give it a new push... :)

[mightor]

Aswin,

Thank you for sharing this. Hopefully I'll find a use for it

- Xander

[aswin0]

though I'm not able to test it - a real good job, congratulations.
I'd love to get it converted to NXC. Pathfinding is my theme as well (as you know from my navigation robot and a*) but the project came to a standstill - above all because of insufficient/missing exact bearing and odometric abilities.
I hope your KF will give it a new push...

Converting it to NXC should be very easy. Only the sensor related functions are specific to robotc, all the rest is just plain ANSI C.

I found out that entering the area of pathfinding equals entering the labyrinth of statistics. There is no way out! Also with this filter the heading information is uncertain.

[HaWe]

what exactly do you mean with this:

Also with this filter the heading information is uncertain.

[aswin0]

I mean that this filter, or any filter, can remove some noise from sensor readings, but it can never remove all the noise in sensor readings.

This filter reduces the noise in compass reading caused by magnetical disturbances quite a bit. Or, looked at it from another perspective, this filter reduces the accumulation of errors in gyro reading quite a bit. But it cannot remove all the noise. This means that you still have to deal with noise when using the output of this filter. If you decide to ignore this, you should be aware that there is still some error in the sensor data that might cause problems.

Also, the heading information from this filter might be trusted in respect to the starting condition but not in respect to true north. With this I mean that if the filter starts at 90 degrees for example, then a reading of 180 degrees will be (more or less) 90 degrees from its starting position. But it is very uncertain that the starting position really was exactly east.

There is one more thing to be aware of when using filters. Most filters, like a low pass filter for example, introduce some kind of lag in the data. This implementation of the Kalman filter does not suffer from lag (I think).

[HaWe]

Just for my understanding:

if the error is Gaussian distributed, you will get the "true" (correct) heading nevertheless in the average during some seconds of driving, I assume.
But what if the error is systematic (non-Gaussian), like the Compass on ships or aeroplanes? Here you have to deal with the deviation and you need to have a compass correction card or a magnetic compensation for it. Can the Gyro sensor correct those faulty compass readings? Or what if the gyro is drifiting (a s they seem to do normally) - can compass readings correct the Gyro?

And another question:
What is the "big difference" of the "results in the end ":
- either if you use the Kalman filter
- or if you use just the arithmetic average or the median (or the median of the average) of both sensor readings?

(I actually don't understand "low pass" or "high pass" in this conceptual link - I know those idioms only from analog electronic filters (using Inductor-Resistor-Capacitor-combinations ) to eliminate low or high sectors out of a frequency spectrum)

edit:
maybe you'll need also odometric data for the KF model to filter faulty readings?
(if odometry and compass say you're driving straight and gyro says you're driving a bend => correct gyro drifting and so on)

[aswin0]

About non guasian errors.
The filter needs gausian errors, otherwise the statistics that it is based on do not apply. However, the filter not only keeps track of the heading but also of the variance in the heading. I have used this information to filter out non gausian noise. In case there are compass disturbances that cannot be explained by gausian noise the filter ignores the compass reading for that moment and uses only gyro information to update the calculated heading. This can be seen in the graph below that shows the result of an experiment in a room with heavy compass disturbances. The red line is the heading according to the compass, the purple line is the heading according to the filter. Things are stable in the beginning, but en I turn my robot for exactly 180 degrees. The compass thinks it is less due to magnetic disturbances as you can see because the red line never makes it 180 units above the starting point. While turning the robot there is a moment when the magnetic disturbances are so big that the filter no longer trusts compass readings, this is indicated by the rising of the green line. The filter heading then starts drifting away from the compass heading and makes the full 180 units rise. Once I turn the robot back to it's original position the filter starts trusting the compass again, the green line drops, and the purple and red lines merge together again. This behavior however is not due to the Kalman algorithms, although it is based on information provided by the filter.

About the arithmic average
This is more or less what the filter does. But it doesn't just take the mean of the two sensors, it weights the two sensors according to there variance. the sensor with the least variance puts in more weight. It also takes history into account and also weights the history according to it's variance.

About drift
The filter does not compensate for drift, although it takes longer before drift starts to be noticeable. I want to add an algorithm to compensate for drift as well, but this still has to be designed. The basics of this will be the subject of my next blog.

About fusing in odometrics
This implementation of the filter uses only two sources of information, compass and gyro. The theory behind the filter allows for unlimited sources of information, so odometrics can be included, also inputs (like steering commands) can be included. but to use more sources of information requires you to use vector math, this is hard to implement and time consuming. Or you can treat the output from the this filter as input for a second filter that fuses odometrics information in.

About the big difference
The graph below shoes the difference with pure compass information. What it doesn't show is the difference with pure gyro information. Although not shown I have measured it, in this case there was a difference of 2 degrees at the end of the experiment. These two degrees were mainly caused by the accumulation of gyro errors. Drift is too small to be seen in this experiment. I have drift values of about 0.16 degrees a minute.

[HaWe]

thank you very much for this bunch of information! I'm slowly getting a idea of what the KF does!

[airboy1]

Kalman filtering is used in STS (shuttle) Onboard Flight Software. The engineers who did the initial work were from MIT.

Airboy