muCSense: Using Calibration

I just added some new code to the muCSense library to help calibrate accelerometers and magnetometers. I plan to use the framework to add calibrators for gyros and other sensors too.

In this post, I’ll walk through an example sketch that uses these new classes. I’ll dig into the internals in later posts.

The Circuit

This sketch assumes that you have connected a SparkFun Sensor Stick to an Arduino (I have an Arduino Uno), just as I described a while back. Here’s the diagram (after the jump):

Using the Library

As explained in the last post, to use the library just download the code from GitHub and put it in a subdirectory of your /libraries/ directory. You may have to create the libraries directory. I put mine in /libraries/muCSense.

This assumes you are using Arduino 1.0.1. I will try to keep the library working on future versions of Arduino.

The Sketch

You can fetch this full sketch from GitHub. I’ll walk through it section by section here.

Preamble

First we need to include some things. We need Wire.h to get I2C communication going with the sensors, and the other headers point to files in muCSense. You don’t need to include all of these if you aren’t using them.

#include <wire.h>
#include <adxl345.h>
#include <hmc5843.h>
#include <i2c.h>
#include <ITG3200.h>
#include <sensor.h>

#include <serialdatalistener.h>
#include <simpledatacollector.h>
#include <bestspheregaussnewtoncalibrator.h>
#include <minmaxspherecalibrator.h>

Then we declare global objects. This includes

  • an array of pointers to Sensors that we will use to access the Sensor Stick sensors
  • a DataCollector that is responsible for collecting observations from the sensors and making them available to clients like Calibrators. This is a very simple DataCollector: specify a number of samples to take and a number of milliseconds to take them, and it takes that many samples in that many millis. More sophisticated DataCollectors may discard outliers, filter time series, etc.
  • pointers to two Calibrators that will listen for updates from the DataCollector and use the observations to estimate calibration parameters for the accelerometer and magnetometer.
//Global objects. Pointers are to objects that are
// created and initialized in setup().

//Pointers to sensors on the SparkFun Sensor Stick
 Sensor* sensors[3];

//A DataCollector object that will simply callect 100
// raw readings over 15 seconds
 SimpleDataCollector sdc(100, 15000);

//The accelerometer and magnetometer will each get a Calibrator
 Calibrator* pAccelCal;
 Calibrator* pMagCal;

Setup()

The code in this section is all part of the setup() routine. This is where the heavy lifting happens.

The first thing we need to do is start the Wire and Serial objects. I do serial communication at 115200 because that is the default for the bluetooth radio I use.

  Serial.begin(115200);  //115200 is default for BlueSMiRF
  Wire.begin();          //start I2C

Then we create and initialize Sensor objects, just like we did in the last post.

 //Create instances of the sensors we'll be using
  sensors[0] = ADXL345::instance();
  sensors[1] = HMC5843::instance();
  sensors[2] = ITG3200::instance();

  //Initialize the sensors, making sure they are ready
  // for continuous measurement.
  // Once initialized, add each sensor to the DataCollector.
  Serial.println("Initializing sensors");
  size_t i;
  for(i=0;i<3;++i) {
    sensors[i]->init();
    sdc.addSensor(sensors[i]);
  }
  Serial.println("Sensors ready");

Now we get into the meat of the calibration setup. The main players here are

  • DataCollectors — orchestrate collection of data from the sensors. These may filter the data, discard outliers, or provide other data quality services. They may also support a data collection protocol, collecting data in phases, lighting up status lights (as Jens does with his RGB Tumbler ), responding to button presses, or more. The SimpleDataCollector used here just collects a fixed number of raw samples at a specified frequency.
  • DataListeners — Responsible for doing something with the data. When a DataCollector has data ready, it notifies its listeners (this follows a textbook Observer design pattern). The listeners might then collect statistics, store the data, or simply write the data out to serial. In this framework, Calibrator objects are a type of DataListener.
  //Now we create some objects that will listen to the
  // data collector.

  //The simplest DataListener is a SerialDataListener. 
  // When notified that a DataCollector
  // has data ready, it simply fetches the data and
  // writes it to Serial
  SerialDataListener* pSDL = new SerialDataListener;

  //Have the SerialDataListener listen to all of the sensors
  for(i=0;i<3;++i) {
     pSDL->addSensor(sensors[i]);
  }

Then we create two Calibrator objects — one for the ADXL345 and the other for the HMC5843. Here we have two kinds of calibrators. The accelerometer uses a heavyweight Gauss-Newton calibrator that finds the model parameters that are optimal (in a certain sense, discussed here)

For the magnetometer, we use a much more lightweight (and less accurate, but still decent) calibrator that uses the minimum and maximum observations in each dimension to estimate offset and sensitivity.

Either one of these calibrators could be used on either a magnetometer or accelerometer. The point here is that you are free to mix and match, or write your own Calibrators, as you see fit. The choice of Calibrator has a major impact on the size of your compiled binary.

//the ADXL345
pAccelCal = new BestSphereGaussNewtonCalibrator(sensors[0]);

//the HMC5843
pMagCal = new MinMaxSphereCalibrator(sensors[1]);

Before going on, we need to let the DataCollector know who is listening for updates.

  //Now we need to register each of our DataListeners with
  // the DataCollector. This
  // lets the DataCollector know which listeners to
  // notify when new data is ready.
  sdc.addListener(pSDL);
  sdc.addListener(pMagCal);
  sdc.addListener(pAccelCal);

And finally we can collect the data and calibrate. As coded, this will collect 100 samples over 15 seconds. While it is running, it will output raw observations to serial (this is what the SerialDataListener is doing). During this time, smoothly rotate the sensor around so that each axis becomes oriented vertically both up and down at least once.

If you are aware of your local magnetic field, try to make sure you orient the axes along the magnetic field too. Where I live in Seattle, the Earth’s magnetic field points about 70 degrees straight down into the ground, so I don’t worry about it much. Near the equator it may matter.

One more note: this SimpleDataCollector is not ideal for accelerometer calibration. It helps to do something more careful, but I haven’t ported that something into this library yet.

  //Run the data collection strategy
  sdc.collect();

  //Perform calibration on the collected data.
  //  We don't have a gyro calibrator here.
  pMagCal->calibrate();
  pAccelCal->calibrate();

At this point, setup is complete and the hard work is done.

loop()

Now we’ll run a simple loop to read, transform, and print sensor readings using our newly computed calibration parameters.
First we’ll name some convenience variables and read the sensors.

  
  //Pointer to a buffer that will cointain the raw readings
  const int16_t* buf = 0;

  Sensor* pAccel = sensors[0];
  Sensor* pMag = sensors[1];

  //read the sensors directly
  for(size_t i=0;i<3;++i) {
     sensors[i]->read();
  }

Then we will print the raw readings, transform the raw readings into calibrated readings, and print the calibrated readings.

  //Now transform the reading using the calibratior.
  //  Accumulate the sum
  // of squares too, so we can check that it is close to 1.
  float calibrated[4];
  calibrated[3] = 0.0;
  pAccelCal->transform(buf, calibrated);
  for(int i = 0; i < 3; ++i) {
     calibrated[3] += calibrated[i]*calibrated[i];
     Serial.print(calibrated[i]);
     Serial.print("\t");
  }
  Serial.println(sqrt(calibrated[3]));

  //Read and print the magnetometer data
  buf = pMag->rawReading();
  printInt16Array(buf,3);

  //Now transform the reading using the calibrator.
  //  Accumulate the sum
  // of squares too, so we can check that it is close to 1.
  calibrated[3] = 0.0;
  pMagCal->transform(buf, calibrated);
  for(int i = 0; i < 3; ++i) {
    calibrated[3] += calibrated[i]*calibrated[i];
    Serial.print(calibrated[i]);
    Serial.print("\t");
  }
  Serial.println(sqrt(calibrated[3]));

Then print a line to separate readings and wait one second.

  Serial.println("_________________________");  delay(1000);

That’s it!

Some Issues

This is freshly written and there are plenty of problems to fix and features to add. The most fundamental problem I see is with using the Gauss-Newton calibration objects. While I’ve tried to implement them efficiently, the objects are huge for the 2K of RAM we’re working with — each instance is 150 bytes! — so you can’t allocate too many of them on the heap without overwriting the stack (this doesn’t happen gracefully on an Arduino BTW). I have some ideas to trim it down, but for now I only see how to reclaim 15 bytes.

What’s worse is that the code size is huge. Using the Gauss-Newton calibrator instead of the Min-Max calibrator bloats the binary sketch by about 10KB. Maybe this just needs to be used to collect good numbers that we store in EEPROM before reprogramming.

With that said, there is structure in the code that could be exploited to significantly reduce the size.

Beyond that, all of the issues I raised in my last post still stand.

So there is work to do. But if you are looking for code to calibrate your accelerometer or magnetometer, this should get you started.

(UPDATE: I originally had named this library “SensorLib” — very creative, I know! — and found there were already a few SensorLibs on the internet. So I changed it to muCSense, for “micro-controller sensor”. It will take me just a little while to purge everything of the name SensorLib, but I’m working on it.)

Advertisements
This entry was posted in Electronics, How To and tagged , , , , , , , , , . Bookmark the permalink.

3 Responses to muCSense: Using Calibration

  1. ralphsrobots says:

    Thanks for sharing, Rolfe. It looks like we have a lot in common! I’ll be following.
    – Ralph

    • Excellent, I’ll be spending some time over at your site too. My son wants to start building robots and I need to figure out how to get started.

      FWIW, posting is going to slow down for a while, but should pick up again in the fall.

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s