Good software IMU with data fusion

A project log for 8BitRobots Module

A common hardware, software and 3D printed module to enable fun, educational robots anyone can print and program.

Tim WilkinsonTim Wilkinson 11/20/2018 at 05:482 Comments

One of the single most expensive components on the Module board is the BNO055 IMU. In single quantities it costs $12 ( I, like many others, chose this chip because it has two very appealing qualities - it self calibrates and outputs quaternions. This avoids lots and lots of math on the host CPU; math I mostly don't understand.

However, as I look at moving this module board towards production, the cost of this chip annoys me; especially when I'm designing various projects which don't immediately need an IMU. It increases the BOM costs substantially, while offering little immediate gain.

Because I do want an IMU on this board, I've begun to look at alternatives. In that process I realized the only way to decrease the cost is to do the math on the PI.

There are three sets of math that must be done for a good software fusion IMU:

Calibration Math

I've taken a stab at IMU calibration math a couple of times before and ended up abandoning the efforts as the results were not good. There are lots of algorithms which attempt this process, and many of them appear to assume IMUs are far better behaved than they actually are. I eventually found this calibration explanation ( by the Cave Pearl Project which uses this tool ( by Sailboat Instruments. Essentially a set of raw datapoint are gather from the magnetometer and accelerometer (each set form an elliptical shape) and maps them into a sphere centered on an origin. The  Sailboat tool (Magneto - generates this as a vector translation and matrix transformation.

While the Cage Pearl article discusses doing this analysis offline (they're using Arduinos which are not up to the online math), they include a C implementation of their algorithm; one quite capable of running on a Pi.

Calibration Cleaning

Just feeding a large set of values to the calibration algorithm will not necessarily get the best results. Ideally you need many points from all orientation of the IMU in order to get the best transformation. Also, because IMUs are noisy devices, they generate outlier values which can confuse the calibration algorithm.

So it is necessary to clean the calibration data before using it to calibrate. To gather a "good" set of points, each point is translated from (x,y,z) form into spherical coordinates (inclination, azimuth); think of this as the longitude and latitude of the point on the surface of a sphere. We divide the surface of the sphere up into a number of "buckets" of approximately equal area, and place each point into the appropriate bucket. The goal for good calibration is to sample enough points and place a minimal number into each bucket.

One the buckets are full, we must eliminate any outliers before passing the points for calibration. Outliers are consider to be any point where an (x,y,z) value is outside 2 standard deviations of the mean. Once removed from out dataset, the resulting points generate excellent calibration data.

Operation Math

Using the calibration data, we can now adjust the IMU's raw values to make them usable. However, we still need to turn this data into a quaternion. A quaternion is a 4-dimensional vector which is used here to represent the rotation of an object (

The best algorithm I found to handle this process is by Sebastian Madgwick ( It takes (x,y,z) inputs from the three IMU sensors (accelerometer, gyroscope and magnetometer) and generates a quaternion (w,x,y,z). The algorithm runs in a loop, with inputs constantly updated in a predicable, period way. The faster the loop, the more sensitive the quaternion to changes in the position of the IMU.


My final implementation is a combination of Javascript and C. The C code handles all the heavy math, while the Javascript does all the data management. Calibration is automatic; the code gathers calibration points continually and adjusting the calibration as necessary. This means that once calibration is established small changes in the environment, which might effect the sensor readings, should be compensated for. Calibration data can also be saved and restored, so re-calibration on startup need only be done when absolutely necessary.


For experimental purposes I've been using an old LSM303DLHC+L3GD20H board (; I think it's discontinued now. This already generates pretty good data and, unlike some other IMUs, the axises are all aligned. Having different sensors with different axises alignments isn't the end of the world, but trying to wrap my mind around how to adjust things has proved ... difficult.

The results from my tests have been excellent and probably more stable than the BNO055 which tends to loose calibration randomly. I'm not sure I expected to end up with a better sensor at the end of this process, but that's what I got.

Which IMU?

I'm now at the point that I need to choose my final, replacement IMU. Cheap is important. It should cost no more than $6 (1-off quantities), preferably less. But it also needs to have great repeatability and low drift because software correction can only do so much with noisy data.

If anyone has any recommendations, please let me know.


t3chflicks wrote 01/09/2020 at 16:22 point

Great to see someone else is interested in solving this problem. We want to be able to do sensor fusion for monitoring waves . Let us know what you decide on, please.

  Are you sure? yes | no

david.a wrote 04/15/2019 at 22:51 point

Thanks for the article. I'm wondering if you have settled on a final IMU yet?

  Are you sure? yes | no