IMU_Filter.cpp

00001 #include "IMU_Filter.h"
00002 
00003 // IMU/AHRS
00004 #define PI 3.1415926535897932384626433832795
00005 #define Kp 0.5f         // proportional gain governs rate of convergence to accelerometer/magnetometer
00006 #define Ki 0.0f//0.005f       // integral gain governs rate of convergence of gyroscope biases
00007 
00008 IMU_Filter::IMU_Filter()
00009 {
00010     for(int i=0; i<3; i++)
00011         angle[i]=0;
00012     
00013     // IMU/AHRS
00014     q0 = 1; q1 = 0; q2 = 0; q3 = 0;
00015     exInt = 0; eyInt = 0; ezInt = 0;
00016 }
00017 
00018 void IMU_Filter::compute(float dt, const float * Gyro_data, const float * Acc_data, const float * Comp_data)
00019 {
00020     #if 0 // all gyro only
00021         for(int i = 0; i < 3; i++) {
00022             d_Gyro_angle[i] = Gyro_data[i] *dt;
00023             angle[i] += d_Gyro_angle[i];
00024         }
00025     #endif
00026     
00027     #if 1 // IMU/AHRS (from http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/)   
00028         float radGyro[3]; // Gyro in radians per second
00029         for(int i=0; i<3; i++)
00030             radGyro[i] = Gyro_data[i] * PI / 180;
00031         
00032         IMUupdate(dt/2, radGyro[0], radGyro[1], radGyro[2], Acc_data[0], Acc_data[1], Acc_data[2]);
00033         //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]);
00034         
00035         float rangle[3]; // calculate angles in radians from quternion output, formula from Wiki (http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles)
00036         
00037         rangle[0] = atan2(2*q0*q1 + 2*q2*q3, 1 - 2*(q1*q1 + q2*q2));
00038         rangle[1] = asin (2*q0*q2 - 2*q3*q1);
00039         rangle[2] = atan2(2*q0*q3 + 2*q1*q2, 1 - 2*(q2*q2 + q3*q3));
00040         
00041         // TODO
00042         // Pitch should have a range of +/-90 degrees. 
00043         // After you pitch past vertical (90 degrees) your roll and yaw value should swing 180 degrees. 
00044         // A pitch value of 100 degrees is measured as a pitch of 80 degrees and inverted flight (roll = 180 degrees). 
00045         // 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.
00046         // And I think this solves the upside down issue...
00047         // Handle roll reversal when inverted
00048         /*if (Acc_data[2] < 0) {
00049             if (Acc_data[0] < 0) {
00050                 rangle[1] = (180 - rangle[1]);
00051             } else {
00052                 rangle[1] = (-180 - rangle[1]);
00053             }
00054         }*/
00055         
00056         for(int i=0; i<2; i++)  // angle in degree
00057             angle[i] = rangle[i] * 180 / PI;
00058     #endif
00059     
00060     angle[2] += Gyro_data[2] *dt; // ATTENTION YAW IS GYRO ONLY
00061 }
00062 
00063 //------------------------------------------------------------------------------------------------------------------
00064 // Code copied from S.O.H. Madgwick (File IMU.c and AHRS.c from https://code.google.com/p/imumargalgorithm30042010sohm/)
00065 //------------------------------------------------------------------------------------------------------------------
00066 
00067 // IMU
00068 void IMU_Filter::IMUupdate(float halfT, float gx, float gy, float gz, float ax, float ay, float az) {
00069     float norm;
00070     float vx, vy, vz;
00071     float ex, ey, ez;         
00072     
00073     // normalise the measurements
00074     norm = sqrt(ax*ax + ay*ay + az*az);
00075     if(norm == 0.0f) return;   
00076     ax /= norm;
00077     ay /= norm;
00078     az /= norm;      
00079     
00080     // estimated direction of gravity
00081     vx = 2*(q1*q3 - q0*q2);
00082     vy = 2*(q0*q1 + q2*q3);
00083     vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
00084     
00085     // error is sum of cross product between reference direction of field and direction measured by sensor
00086     ex = (ay*vz - az*vy);
00087     ey = (az*vx - ax*vz);
00088     ez = (ax*vy - ay*vx);
00089     
00090     // integral error scaled integral gain
00091     exInt += ex*Ki;
00092     eyInt += ey*Ki;
00093     ezInt += ez*Ki;
00094     
00095     // adjusted gyroscope measurements
00096     gx += Kp*ex + exInt;
00097     gy += Kp*ey + eyInt;
00098     gz += Kp*ez + ezInt;
00099     
00100     // integrate quaternion rate and normalise
00101     float q0o = q0; // he did the MATLAB to C error by not thinking of the beginning vector elements already being changed for the calculation of the rest!
00102     float q1o = q1;
00103     float q2o = q2;
00104     float q3o = q3;
00105     q0 += (-q1o*gx - q2o*gy - q3o*gz)*halfT;
00106     q1 += (q0o*gx + q2o*gz - q3o*gy)*halfT;
00107     q2 += (q0o*gy - q1o*gz + q3o*gx)*halfT;
00108     q3 += (q0o*gz + q1o*gy - q2o*gx)*halfT;  
00109     
00110     // normalise quaternion
00111     norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
00112     q0 = q0 / norm;
00113     q1 = q1 / norm;
00114     q2 = q2 / norm;
00115     q3 = q3 / norm;
00116 }
00117 
00118 // AHRS
00119 void IMU_Filter::AHRSupdate(float halfT, float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
00120     float norm;
00121     float hx, hy, hz, bx, bz;
00122     float vx, vy, vz, wx, wy, wz;
00123     float ex, ey, ez;
00124 
00125     // auxiliary variables to reduce number of repeated operations
00126     float q0q0 = q0*q0;
00127     float q0q1 = q0*q1;
00128     float q0q2 = q0*q2;
00129     float q0q3 = q0*q3;
00130     float q1q1 = q1*q1;
00131     float q1q2 = q1*q2;
00132     float q1q3 = q1*q3;
00133     float q2q2 = q2*q2;   
00134     float q2q3 = q2*q3;
00135     float q3q3 = q3*q3;          
00136     
00137     // normalise the measurements
00138     norm = sqrt(ax*ax + ay*ay + az*az);
00139     if(norm == 0.0f) return;
00140     ax = ax / norm;
00141     ay = ay / norm;
00142     az = az / norm;
00143     norm = sqrt(mx*mx + my*my + mz*mz);
00144     if(norm == 0.0f) return;
00145     mx = mx / norm;
00146     my = my / norm;
00147     mz = mz / norm;         
00148     
00149     // compute reference direction of flux
00150     hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
00151     hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
00152     hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);         
00153     bx = sqrt((hx*hx) + (hy*hy));
00154     bz = hz;        
00155     
00156     // estimated direction of gravity and flux (v and w)
00157     vx = 2*(q1q3 - q0q2);
00158     vy = 2*(q0q1 + q2q3);
00159     vz = q0q0 - q1q1 - q2q2 + q3q3;
00160     wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
00161     wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
00162     wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);  
00163     
00164     // error is sum of cross product between reference direction of fields and direction measured by sensors
00165     ex = (ay*vz - az*vy) + (my*wz - mz*wy);
00166     ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
00167     ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
00168     
00169     // integral error scaled integral gain
00170     exInt = exInt + ex*Ki;
00171     eyInt = eyInt + ey*Ki;
00172     ezInt = ezInt + ez*Ki;
00173     
00174     // adjusted gyroscope measurements
00175     gx = gx + Kp*ex + exInt;
00176     gy = gy + Kp*ey + eyInt;
00177     gz = gz + Kp*ez + ezInt;
00178     
00179     // integrate quaternion rate and normalise
00180     q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
00181     q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
00182     q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
00183     q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;  
00184     
00185     // normalise quaternion
00186     norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
00187     q0 = q0 / norm;
00188     q1 = q1 / norm;
00189     q2 = q2 / norm;
00190     q3 = q3 / norm;
00191 }