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.
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".