LSM303DLHC - Calibration, Pitch, Roll and Tilt Compensated Heading

Using Arduino Sensors
8

Hi Jim,

Thanks for your reply and I apologize for not providing the code and data earlier.

My code was copy pasted from above, with only my specific calibration values added.

#include <Wire.h>
#include <LSM303.h>

LSM303 compass;

const float alpha = 0.15;
float fXa = 0;
float fYa = 0;
float fZa = 0;
float fXm = 0;
float fYm = 0;
float fZm = 0;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  compass.init();
  compass.enableDefault();
}

void loop()
{
  compass.read();
  float pitch, pitch_print, roll, roll_print, Heading, Xa_off, Ya_off, Za_off, Xa_cal, Ya_cal, Za_cal, Xm_off, Ym_off, Zm_off, Xm_cal, Ym_cal, Zm_cal, fXm_comp, fYm_comp;

  // Accelerometer calibration
  Xa_off = compass.a.x / 16.0 + 17.224598; //X-axis combined bias (Non calibrated data - bias)
  Ya_off = compass.a.y / 16.0 + 12.282323; //Y-axis combined bias (Default: substracting bias)
  Za_off = compass.a.z / 16.0 - 3.162899; //Z-axis combined bias
  Xa_cal =  0.949813 * Xa_off + 0.009282 * Ya_off + 0.004454 * Za_off; //X-axis correction for combined scale factors (Default: positive factors)
  Ya_cal =  0.009282 * Xa_off + 0.983771 * Ya_off - 0.001821 * Za_off; //Y-axis correction for combined scale factors
  Za_cal =  0.004454 * Xa_off - 0.001821 * Ya_off + 0.980313 * Za_off; //Z-axis correction for combined scale factors

  // Magnetometer calibration
  Xm_off = compass.m.x * (100000.0 / 1100.0) + 683.481495; //X-axis combined bias (Non calibrated data - bias)
  Ym_off = compass.m.y * (100000.0 / 1100.0) + 8449.523695; //Y-axis combined bias (Default: substracting bias)
  Zm_off = compass.m.z * (100000.0 / 980.0 ) + 6823.709607; //Z-axis combined bias
  Xm_cal =  0.882879 * Xm_off + 0.024205 * Ym_off + 0.016546 * Zm_off; //X-axis correction for combined scale factors (Default: positive factors)
  Ym_cal =  0.024205 * Xm_off + 0.863208 * Ym_off + 0.007950 * Zm_off; //Y-axis correction for combined scale factors
  Zm_cal =  0.016546 * Xm_off + 0.007950 * Ym_off + 0.829441 * Zm_off; //Z-axis correction for combined scale factors


  // Low-Pass filter accelerometer
  fXa = Xa_cal * alpha + (fXa * (1.0 - alpha));
  fYa = Ya_cal * alpha + (fYa * (1.0 - alpha));
  fZa = Za_cal * alpha + (fZa * (1.0 - alpha));

  // Low-Pass filter magnetometer
  fXm = Xm_cal * alpha + (fXm * (1.0 - alpha));
  fYm = Ym_cal * alpha + (fYm * (1.0 - alpha));
  fZm = Zm_cal * alpha + (fZm * (1.0 - alpha));

  // Pitch and roll
  roll  = atan2(fYa, sqrt(fXa * fXa + fZa * fZa));
  pitch = atan2(fXa, sqrt(fYa * fYa + fZa * fZa));
  roll_print = roll * 180.0 / M_PI;
  pitch_print = pitch * 180.0 / M_PI;

  // Tilt compensated magnetic sensor measurements
  fXm_comp = fXm * cos(pitch) + fZm * sin(pitch);
  fYm_comp = fXm * sin(roll) * sin(pitch) + fYm * cos(roll) - fZm * sin(roll) * cos(pitch);

  // Arctangent of y/x
  Heading = (atan2(fYm_comp, fXm_comp) * 180.0) / M_PI;
  if (Heading < 0)
    Heading += 360;

  Serial.print("Pitch (X): "); Serial.print(pitch_print); Serial.print("  ");
  Serial.print("Roll (Y): "); Serial.print(roll_print); Serial.print("  ");
  Serial.print("Heading: "); Serial.println(Heading);
  delay(250);
}

I did a quick test with keeping the heading steady and alternately increasing the pitch and roll to about 90°. My results are shown below:

1200×800 40.2 KB

It seems when in my attempts at calibration, I've always experienced similar results. This leads me to believe there is definitely a systematic error on my part somewhere. I confirmed the signs of the corrected values, and will spend some more time with Magneto to make sure I continue to get offsets approaching zero.

I have had very good results with the tilt compensation calibration script from Pololu, and look forward to testing out your enhanced version.

Thanks again for the help!
Adam