The orientation filter used in the IMUfilter library takes in linear acceleration and angular velocity values in m/s2 and radians/s, respectively, and calculates the current Euler, or roll, pitch and yaw angles. It fuses the data in such a way as to make the current orientation more accurate than if an accelerometer or gyroscope were used separately to calculate it.

The filter is ported from Sebastian Madgwick's paper, "An efficient orientation filter for inertial and inertial/magnetic sensor arrays". More information about his work can be found here

Hello World!

The filter uses the sampling time passed in to the constructor in its calculations. In order to get the most out of the filter, ensure you update the filter (by passing in new readings for the acceleration and angular velocity) and calculate the new Euler angles, at the same rate as was specified when the filter object was created.

Import program

00001 #include "IMUfilter.h"
00003 Serial pc(USBTX, USBRX);
00005 //Change the second parameter (which is a tuning parameter)
00006 //to try and get optimal results. 
00007 IMUfilter imuFilter(0.1, 10);
00009 AnalogIn accelerometerX(p15);
00010 AnalogIn accelerometerY(p16);
00011 AnalogIn accelerometerZ(p17);
00012 AnalogIn gyroscopeX(p18);
00013 AnalogIn gyroscopeY(p19);
00014 AnalogIn gyroscopeZ(p20);
00016 int main() {
00018     while(1){
00020         wait(0.1);
00021         //Gyro and accelerometer values must be converted to rad/s and m/s/s
00022         //before being passed to the filter.
00023         imuFilter.updateFilter(,
00024                      ,
00025                      ,
00026                      ,
00027                      ,
00028                      ;
00029         imuFilter.computeEuler();
00030         pc.printf("%f, %f, %f\n", imuFilter.getRoll(), imuFilter.getPitch(), imuFilter.getYaw());
00032     }
00034 }

Example IMU: ADXL345 accelerometer and ITG-3200 gyroscope

We can use an ADXL345 and ITG-3200 to create an inertial measurement unit which can supply acceleration and angular velocity readings to the IMUfilter.

For use of the Digital Combo Board

If using the Digital Combo Board (sold by sparkfun) which includes both ADXL345 and ITG-3200 in the same chip, then import the I2C version of the ADXL345 program along with the ITG-3200 program and choose the same pins for their input/output. The Digital Combo board communicates by sharing the same pins over I2C. /media/uploads/peterswanson87/digitalcomboboard.jpg


Both the ADXL345 and ITG-3200 examples show how to set up the devices; we're mainly concerned with how fast we're going to sample the devices. As we increase the sample rate, the data we receive will get noisier but we will be able to average lots of samples to try and minimize this. We'll choose a sweet spot of 200Hz which is slow enough to reduce a lot of noise but fast enough to be able to average around 4 values before they're plugged into the filter.

The filter can operate well at rates as low as 10Hz. We'll choose a rate of 40Hz as it provides a better update rate when observing the orientation graphically.


Most likely, our inertial sensors won't read 0 m/s2 or 0 radians/s even when they're still; this means we'll need to calibrate them and calculate their zero offset - that is, how far their readings are from zero. If we take this value away from all the further readings we will then get sensible results instead of thinking we're accelerating or turning even when we're stationary.

The calibration presented here is simply taking a set of readings and averaging them to calculate the zero offset, which will then be substracted from all future readings. We will use the typical sensitivity values (how the raw binary representation relates to a real world value of m/s2 or radians/s) presented on the datasheets. You can find a detailed look at more robust inertial sensor calibration here.


To try and reduce some of the noise from our sensors, instead of just taking one reading, we'll take several and then use the mean of the samples as our final result. With the data and update rates we've chosen, we have enough time for about 4 samples per filter update.

Example Program

The following example program shows how to initialize, calibrate and sample an ADXL345 and ITG3200, as well as feeding their results into the IMUfilter which then calculates the current Euler angles.

Import program

00001 /**
00002  * IMU filter example.
00003  *
00004  * Calculate the roll, pitch and yaw angles.
00005  */
00006 #include "IMUfilter.h"
00007 #include "ADXL345.h"
00008 #include "ITG3200.h"
00010 //Gravity at Earth's surface in m/s/s
00011 #define g0 9.812865328
00012 //Number of samples to average.
00013 #define SAMPLES 4
00014 //Number of samples to be averaged for a null bias calculation
00015 //during calibration.
00016 #define CALIBRATION_SAMPLES 128
00017 //Convert from radians to degrees.
00018 #define toDegrees(x) (x * 57.2957795)
00019 //Convert from degrees to radians.
00020 #define toRadians(x) (x * 0.01745329252)
00021 //ITG-3200 sensitivity is 14.375 LSB/(degrees/sec).
00022 #define GYROSCOPE_GAIN (1 / 14.375)
00023 //Full scale resolution on the ADXL345 is 4mg/LSB.
00024 #define ACCELEROMETER_GAIN (0.004 * g0)
00025 //Sampling gyroscope at 200Hz.
00026 #define GYRO_RATE   0.005
00027 //Sampling accelerometer at 200Hz.
00028 #define ACC_RATE    0.005
00029 //Updating filter at 40Hz.
00030 #define FILTER_RATE 0.1
00032 Serial pc(USBTX, USBRX);
00033 //At rest the gyroscope is centred around 0 and goes between about
00034 //-5 and 5 counts. As 1 degrees/sec is ~15 LSB, error is roughly
00035 //5/15 = 0.3 degrees/sec.
00036 IMUfilter imuFilter(FILTER_RATE, 0.3);
00037 ADXL345 accelerometer(p5, p6, p7, p8);
00038 ITG3200 gyroscope(p9, p10);
00039 Ticker accelerometerTicker;
00040 Ticker gyroscopeTicker;
00041 Ticker filterTicker;
00043 //Offsets for the gyroscope.
00044 //The readings we take when the gyroscope is stationary won't be 0, so we'll
00045 //average a set of readings we do get when the gyroscope is stationary and
00046 //take those away from subsequent readings to ensure the gyroscope is offset
00047 //or "biased" to 0.
00048 double w_xBias;
00049 double w_yBias;
00050 double w_zBias;
00052 //Offsets for the accelerometer.
00053 //Same as with the gyroscope.
00054 double a_xBias;
00055 double a_yBias;
00056 double a_zBias;
00058 //Accumulators used for oversampling and then averaging.
00059 volatile double a_xAccumulator = 0;
00060 volatile double a_yAccumulator = 0;
00061 volatile double a_zAccumulator = 0;
00062 volatile double w_xAccumulator = 0;
00063 volatile double w_yAccumulator = 0;
00064 volatile double w_zAccumulator = 0;
00066 //Accelerometer and gyroscope readings for x, y, z axes.
00067 volatile double a_x;
00068 volatile double a_y;
00069 volatile double a_z;
00070 volatile double w_x;
00071 volatile double w_y;
00072 volatile double w_z;
00074 //Buffer for accelerometer readings.
00075 int readings[3];
00076 //Number of accelerometer samples we're on.
00077 int accelerometerSamples = 0;
00078 //Number of gyroscope samples we're on.
00079 int gyroscopeSamples = 0;
00081 /**
00082  * Prototypes
00083  */
00084 //Set up the ADXL345 appropriately.
00085 void initializeAcceleromter(void);
00086 //Calculate the null bias.
00087 void calibrateAccelerometer(void);
00088 //Take a set of samples and average them.
00089 void sampleAccelerometer(void);
00090 //Set up the ITG3200 appropriately.
00091 void initializeGyroscope(void);
00092 //Calculate the null bias.
00093 void calibrateGyroscope(void);
00094 //Take a set of samples and average them.
00095 void sampleGyroscope(void);
00096 //Update the filter and calculate the Euler angles.
00097 void filter(void);
00099 void initializeAccelerometer(void) {
00101     //Go into standby mode to configure the device.
00102     accelerometer.setPowerControl(0x00);
00103     //Full resolution, +/-16g, 4mg/LSB.
00104     accelerometer.setDataFormatControl(0x0B);
00105     //200Hz data rate.
00106     accelerometer.setDataRate(ADXL345_200HZ);
00107     //Measurement mode.
00108     accelerometer.setPowerControl(0x08);
00109     //See
00110     wait_ms(22);
00112 }
00114 void sampleAccelerometer(void) {
00116     //Have we taken enough samples?
00117     if (accelerometerSamples == SAMPLES) {
00119         //Average the samples, remove the bias, and calculate the acceleration
00120         //in m/s/s.
00121         a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN;
00122         a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN;
00123         a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN;
00125         a_xAccumulator = 0;
00126         a_yAccumulator = 0;
00127         a_zAccumulator = 0;
00128         accelerometerSamples = 0;
00130     } else {
00131         //Take another sample.
00132         accelerometer.getOutput(readings);
00134         a_xAccumulator += (int16_t) readings[0];
00135         a_yAccumulator += (int16_t) readings[1];
00136         a_zAccumulator += (int16_t) readings[2];
00138         accelerometerSamples++;
00140     }
00142 }
00144 void calibrateAccelerometer(void) {
00146     a_xAccumulator = 0;
00147     a_yAccumulator = 0;
00148     a_zAccumulator = 0;
00150     //Take a number of readings and average them
00151     //to calculate the zero g offset.
00152     for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
00154         accelerometer.getOutput(readings);
00156         a_xAccumulator += (int16_t) readings[0];
00157         a_yAccumulator += (int16_t) readings[1];
00158         a_zAccumulator += (int16_t) readings[2];
00160         wait(ACC_RATE);
00162     }
00164     a_xAccumulator /= CALIBRATION_SAMPLES;
00165     a_yAccumulator /= CALIBRATION_SAMPLES;
00166     a_zAccumulator /= CALIBRATION_SAMPLES;
00168     //At 4mg/LSB, 250 LSBs is 1g.
00169     a_xBias = a_xAccumulator;
00170     a_yBias = a_yAccumulator;
00171     a_zBias = (a_zAccumulator - 250);
00173     a_xAccumulator = 0;
00174     a_yAccumulator = 0;
00175     a_zAccumulator = 0;
00177 }
00179 void initializeGyroscope(void) {
00181     //Low pass filter bandwidth of 42Hz.
00182     gyroscope.setLpBandwidth(LPFBW_42HZ);
00183     //Internal sample rate of 200Hz. (1kHz / 5).
00184     gyroscope.setSampleRateDivider(4);
00186 }
00188 void calibrateGyroscope(void) {
00190     w_xAccumulator = 0;
00191     w_yAccumulator = 0;
00192     w_zAccumulator = 0;
00194     //Take a number of readings and average them
00195     //to calculate the gyroscope bias offset.
00196     for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
00198         w_xAccumulator += gyroscope.getGyroX();
00199         w_yAccumulator += gyroscope.getGyroY();
00200         w_zAccumulator += gyroscope.getGyroZ();
00201         wait(GYRO_RATE);
00203     }
00205     //Average the samples.
00206     w_xAccumulator /= CALIBRATION_SAMPLES;
00207     w_yAccumulator /= CALIBRATION_SAMPLES;
00208     w_zAccumulator /= CALIBRATION_SAMPLES;
00210     w_xBias = w_xAccumulator;
00211     w_yBias = w_yAccumulator;
00212     w_zBias = w_zAccumulator;
00214     w_xAccumulator = 0;
00215     w_yAccumulator = 0;
00216     w_zAccumulator = 0;
00218 }
00220 void sampleGyroscope(void) {
00222     //Have we taken enough samples?
00223     if (gyroscopeSamples == SAMPLES) {
00225         //Average the samples, remove the bias, and calculate the angular
00226         //velocity in rad/s.
00227         w_x = toRadians(((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN);
00228         w_y = toRadians(((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN);
00229         w_z = toRadians(((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN);
00231         w_xAccumulator = 0;
00232         w_yAccumulator = 0;
00233         w_zAccumulator = 0;
00234         gyroscopeSamples = 0;
00236     } else {
00237         //Take another sample.
00238         w_xAccumulator += gyroscope.getGyroX();
00239         w_yAccumulator += gyroscope.getGyroY();
00240         w_zAccumulator += gyroscope.getGyroZ();
00242         gyroscopeSamples++;
00244     }
00246 }
00248 void filter(void) {
00250     //Update the filter variables.
00251     imuFilter.updateFilter(w_y, w_x, w_z, a_y, a_x, a_z);
00252     //Calculate the new Euler angles.
00253     imuFilter.computeEuler();
00255 }
00257 int main() {
00259     pc.printf("Starting IMU filter test...\n");
00261     //Initialize inertial sensors.
00262     initializeAccelerometer();
00263     calibrateAccelerometer();
00264     initializeGyroscope();
00265     calibrateGyroscope();
00268     //Set up timers.
00269     //Accelerometer data rate is 200Hz, so we'll sample at this speed.
00270     accelerometerTicker.attach(&sampleAccelerometer, 0.005);
00271     //Gyroscope data rate is 200Hz, so we'll sample at this speed.
00272     gyroscopeTicker.attach(&sampleGyroscope, 0.005);
00273     //Update the filter variables at the correct rate.
00274     filterTicker.attach(&filter, FILTER_RATE);
00276     while (1) {
00278         wait(FILTER_RATE);
00281         pc.printf("%f,%f,%f\n",toDegrees(imuFilter.getRoll()),
00282                   toDegrees(imuFilter.getPitch()),
00283                   toDegrees(imuFilter.getYaw()));
00286     }
00288 }

You can also find this example wrapped up in a class [for easier interfacing in other programs] here:

Import programIMU

The IMUfilter Roll/Pitch/Yaw example wrapped up into a class for easier use with other programs:


Using LabVIEW to visualise the roll, pitch and yaw angles calculated by filter.


Import library

Public Member Functions

  IMUfilter (double rate, double gyroscopeMeasurementError)
void  updateFilter (double w_x, double w_y, double w_z, double a_x, double a_y, double a_z)
  Update the filter variables.
void  computeEuler (void)
  Compute the Euler angles based on the current filter data.
double  getRoll (void)
  Get the current roll.
double  getPitch (void)
  Get the current pitch.
double  getYaw (void)
  Get the current yaw.
void  reset (void)
  Reset the filter.


Import libraryIMUfilter

Inertial measurement unit orientation filter. Ported from Sebastian Madgwick's paper, "An efficient orientation filter for inertial and inertial/magnetic sensor arrays".


1 related question:


04 Feb 2011

I'm getting 'nan' faults when I pitch my sensor board to 90 degrees.

Is there a way to make sure that I have my x's,y's and z's all in the correct planes and at the right polarities?

13 Mar 2011

Hi Ian, me, too. I'm getting 'nan' when I pitch my sensor board to 90 degs. Do you got a solution for this? Regards Stanislaus

13 Mar 2011

I think I have a partial solution. I am using my own accelerometer/gyro setup and I changed the axes around and the problem has improved. Also playing with the tuning settings has helped.

15 Mar 2011

Ian Molesworth wrote:

I think I have a partial solution. I am using my own accelerometer/gyro setup and I changed the axes around and the problem has improved. Also playing with the tuning settings has helped.

Yah, the axes were a problem on my side, too. There's a new module ( available. The chips had another arrangement as I had on my bread board. What tuning settings do you mean exactly? I think the IMU should be calibrated at all ( and and That's the next step I have to take. Thanks Ian.

16 Apr 2011

Hi, I have been writing a library for the ADXL345 to be able to communicate over I2C. This is needed to use the IMU Digital Combo Board which could implement this filter using 1 chip. However, I also am having the issue with "Nan" being outputted for the pitch at various times. Also, while the filter initializes the angles to zero, after I move the chip to various angles and bring it back to its original location, it now has an offset and does not produce the same readings that it did when it started at that same angle.

- Peter

31 May 2011

Hello A Berk This is something gr8 work. Have u tested the whole set up with X-io GUI software. Sebastian Madgwick has recently upadate the new version of X-io board with new GUI Software The open source x-IMU software allow users configure all internal x-IMU settings, view sensor data in real-time and export data to software such as MATLAB and Microsoft Excel. Custom user software may be developed using the x-IMU API. So we should check mbed setup with IMU with Software. I will very soon put my work on it.

Here is Madgwick work

07 Jul 2011

Hi, is there a version of the IMU filter for a 5DOF IMU? My gyro does not have a Z axis (or w_z axis)... Thanks

21 Jul 2011


Did you find a version for the 5DOF? I also have the 5DOF and would love a version. If one isnt available I guess we'll have to port his existing library. It should be fairly straightforward.

25 Jul 2011


Perhaps you guys should check this link out The software is easy to change and run on an mbed with the added advantage that the reported NAN goes away and the software / hardware is still being developed.


07 Aug 2011

Matt Clement wrote:


Did you find a version for the 5DOF? I also have the 5DOF and would love a version. If one isnt available I guess we'll have to port his existing library. It should be fairly straightforward.

No, I did not. I gave my attempt at porting his code but I don't know enough about quaternion math to successfully do it. Let me know if you figure it out.

18 Sep 2011

So do I still need Kalman or Complimentary Filter if I use this?

19 Sep 2011


The link I posted above is for a 9DOF and works really well, but if you just want a Kalman Filter to run on x,y then google "tilt.c" . Its been reported as an EKF in some quarters. It works when setup correctly ie using the mbed ticker and attach, there are also 2 parameters that need to be setup the given defaults will put you in the ballpark.



23 Sep 2011

I have dug out a 6DOF IMU lib it can also be used on 5DOF by giving the z axis gyro a value of 0, but dont expect the yaw axis to be stable. /media/uploads/dflet/


23 Sep 2011

I've just ordered a 9dof board for use with a tricopter platform I'm building.

26 Sep 2011

From using IMUFilter with neglectable tilt, I don't see yaw angle drift as much, but I know it's been said we need magnetometer to get correct readings for yaw angles. Trying to understand quaternions has been super dizzy! I'm trying to work out the equations from S.O.H. Madgwick to see how they are being reshuffled and constant optimized in this version by Aaron Berk, but it gets very complicated.

26 Sep 2011


This link may help you there may be better sites. You can also checkout S.O.H.Madgwick's papers on the web. Aaron Berk's implementation is almost the same as IMU3_6DOF I posted above, which I believe came from Fabio Varesano's freeIMU project. If your comments above ref yaw drift is in respect to 6DOF you should be able to trim most of the drift out, further reading of Madgwick's paper is advised. Good luck with your understanding of quaternions.


27 Sep 2011


Thanks for the link and references. Yes, I want to understand about quaternion, Euler angles, matrix rotations, etc., but the my linear algebra has been rusty since college days. About the yaw drift, I noticed experimentally if I'm in constant z-axis changes, the yaw angles stay within a range of a few degress. But if I held steady, I see drifts to totally different values.

I just bought HMC5883 breakout boards, and I'm trying to integrate it into IMUFilter this week. I'll also add in magnetometer calibration as well. Let's how things go.

28 Sep 2011


If you are going to use the HMC5843 library for your HMC5883 you will need to swap y and z as they are different. While your at it you may also want to change the gauss values along with the DORs, check the data sheet, these two are not show stoppers. With respect to your intergrating the magnetomers into the quaternions, if you loose your faith in going so, i have already ported Fabio Varesano's Arduino filter code to the mbed. There is also available, calibration code for the magnetometers using Processing. One more comment, you have probably seen all the hype on Youtube etc showing the videos of spinning graphics, yes you can do that with very little extra work, but if you intend to interface to the outside world using Euler angles your going to meet "gibal lock" or "singularities". If your not aware of these problems I leave you and any other reader to do a web search. I have not seen any solutions (there are a number of work arounds) that really solve the problem, if anybody has, please post it.... It's probably not going to bother you, if your intended use is to manualy control some type of rolling, walking, or flying object and auto-balancing, "so no worries then!", but keep it in mind, if you get bored and find yourself watching Euler angles change.


29 Sep 2011


Yes, I was looking at HMC5843 library. You're so right! The DOR MSB/LSB values for y-axis and z-axis are swapped in HMC5883. Thanks for pointing this out, because I wrongly assumed "backwards compatibility". The HMC5843 library uses the default +/-1.0GA, but HMC5883 has +/-1.5GA default with Register B at 0x20 too. How do I know what's the "right" gain (LSb/gauss)? Yes, I did come across the term "gimbal lock" when I was reading about gyroscope, but I forgot about it. Is your code published on mBed site? As for Processing language, that's one more thing I haven't tried out yet.

Thanks. You surely are an expert in this! I appreciate your taking the time to help out. :)

29 Sep 2011


I am certainly not an expert, just picked up a few things along the path looking for a good IMU, AHRS or DCM. Open source is a wonderful world were you dont need to re-invent the wheel just make it better if possible. The right gain for your magnetometer depends on the invironment it's going to be used, the default is a good place to start. Here is the filter I ported from Fabio Varesano's Arduino code, of course the filter is Seb Madgwick's work. There is also a Processing sketch to view orientations made with your project if you need it, let me know.

EDIT: Corrected ahrs_port /media/uploads/dflet/