Quick and Dirty Compass Calibration in 3d!

No, you don't need funny glasses to read this.

As I continue to wage epic, nightly battles against the army of problems on my robot, Data Bus, I thought I'd share my simplistic approach towards 3d compass calibration that hopefully is "good enough" to get you started.

The problem: incorrect headings from 2- and 3-axis magnetometers not only in absolute terms, but also relative. When the compass is rotated 180 degrees, the resulting heading changes by more or less than 180 degrees.

References:
Using LSM303DLH for a tilt compensated electronic compass (pdf)
Compensating for Tilt, Hard Iron, Soft Iron Effects
http://www.varesano.net/blog/fabio/first-steps-hmc5843-arduino-verify-accuracy-its-results

Types of Correctable Distortion

Two types of correctable distortion affect magnetometers like the ST Microelectronics LSM303DLH and 2-axis Honeywell HMC6532 that I've been experimenting with: hard iron and soft iron distortions.

Hard iron distortions are caused by magnetized objects in a fixed location relative to the mounting point of the compass.  Soft iron distortions are caused by magnetically soft objects or PCB traces that alter the magnetic field around the compass depending on the direction the compass is pointing.

If you plot the x, y and z magnetic readings at various positions in a 3d scatter plot, in a perfect world you'd see a sphere surface mapped out by the points.

Hard iron distortions will offset the sphere along the x, y and z axes. Soft iron distortions turn the sphere into a tilted ellipsoid.

There's also scale distortion. All the axes should have equal sensitivity but they rarely, if ever, do. Correcting this is a question of normalizing the reported results of each axis.

Simpleton Calibration

The ST Datasheet explains the mathematical equations and approaches required to identify hard and soft iron distortions and corrections. I'm still coming up to speed on matrix math, linear algebra, etc., and time is running out very quickly so I decided to try a quick and dirty (read: simpleton) approach.

To wit: simply taking maximum and minimum values for each axis to compute offset and scale. That's assuming no significant soft iron distortion.

To verify that assumption, I needed to visualize the measurements from my LSM303DLH breakout from Pololu. I modifed an example program on mbed.org to print out X, Y and Z readings in a form that could be plotted easily by gnuplot.

I manually spun the compass around for a few minutes to collect a reasonable number of data points then I plotted the results in a 3d scatter plot (in gnuplot: splot "file.csv") I don't have a handy way to create an anaglyph so let me show you the X-Y, X-Z and Y-Z plots.

You can see that there is a pretty big x-axis offset and obvious Y scale distortion. I couldn't tell by eyeballing if the Y or Z axes were offset or mis-scaled.

A pretty simple perl script calculated the answers for me:

Xmax =  98.00  Xmin = -157.00 Xoff = -29.50
Ymax = 124.00  Ymin = -123.00 Yoff =   0.50
Zmax = 101.00  Zmin = -109.00 Zoff =  -4.00
Max  = 127.50  Min  = -127.50
Xsca =   1.00  Ysca =   1.03  Zsca =   1.21

------------8<--- cut here ---8<------------

#!/usr/bin/perl

open(FIN, "<$ARGV[0]") || die "can't open: $ARGV[0]";

$label{0} = "X";
$label{1} = "Y";
$label{2} = "Z";

$count = 0;
while () {

    s/^\s+//;
    @data = split(/\s+/);
    for ($i = 0; $i < 3; $i++) {
        $sum[$i] += $data[$i];
        $max[$i] = $data[$i] if ($data[$i] > $max[$i]);
        $min[$i] = $data[$i] if ($data[$i] < $min[$i]);
    }
    $count++;
}
close FIN;

for ($i = 0; $i < 3; $i++) {
    $med[$i] = ($max[$i]+$min[$i])/2.0;
    printf STDERR "%smax = %6.2f  %smin = %6.2f %soff = %6.2f\n",
      $label{$i}, $max[$i],
      $label{$i}, $min[$i],
      $label{$i}, $med[$i];
    $max[$i] -= $med[$i];
    $min[$i] -= $med[$i];
    $max = $max[$i] if ($max[$i] > $max);

    $min = $min[$i] if ($min[$i] < $min);
}

printf STDERR "Max  = %6.2f  Min  = %6.2f\n", $max, $min;

for ($i = 0; $i < 3; $i++) {
    $scale[$i]=$max/$max[$i];
    printf STDERR "%ssca = %6.2f  ", $label{$i}, $max/$max[$i];
}
printf STDERR "\n";

Results

The C++ library that I'm writing/porting/adapting on mbed.org for the LSM303DLH features two member functions, setOffset() and setScale() to which I can provide the offset and scale numbers from above.

A simple test program exercises the library and tests the scale and offset values.  Rough tests show improved results in heading as well as when swinging the compass 180 degrees.  The true test will happen when I mount the new compass onto my robot and collect data. Hopefully the calibration is close enough to significantly improve dead reckoning position calculations.