Matthias Grob / Mbed 2 deprecated IMU_10DOF

An fully working IMU-Filter and Sensor drivers for the 10DOF-Board over I2C. All in one simple class. Include, calibrate sensors, call read, get angles. (3D Visualisation code for Python also included) Sensors: L3G4200D, ADXL345, HMC5883, BMP085

Dependencies:   mbed

IMU/IMU_Filter/IMU_Filter.cpp@4:f62337b907e5, 2013-08-29 (annotated)

Committer:
maetugr
Date:
Thu Aug 29 13:52:30 2013 +0000
Revision:
4:f62337b907e5
Parent:
0:3e7450f1a938 
The Altitude Sensor is now implemented, it's really 10DOF now ;); TODO: Autocalibration

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maetugr 0:3e7450f1a938 1 #include "IMU_Filter.h"
maetugr 0:3e7450f1a938 2
maetugr 0:3e7450f1a938 3 // IMU/AHRS
maetugr 0:3e7450f1a938 4 #define PI 3.1415926535897932384626433832795
maetugr 0:3e7450f1a938 5 #define Kp 2.0f // proportional gain governs rate of convergence to accelerometer/magnetometer
maetugr 0:3e7450f1a938 6 #define Ki 0.005f // integral gain governs rate of convergence of gyroscope biases
maetugr 0:3e7450f1a938 7
maetugr 0:3e7450f1a938 8 IMU_Filter::IMU_Filter()
maetugr 0:3e7450f1a938 9 {
maetugr 0:3e7450f1a938 10 for(int i=0; i<3; i++)
maetugr 0:3e7450f1a938 11 angle[i]=0;
maetugr 0:3e7450f1a938 12
maetugr 0:3e7450f1a938 13 // IMU/AHRS
maetugr 0:3e7450f1a938 14 q0 = 1; q1 = 0; q2 = 0; q3 = 0;
maetugr 0:3e7450f1a938 15 exInt = 0; eyInt = 0; ezInt = 0;
maetugr 0:3e7450f1a938 16 }
maetugr 0:3e7450f1a938 17
maetugr 0:3e7450f1a938 18 void IMU_Filter::compute(float dt, const float * Gyro_data, const float * Acc_data, const float * Comp_data)
maetugr 0:3e7450f1a938 19 {
maetugr 0:3e7450f1a938 20 // IMU/AHRS (from http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/)
maetugr 0:3e7450f1a938 21
maetugr 0:3e7450f1a938 22 float radGyro[3]; // Gyro in radians per second
maetugr 0:3e7450f1a938 23 for(int i=0; i<3; i++)
maetugr 0:3e7450f1a938 24 radGyro[i] = Gyro_data[i] * PI / 180;
maetugr 0:3e7450f1a938 25
maetugr 0:3e7450f1a938 26 //IMUupdate(dt/2, radGyro[0], radGyro[1], radGyro[2], Acc_data[0], Acc_data[1], Acc_data[2]);
maetugr 0:3e7450f1a938 27 AHRSupdate(dt/2, radGyro[0], radGyro[1], radGyro[2], Acc_data[0], Acc_data[1], Acc_data[2], Comp_data[0], Comp_data[1], Comp_data[2]);
maetugr 0:3e7450f1a938 28
maetugr 0:3e7450f1a938 29 float rangle[3]; // calculate angles in radians from quternion output, formula from Wiki (http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles)
maetugr 0:3e7450f1a938 30 rangle[0] = atan2(2*q0*q1 + 2*q2*q3, 1 - 2*(q1*q1 + q2*q2));
maetugr 0:3e7450f1a938 31 rangle[1] = asin(2*q0*q2 - 2*q3*q1);
maetugr 0:3e7450f1a938 32 rangle[2] = atan2(2*q0*q3 + 2*q1*q2, 1 - 2*(q2*q2 + q3*q3));
maetugr 0:3e7450f1a938 33
maetugr 0:3e7450f1a938 34 // TODO
maetugr 0:3e7450f1a938 35 // Pitch should have a range of +/-90 degrees.
maetugr 0:3e7450f1a938 36 // After you pitch past vertical (90 degrees) your roll and yaw value should swing 180 degrees.
maetugr 0:3e7450f1a938 37 // A pitch value of 100 degrees is measured as a pitch of 80 degrees and inverted flight (roll = 180 degrees).
maetugr 0:3e7450f1a938 38 // Another example is a pitch of 180 degrees (upside down). This is measured as a level pitch (0 degrees) and a roll of 180 degrees.
maetugr 0:3e7450f1a938 39 // And I think this solves the upside down issue...
maetugr 0:3e7450f1a938 40 // Handle roll reversal when inverted
maetugr 0:3e7450f1a938 41 /*if (Acc_data[2] < 0) {
maetugr 0:3e7450f1a938 42 if (Acc_data[0] < 0) {
maetugr 0:3e7450f1a938 43 rangle[1] = (180 - rangle[1]);
maetugr 0:3e7450f1a938 44 } else {
maetugr 0:3e7450f1a938 45 rangle[1] = (-180 - rangle[1]);
maetugr 0:3e7450f1a938 46 }
maetugr 0:3e7450f1a938 47 }*/
maetugr 0:3e7450f1a938 48
maetugr 0:3e7450f1a938 49 for(int i=0; i<3; i++) // angle in degree
maetugr 0:3e7450f1a938 50 angle[i] = rangle[i] * 180 / PI;
maetugr 0:3e7450f1a938 51 }
maetugr 0:3e7450f1a938 52
maetugr 0:3e7450f1a938 53 //------------------------------------------------------------------------------------------------------------------
maetugr 4:f62337b907e5 54 // Code copied from S.O.H. Madgwick (File AHRS.c from https://code.google.com/p/imumargalgorithm30042010sohm/)
maetugr 0:3e7450f1a938 55 //------------------------------------------------------------------------------------------------------------------
maetugr 0:3e7450f1a938 56
maetugr 0:3e7450f1a938 57 // IMU
maetugr 0:3e7450f1a938 58 void IMU_Filter::IMUupdate(float halfT, float gx, float gy, float gz, float ax, float ay, float az) {
maetugr 0:3e7450f1a938 59 float norm;
maetugr 0:3e7450f1a938 60 float vx, vy, vz;
maetugr 0:3e7450f1a938 61 float ex, ey, ez;
maetugr 0:3e7450f1a938 62
maetugr 0:3e7450f1a938 63 // normalise the measurements
maetugr 0:3e7450f1a938 64 norm = sqrt(ax*ax + ay*ay + az*az);
maetugr 0:3e7450f1a938 65 if(norm == 0.0f) return;
maetugr 0:3e7450f1a938 66 ax = ax / norm;
maetugr 0:3e7450f1a938 67 ay = ay / norm;
maetugr 0:3e7450f1a938 68 az = az / norm;
maetugr 0:3e7450f1a938 69
maetugr 0:3e7450f1a938 70 // estimated direction of gravity
maetugr 0:3e7450f1a938 71 vx = 2*(q1*q3 - q0*q2);
maetugr 0:3e7450f1a938 72 vy = 2*(q0*q1 + q2*q3);
maetugr 0:3e7450f1a938 73 vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
maetugr 0:3e7450f1a938 74
maetugr 0:3e7450f1a938 75 // error is sum of cross product between reference direction of field and direction measured by sensor
maetugr 0:3e7450f1a938 76 ex = (ay*vz - az*vy);
maetugr 0:3e7450f1a938 77 ey = (az*vx - ax*vz);
maetugr 0:3e7450f1a938 78 ez = (ax*vy - ay*vx);
maetugr 0:3e7450f1a938 79
maetugr 0:3e7450f1a938 80 // integral error scaled integral gain
maetugr 0:3e7450f1a938 81 exInt = exInt + ex*Ki;
maetugr 0:3e7450f1a938 82 eyInt = eyInt + ey*Ki;
maetugr 0:3e7450f1a938 83 ezInt = ezInt + ez*Ki;
maetugr 0:3e7450f1a938 84
maetugr 0:3e7450f1a938 85 // adjusted gyroscope measurements
maetugr 0:3e7450f1a938 86 gx = gx + Kp*ex + exInt;
maetugr 0:3e7450f1a938 87 gy = gy + Kp*ey + eyInt;
maetugr 0:3e7450f1a938 88 gz = gz + Kp*ez + ezInt;
maetugr 0:3e7450f1a938 89
maetugr 0:3e7450f1a938 90 // integrate quaternion rate and normalise
maetugr 0:3e7450f1a938 91 q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
maetugr 0:3e7450f1a938 92 q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
maetugr 0:3e7450f1a938 93 q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
maetugr 0:3e7450f1a938 94 q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;
maetugr 0:3e7450f1a938 95
maetugr 0:3e7450f1a938 96 // normalise quaternion
maetugr 0:3e7450f1a938 97 norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
maetugr 0:3e7450f1a938 98 q0 = q0 / norm;
maetugr 0:3e7450f1a938 99 q1 = q1 / norm;
maetugr 0:3e7450f1a938 100 q2 = q2 / norm;
maetugr 0:3e7450f1a938 101 q3 = q3 / norm;
maetugr 0:3e7450f1a938 102 }
maetugr 0:3e7450f1a938 103
maetugr 0:3e7450f1a938 104 // AHRS
maetugr 0:3e7450f1a938 105 void IMU_Filter::AHRSupdate(float halfT, float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
maetugr 0:3e7450f1a938 106 float norm;
maetugr 0:3e7450f1a938 107 float hx, hy, hz, bx, bz;
maetugr 0:3e7450f1a938 108 float vx, vy, vz, wx, wy, wz;
maetugr 0:3e7450f1a938 109 float ex, ey, ez;
maetugr 0:3e7450f1a938 110
maetugr 0:3e7450f1a938 111 // auxiliary variables to reduce number of repeated operations
maetugr 0:3e7450f1a938 112 float q0q0 = q0*q0;
maetugr 0:3e7450f1a938 113 float q0q1 = q0*q1;
maetugr 0:3e7450f1a938 114 float q0q2 = q0*q2;
maetugr 0:3e7450f1a938 115 float q0q3 = q0*q3;
maetugr 0:3e7450f1a938 116 float q1q1 = q1*q1;
maetugr 0:3e7450f1a938 117 float q1q2 = q1*q2;
maetugr 0:3e7450f1a938 118 float q1q3 = q1*q3;
maetugr 0:3e7450f1a938 119 float q2q2 = q2*q2;
maetugr 0:3e7450f1a938 120 float q2q3 = q2*q3;
maetugr 0:3e7450f1a938 121 float q3q3 = q3*q3;
maetugr 0:3e7450f1a938 122
maetugr 0:3e7450f1a938 123 // normalise the measurements
maetugr 0:3e7450f1a938 124 norm = sqrt(ax*ax + ay*ay + az*az);
maetugr 0:3e7450f1a938 125 if(norm == 0.0f) return;
maetugr 0:3e7450f1a938 126 ax = ax / norm;
maetugr 0:3e7450f1a938 127 ay = ay / norm;
maetugr 0:3e7450f1a938 128 az = az / norm;
maetugr 0:3e7450f1a938 129 norm = sqrt(mx*mx + my*my + mz*mz);
maetugr 0:3e7450f1a938 130 if(norm == 0.0f) return;
maetugr 0:3e7450f1a938 131 mx = mx / norm;
maetugr 0:3e7450f1a938 132 my = my / norm;
maetugr 0:3e7450f1a938 133 mz = mz / norm;
maetugr 0:3e7450f1a938 134
maetugr 0:3e7450f1a938 135 // compute reference direction of flux
maetugr 0:3e7450f1a938 136 hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
maetugr 0:3e7450f1a938 137 hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
maetugr 0:3e7450f1a938 138 hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);
maetugr 0:3e7450f1a938 139 bx = sqrt((hx*hx) + (hy*hy));
maetugr 0:3e7450f1a938 140 bz = hz;
maetugr 0:3e7450f1a938 141
maetugr 0:3e7450f1a938 142 // estimated direction of gravity and flux (v and w)
maetugr 0:3e7450f1a938 143 vx = 2*(q1q3 - q0q2);
maetugr 0:3e7450f1a938 144 vy = 2*(q0q1 + q2q3);
maetugr 0:3e7450f1a938 145 vz = q0q0 - q1q1 - q2q2 + q3q3;
maetugr 0:3e7450f1a938 146 wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
maetugr 0:3e7450f1a938 147 wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
maetugr 0:3e7450f1a938 148 wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);
maetugr 0:3e7450f1a938 149
maetugr 0:3e7450f1a938 150 // error is sum of cross product between reference direction of fields and direction measured by sensors
maetugr 0:3e7450f1a938 151 ex = (ay*vz - az*vy) + (my*wz - mz*wy);
maetugr 0:3e7450f1a938 152 ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
maetugr 0:3e7450f1a938 153 ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
maetugr 0:3e7450f1a938 154
maetugr 0:3e7450f1a938 155 // integral error scaled integral gain
maetugr 0:3e7450f1a938 156 exInt = exInt + ex*Ki;
maetugr 0:3e7450f1a938 157 eyInt = eyInt + ey*Ki;
maetugr 0:3e7450f1a938 158 ezInt = ezInt + ez*Ki;
maetugr 0:3e7450f1a938 159
maetugr 0:3e7450f1a938 160 // adjusted gyroscope measurements
maetugr 0:3e7450f1a938 161 gx = gx + Kp*ex + exInt;
maetugr 0:3e7450f1a938 162 gy = gy + Kp*ey + eyInt;
maetugr 0:3e7450f1a938 163 gz = gz + Kp*ez + ezInt;
maetugr 0:3e7450f1a938 164
maetugr 0:3e7450f1a938 165 // integrate quaternion rate and normalise
maetugr 0:3e7450f1a938 166 q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
maetugr 0:3e7450f1a938 167 q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
maetugr 0:3e7450f1a938 168 q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
maetugr 0:3e7450f1a938 169 q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;
maetugr 0:3e7450f1a938 170
maetugr 0:3e7450f1a938 171 // normalise quaternion
maetugr 0:3e7450f1a938 172 norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
maetugr 0:3e7450f1a938 173 q0 = q0 / norm;
maetugr 0:3e7450f1a938 174 q1 = q1 / norm;
maetugr 0:3e7450f1a938 175 q2 = q2 / norm;
maetugr 0:3e7450f1a938 176 q3 = q3 / norm;
maetugr 0:3e7450f1a938 177 }