This time I’ll show you the first results of incorporating a 3-axis magnetometer (or compass) into my IMU filter.
A quick round-up first. I have a IMU sensor that houses a 3-axis accelerometer and a 3-axis gyro sensor. Well, actually I got two, one that I built myself and one from Dexter Industries. To use this type of sensor I have written a non-linear complementary filter (NLC-filter) who’s job it is to fuse the output of the two sensors to a drift free low noise attitude signal. The filter tells me the tilt over the x-axis and tilt over the y-axis. It also tells me the heading. The heading signal is not drift-free. The reason for this is that the accelerometer can provide tilt data but it cannot provide heading data. For this you need a compass.
A compass sensor should really be called an magnetometer because it doesn’t give you heading like a sailors compass does. Instead it gives you the force of the magnetic field over three axis. From this one can calculate heading.
It was technically not very difficult to incorporate the magnetometer data into the filter. Although it took me a long time to figure out how to do this. The main thing was to tune the PI-controller for the compass. I’m not finished with this but I can show you some first results of the filter anyway. The measurements were taken with the setup that you see in the picture.
The NXT is mounted on a platform some 20 cm above the ground. This is to minimize effects of the steel enforced concrete floor. It can rotate endlessly as it is mounted on a turn table and driven by a NXT motor and a worm wheel. The worm wheel in combination with a regulated motor gives a very stable rotation speed. The sensors are mounted 10 cm from the NXT to prevent disturbances from the NXT. Measurements are sent over bluetooth to the PC in real time.
this first graph was taken while the NXT was stationary. It shows how stable the filter signal is. 
Note that the scale of the vertical ax is 0.1 degree, the vertical axis ranging from -4 to 0 degrees. The roll and pitch signals are very stable. During the 15 minutes of the test these signals stayed within a bandwidth of 0.4 degrees. This is all that is left of the gyro drift. The yaw signal (that is controlled by the compass) is less stable, it floats between -0.8 and -2.0 degrees. But also in the yaw signal the gyro drift is eliminated to a large extend. I am very, very pleased with these noise levels as the bandwidth of noise from an accelerometer is around 5 degrees.
The second graph compares the output signal from the compass with that of the filter. This time the NXT was spinning around the Z-axis. The graph shows the results of a full spin. 
. You basically see two descending lines, indicating a constant movement of the sensors. The signal from the compass, in blue, is not exactly over the signal from the filter. There is a slight difference in the heading that these two report. The reason for this is that the setup of the NXT is not exactly level. The compass signal suffers from this. Its line is not really straight but it is like an S-curve due to this. The filter signal on the other hand is tilt compensated and almost straight. The smoother red line also indicates a lower noise level.
This last image shows the effect of a magnetical disturbance on the compass sensor and on the filter. While taking this graph I slowly moved a head phone along side of the sensor.
The compass suffers from this disturbance, at the peak the signal is over 10 degrees of. The filter on the other hand is not affected by the disturbance. It detects the disturbance and ignores the compass signal while it is disturbed. During this time the filter relies solely on the gyro to keep track of heading.
I think the low noise levels of the filter are nice. But the true advantages are in the tilt compensation and the robustness to magnetic disturbances. The filter is equally robust to disturbances in acceleration I think. This however I cannot show you using this setup.
Great work, the results look awesome! One question: are you still going to use the IMU for your ball-balancing robot?
Very nice as always. I’d love to see your code as it now stands. I’ve been playing with a Pololu minIMU-9 and am nearly done porting their example code (based on ArduPilot 1.5) to mbed and leaning some things in the process. This is the Premerlani algorithm that uses a direction cosine matrix to track the orientation state. I still need to work on magnetometer calibration. Once I get that I’ll be in a position to do some testing. I’d love to know how the algorithm handles magnetic field disturbances, e.g., big steel objects that the robot drives by. Take care! –Michael
Hi Michael,
My filter is also based on the Premerlani algorithm. I handled the compass data this way:
1. Remove the Zw component from the data to prevent this from interfering with the accelerometer.
1a. Transform signal to world frame
1b. Set Z-axis signal to 0
1c. Transform back to body frame
1d. Normalize
2. Calculate error of compass data and dcm by taking the cross product of compass data and Xrow of dcm.
3. Use this error to calculate a correction term and I term just like the algorithm does for the accelerometer. But make sure you use dedicated P and I factors for the compass
I detect disturbances this way. At start of the filter I calculate the average length of the compass and accelerometer vectors using x samples. (The length of the compass vector should be equal to the strength of the earth magnetic field, that of the accelerometer should be 1G). Every time the filter takes a sample of these sensor it checks the length of the sample vector to the average. If the difference is too big the sample is ignored. Currently I use a margin of 10%, but this can be changed by the user.
The code will be available in the (near) future. I want to do more testing and implement some optimizations first.