IMU

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"
00002 
00003 Serial pc(USBTX, USBRX);
00004 
00005 //Change the second parameter (which is a tuning parameter)
00006 //to try and get optimal results. 
00007 IMUfilter imuFilter(0.1, 10);
00008 
00009 AnalogIn accelerometerX(p15);
00010 AnalogIn accelerometerY(p16);
00011 AnalogIn accelerometerZ(p17);
00012 AnalogIn gyroscopeX(p18);
00013 AnalogIn gyroscopeY(p19);
00014 AnalogIn gyroscopeZ(p20);
00015 
00016 int main() {
00017 
00018     while(1){
00019     
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(gyroscopeX.read(),
00024                                gyroscopeY.read(),
00025                                gyroscopeZ.read(),
00026                                accelerometerX.read(),
00027                                accelerometerY.read(),
00028                                accelerometerZ.read());
00029         imuFilter.computeEuler();
00030         pc.printf("%f, %f, %f\n", imuFilter.getRoll(), imuFilter.getPitch(), imuFilter.getYaw());
00031     
00032     }
00033 
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

Initialization

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.

Calibration

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.

Sampling

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"
00009 
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
00031 
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;
00042 
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;
00051 
00052 //Offsets for the accelerometer.
00053 //Same as with the gyroscope.
00054 double a_xBias;
00055 double a_yBias;
00056 double a_zBias;
00057 
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;
00065 
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;
00073 
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;
00080 
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);
00098 
00099 void initializeAccelerometer(void) {
00100 
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 http://www.analog.com/static/imported-files/application_notes/AN-1077.pdf
00110     wait_ms(22);
00111 
00112 }
00113 
00114 void sampleAccelerometer(void) {
00115 
00116     //Have we taken enough samples?
00117     if (accelerometerSamples == SAMPLES) {
00118 
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;
00124 
00125         a_xAccumulator = 0;
00126         a_yAccumulator = 0;
00127         a_zAccumulator = 0;
00128         accelerometerSamples = 0;
00129 
00130     } else {
00131         //Take another sample.
00132         accelerometer.getOutput(readings);
00133 
00134         a_xAccumulator += (int16_t) readings[0];
00135         a_yAccumulator += (int16_t) readings[1];
00136         a_zAccumulator += (int16_t) readings[2];
00137 
00138         accelerometerSamples++;
00139 
00140     }
00141 
00142 }
00143 
00144 void calibrateAccelerometer(void) {
00145 
00146     a_xAccumulator = 0;
00147     a_yAccumulator = 0;
00148     a_zAccumulator = 0;
00149     
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++) {
00153 
00154         accelerometer.getOutput(readings);
00155 
00156         a_xAccumulator += (int16_t) readings[0];
00157         a_yAccumulator += (int16_t) readings[1];
00158         a_zAccumulator += (int16_t) readings[2];
00159 
00160         wait(ACC_RATE);
00161 
00162     }
00163 
00164     a_xAccumulator /= CALIBRATION_SAMPLES;
00165     a_yAccumulator /= CALIBRATION_SAMPLES;
00166     a_zAccumulator /= CALIBRATION_SAMPLES;
00167 
00168     //At 4mg/LSB, 250 LSBs is 1g.
00169     a_xBias = a_xAccumulator;
00170     a_yBias = a_yAccumulator;
00171     a_zBias = (a_zAccumulator - 250);
00172 
00173     a_xAccumulator = 0;
00174     a_yAccumulator = 0;
00175     a_zAccumulator = 0;
00176 
00177 }
00178 
00179 void initializeGyroscope(void) {
00180 
00181     //Low pass filter bandwidth of 42Hz.
00182     gyroscope.setLpBandwidth(LPFBW_42HZ);
00183     //Internal sample rate of 200Hz. (1kHz / 5).
00184     gyroscope.setSampleRateDivider(4);
00185 
00186 }
00187 
00188 void calibrateGyroscope(void) {
00189 
00190     w_xAccumulator = 0;
00191     w_yAccumulator = 0;
00192     w_zAccumulator = 0;
00193 
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++) {
00197 
00198         w_xAccumulator += gyroscope.getGyroX();
00199         w_yAccumulator += gyroscope.getGyroY();
00200         w_zAccumulator += gyroscope.getGyroZ();
00201         wait(GYRO_RATE);
00202 
00203     }
00204 
00205     //Average the samples.
00206     w_xAccumulator /= CALIBRATION_SAMPLES;
00207     w_yAccumulator /= CALIBRATION_SAMPLES;
00208     w_zAccumulator /= CALIBRATION_SAMPLES;
00209 
00210     w_xBias = w_xAccumulator;
00211     w_yBias = w_yAccumulator;
00212     w_zBias = w_zAccumulator;
00213 
00214     w_xAccumulator = 0;
00215     w_yAccumulator = 0;
00216     w_zAccumulator = 0;
00217 
00218 }
00219 
00220 void sampleGyroscope(void) {
00221 
00222     //Have we taken enough samples?
00223     if (gyroscopeSamples == SAMPLES) {
00224 
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);
00230 
00231         w_xAccumulator = 0;
00232         w_yAccumulator = 0;
00233         w_zAccumulator = 0;
00234         gyroscopeSamples = 0;
00235 
00236     } else {
00237         //Take another sample.
00238         w_xAccumulator += gyroscope.getGyroX();
00239         w_yAccumulator += gyroscope.getGyroY();
00240         w_zAccumulator += gyroscope.getGyroZ();
00241 
00242         gyroscopeSamples++;
00243 
00244     }
00245 
00246 }
00247 
00248 void filter(void) {
00249 
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();
00254 
00255 }
00256 
00257 int main() {
00258 
00259     pc.printf("Starting IMU filter test...\n");
00260 
00261     //Initialize inertial sensors.
00262     initializeAccelerometer();
00263     calibrateAccelerometer();
00264     initializeGyroscope();
00265     calibrateGyroscope();
00266 
00267 
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);
00275    
00276     while (1) {
00277 
00278         wait(FILTER_RATE);
00279 
00280         
00281         pc.printf("%f,%f,%f\n",toDegrees(imuFilter.getRoll()),
00282                   toDegrees(imuFilter.getPitch()),
00283                   toDegrees(imuFilter.getYaw()));
00284          
00285 
00286     }
00287 
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: http://mbed.org/users/aberk/programs/IMUfilter_RPYExample/latest

Demonstration

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

API

Import library

Public Member Functions

  IMUfilter (double rate, double gyroscopeMeasurementError)
  Constructor.
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.

Library

Import libraryIMUfilter

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

Reference




1 related question:


30 comments:

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 (http://www.watterott.com/de/IMU-Digital-Combo-Board-6-Degrees-of-Freedom-ITG3200/ADXL345) 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 (http://www.youtube.com/watch?v=XqQCbkncVYI and http://www.rcgroups.com/forums/showthread.php?t=1024452 and http://mycoordinates.org/calibration-method-of-imu-based-only-accelerometers-2/). 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 http://www.x-io.co.uk/node/9. 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

Tony

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

Hi,

Perhaps you guys should check this link out http://www.varesano.net/projects/hardware/FreeIMU#0.3.5_MS 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.

Dave.

07 Aug 2011

Matt Clement wrote:

Tony

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

Hi

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.

Regards,

Dave

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/imu3_6dof.zip

Dave

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

Alex

This link may help you http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/ 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.

Dave.

27 Sep 2011

Dave,

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

Alex,

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.

Dave.

29 Sep 2011

Dave,

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

Alex,

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/ahrs_port.zip

Dave