Inertial Orientation Sensing

Phones all have gyros and compasses and accelerometers to provide orientation sensing for fairly pedestrian applications like mapping and gaming. There's lots of reasons to want to sense position and orientation for more exacting applications like UAVs, sailboat autopilots, and Human Mobility Research. I'm going to see what I can do with two sensors that were on special before Christmas at Pololu. The LSM303DLHC 3D Compass and Accelerometer Carrier with Voltage Regulator and L3GD20 3-Axis Gyro Carrier with Voltage Regulator are tiny, friendly to 3 and 5 volt logic levels and communicate over I2C to keep the pin requirements reasonable.

Pololu provides basic libraries for reading them with the Arduino. The results come back as integers for each of the 9 components, and they need to be translated into appropriate units before they can be used in further calculations.

The gyro chip starts on the 250 degree per second scale, returning +/-32768 for about 250 degrees per second and close to zero when sitting still.

The magnetometer starts off on the 1.3 Gauss scale, with 1100 LSB / Gauss on the X and Y axes and 980 LSB / Gauss on the Z axis, according to the datasheet. The Pololu library includes a calibration program to read the local maximum values for each axis to do a calibration, which presumably goes into its compass heading calculation.

The accelerometer reads integers in about cm/s^2, or thousandths of a g, so one gravity is about 981 or 1000 and the reading will be close to zero for the horizontal axes.

None of these sensors are exact to their ranges, so they will need to be calibrated for physical units.

It takes about 3 ms to read both sensors on an Arduino UNO, so the upper limit is probably about 300 Hz for an update rate. (Code below.)

Calibrating the Sensors

The sensors have to be assumed linear, but we can determine the zero points and scale factors by observation. The maxima should correspond to the limits and the zero values should be between them.

I'm near sea level in Kingston, Ontario, Canada so down should be 1 standard gravity. The #defined accelerometer calibration constants in the code below were determined by running the program for the particular sensors being used and finding the average reading in the six cardinal poses and assuming they were +/-g with 0 in the middle.

X from -1058 to 1044, 0 midpoint = -7, 1 g = 1051
Y from -1008 to 1012, 0 midpoint =  2, 1 g = 1010
Z from -979 to 1045, 0 midpoint = 33, 1 g = 1012

From wikipedia the local field strength is about 0.55 Gauss. The magnetometer maximum readings should correspond to +/- 0.55 Gauss, achieved when each of the axes is pointing north and down or south and up.

X from -564 to 633,  0 midpoint = 34.5, 1 Gauss = 1088
Y from -877 to 430,  0 midpoint = -223.5, 1 Gauss = 1188
Z from -517 to 593,  0 midpoint = 38, 1 Gauss = 1009

A full rotation about an axis should integrate to 360 degrees or 2 pi radians. The code below measures and outputs the raw values needed to complete the calibrations. The offset constants for the gyro were taken from stationary averages. The scale factors were taken from an integration of a complete 360 degree rotation about each axis.

The calibrations were all based on the assumption that the table top was flat and square and didn't move. They can all be better if taken under more controlled conditions. 


Compass redone:
X from -618 to 623,  0 midpoint = 2.5, 1 Gauss = 1128
Y from -896 to 511,  0 midpoint = -192.5, 1 Gauss = 1279
Z from -653 to 620,  0 midpoint = -33, 1 Gauss = 1157

Update 2013/01/08: Calibration is critical to get a good compass bearing even standing still. Once that is accomplished, gyro calibration needs to be accurate to avoid dynamic errors, and you really need to be sure the algorithm is getting updated often enough not to lose accuracy in the integration. The gyro integration assumes linearity. That works really well for short time steps, but fails if the time step gets longer due to e.g. attending to output and controlling other parts of the system. Ideally, it would run on a timer interrupt, but other software systems (PWM, etc.) are already overusing the 3 timers available on the UNO, so maybe an externally clocked interrupt would be best. For the moment I can get by with a static estimate....

Madgwick's Algorithm

Sebastian Madgwick's Attitude and Heading Reference System (AHRS) algorithm is written for units of g-force, radians / second, and Gauss, so the binary inputs need to be converted.

The C code supplied by Madgwick includes a routine for a fast inverse square root that relies on certain hardware specific elements of IEEE floating point and integer representations. That led me into trouble that Tobias Simon had found previously. To avoid potential platform dependent weirdness, I changed the Madgwick code to simply use the computationally expensive sqrt() function. It adds much less than 1 ms / update on the UNO, so I'm not going to get excited.

The sample frequency is also hard-coded in as a #define. It is used only to specify the integration time scale for advancing the estimate of the orientation, so this value could be taken directly from the system clock time elapsed since the last iteration. This code change does that (provided lastTime = micros() is updated at the end of each estimate):


//RWS use actual elapsed time instead of fixed sample frequency
#define sampleFreq    (1000000.0f / (micros() - lastTime))
unsigned long lastTime=0;

//#define sampleFreq 512.0f // sample frequency in Hz

The gain, beta, is set to 0.1 in the supplied code, but Madgwick's report suggests using a much larger value (2.5) initially to speed convergence.  The whole package winds up being almost two big to fit into the 32K available for code on an UNO, but it's running and spitting out values that make reasonable sense.

Code for Taking the Raw and Calibration Readings

This code is for the sketch Pololu_IMU_Bias. 

#include <Wire.h>
#include <math.h>
#include <MadgwickAHRS.h>
#include <LSM303.h>
#include <L3G.h>



#define AX0 -7      // accelerometer offset
#define AX1 1051    // divide by scale factor of about 100 to m/s^2
#define AY0 2       // LSM303DLHC returns integer values in about cm/s^2 or mg
#define AY1 1010    // divide by scale factor of about 1000 to g-force
#define AZ0 33
#define AZ1 1012

#define MX0 34.5    // magnetometer offset
#define MX1 1088    // divide by scale factor to Gauss
#define MY0 -223.5  // LSM303DLHC returns int values at 1100 LSB/Gauss on X and Y
#define MY1 1188
#define MZ0 38
#define MZ1 1009    // 980 LSB/Gauss on Z for default 1.3 Gauss scale

#define GX0 -109    // gyro offset from bias calibration
#define GX1 5919    // divide by 32768 / 250 deg/s = 131 scale factor to deg/s
#define GY0 0       // default for the L3DG20 is 250 deg/s
#define GY1 6411    // divide by 7510 scale factor to radians/s
#define GZ0 -242    // actual accumulation gives X 103.3 Y 111.9 Z 109.2
#define GZ1 6257    // translated for radians/s  X 5919  Y 6411  Z 6257

#define sp(x,y,z) Serial.print(x);Serial.print(" ");Serial.print(y);Serial.print(" ");Serial.print(z);

L3G gyro;
LSM303 compass;

char scratchy[200];

void setup() {
  Serial.begin(57600);
  Wire.begin();

  compass.init();
  compass.enableDefault();
  // Calibration values. Use the Calibrate example program to get the values for
  // your compass.
  compass.m_min.x = -639; compass.m_min.y = -834; compass.m_min.z = -597;
  compass.m_max.x = +536; compass.m_max.y = +414; compass.m_max.z = +599;

  
  if (!gyro.init())
  {
    Serial.println("Failed to autodetect gyro type!");
    while (1);
  }

  gyro.enableDefault();

}

void loop() {
  int aax,aay,aaz,amx,amy,amz,agx,agy,agz;
  int iax,iay,iaz,imx,imy,imz,igx,igy,igz;
  static long int n = 0,sax = 0,say = 0,saz = 0,smx = 0,smy = 0,smz = 0,sgx = 0,sgy = 0,sgz = 0;
  static double stgx = 0.0,stgy = 0.0,stgz = 0.0,ft = 0.0;
  static long lastTime = micros(),startTime,endTime;
  
  startTime = micros();
  compass.read();
  gyro.read();
  iax = compass.a.x; iay = compass.a.y; iaz = compass.a.z;
  imx = compass.m.x; imy = compass.m.y, imz = compass.m.z;
  igx = gyro.g.x; igy = gyro.g.y; igz = gyro.g.z;
    
  // Accumulate and calculate running averages on absolute inputs
  sax += (int) compass.a.x;
  say += (int) compass.a.y;
  saz += (int) compass.a.z;
  smx += (int) compass.m.x;
  smy += (int) compass.m.y;
  smz += (int) compass.m.z;
  sgx += (int) gyro.g.x;
  sgy += (int) gyro.g.y;
  sgz += (int) gyro.g.z;
  n++;
  aax = sax / n;
  aay = say / n;
  aaz = saz / n;
  amx = smx / n;
  amy = smy / n;
  amz = smz / n;
  agx = sgx / n;
  agy = sgy / n;
  agz = sgz / n;

  endTime = micros();
  ft = (double) (endTime-lastTime) / 1000000.0;    // floating point time in seconds
  lastTime = micros();

  // integrate the gyro performance in LSB*seconds
  stgx += ((double) gyro.g.x - GX0) * ft;
  stgy += ((double) gyro.g.y - GY0) * ft;
  stgz += ((double) gyro.g.z - GZ0) * ft;
    
    
  if(millis() % 1000 < 10){
    if(millis() %30000 < 10) stgx = stgy = stgz = 0.0;   // zero out the gyro drift now and then
    
    // Time in micro seconds
    sprintf(scratchy,"%6i us  ",endTime-startTime);
    Serial.print(scratchy);
    
    // Raw readings
    sprintf(scratchy," INST Acc X:%5i Y:%5i Z:%5i   Mag X:%5i Y:%5i Z:%5i   Gyro X:%5i Y:%5i Z:%5i  ",iax,iay,iaz,imx,imy,imz,igx,igy,igz);  
    Serial.print(scratchy);
    
    // Average raw readings for calibration constants 
    sprintf(scratchy,"    AVG Acc X:%5i Y:%5i Z:%5i   Mag X:%5i Y:%5i Z:%5i   Gyro X:%5i Y:%5i Z:%5i  ",aax,aay,aaz,amx,amy,amz,agx,agy,agz);
    Serial.print(scratchy);  

    // Accumulated zeroed gyro readings 
    Serial.print("    ACCUM  Gyro XYZ:  ");
    sp(stgx,stgy,stgz);
  
    Serial.println();  
  }
}