Monday, April 18, 2011

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.

8 comments:

  1. good luck
    The arduino library for that compass has the calibration routines in it already. You might want to check that out and try and port it to mbed.

    ReplyDelete
  2. Hello.

    Nice post, and nice series of posts about your AVC entry. My library doesn't really have calibration routines. You can specify the scale values, but it doesn't do offsets. What Michael is doing is much nicer.

    Michael, if you are ever in Las Vegas, you should visit us!

    - Ryan Mulligan

    ReplyDelete
  3. @Ryan -- thanks, next time I'm in Vegas I'll give you a shout. Would love to meet you guys!

    ReplyDelete
  4. I just realized what I said in my last post is wrong. The library uses the calibration inputs to offset and scale the result. Doing what Michael is doing will let you visualize what is going on a lot better, and, if there are non-linearities, see those.

    - Ryan

    ReplyDelete
  5. Hi, all. Sorry I can't see who is the autor of this topic. But i very like it. And i'm trying to do the same project. I'm implement a simple moving average to data coming from the magnetometr.And i'm wrote a programm whith graphical interface in .Net. The program shows comming data and displaide it as attitude of Х and Y. So now i'm see elipsoid, but it has more values inside elipsoid then around it. Can you help me, What i'm doing wrong? And sorry for my english:)

    ReplyDelete
  6. @Yuri -- Hi, I am the blog and topic author. If you'd like to send me a screenshot maybe I can help. Use the "Contact me" link and send a URL to a picture.

    ReplyDelete
    Replies
    1. Hi,

      I am trying to implement a compass with LSM9DS0. How ever it seems that any time it starts to calculate the heading you need to move the sensor for a few seconds and after it starts to show the approximately true heading. How can i avoid this? is there any way that i calibrate my sensor only the first time?

      Thank you very much

      Delete
    2. Hi,

      Im trying to implement a compass with LSM9DS0. However it seems that anytime you wanna calculate the heading, before it's necessary to move the sensor for a few seconds in order to calibrate it. How can i avoid this? is there any other ways to calibrate the sensor?

      Thank you very much

      Delete