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


All wikipages