IMU_Filter.cpp

00001 #include "IMU_Filter.h"
00002 
00003 // MARG
00004 #define PI 3.1415926535897932384626433832795
00005 #define Kp 2.0f         // proportional gain governs rate of convergence to accelerometer/magnetometer
00006 #define Ki 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     // MARG
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)
00019 {
00020     // calculate angles for each sensor
00021     for(int i = 0; i < 3; i++)
00022         d_Gyro_angle[i] = Gyro_data[i] *dt;
00023     get_Acc_angle(Acc_data);
00024     
00025     // Complementary Filter
00026     #if 0 // (formula from http://diydrones.com/m/discussion?id=705844%3ATopic%3A669858)
00027         angle[0] = (0.999*(angle[0] + d_Gyro_angle[0]))+(0.001*(Acc_angle[0]));
00028         angle[1] = (0.999*(angle[1] + d_Gyro_angle[1]))+(0.001*(Acc_angle[1]));// + 3)); // TODO Offset accelerometer einstellen
00029         angle[2] += d_Gyro_angle[2]; // gyro only here TODO: Compass + 3D
00030     #endif
00031     
00032     #if 0 // alte berechnung, vielleicht Accelerometer zu stark gewichtet
00033         angle[0] += (Acc.angle[0] - angle[0])/50 + d_Gyro_angle[0];
00034         angle[1] += (Acc.angle[1] - angle[1])/50 + d_Gyro_angle[1];// TODO Offset accelerometer einstellen
00035         //tempangle += (Comp.get_angle() - tempangle)/50 + Gyro.data[2] *dt/15000000.0;
00036         angle[2] = Gyro_angle[2]; // gyro only here
00037     #endif
00038     
00039     #if 0 // neuer Test 2 (funktioniert wahrscheinlich nicht, denkfehler)
00040         angle[0] += Gyro_angle[0] * 0.98 + Acc.angle[0] * 0.02;
00041         angle[1] += Gyro_angle[1] * 0.98 + (Acc.angle[1] + 3) * 0.02; // TODO: Calibrierung Acc
00042         angle[2] = Gyro_angle[2]; // gyro only here
00043     #endif
00044     
00045     #if 0 // all gyro only
00046         for(int i = 0; i < 3; i++)
00047             angle[i] += d_Gyro_angle[i];
00048     #endif
00049     
00050     // MARG
00051     #if 1 // (from http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/)
00052         float radGyro[3];
00053         
00054         for(int i=0; i<3; i++)  // Radians per second
00055             radGyro[i] = Gyro_data[i] * PI / 180;
00056         
00057         IMUupdate(dt/2, radGyro[0], radGyro[1], radGyro[2], Acc_data[0], Acc_data[1], Acc_data[2]);
00058         
00059         float rangle[3]; // calculate angles in radians from quternion output
00060         rangle[0] = atan2(2*q0*q1 + 2*q2*q3, 1 - 2*(q1*q1 + q2*q2)); // from Wiki
00061         rangle[1] = asin(2*q0*q2 - 2*q3*q1);
00062         rangle[2] = atan2(2*q0*q3 + 2*q1*q2, 1 - 2*(q2*q2 + q3*q3));
00063         
00064         // TODO
00065         // Pitch should have a range of +/-90 degrees. 
00066         // After you pitch past vertical (90 degrees) your roll and yaw value should swing 180 degrees. 
00067         // A pitch value of 100 degrees is measured as a pitch of 80 degrees and inverted flight (roll = 180 degrees). 
00068         // 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.
00069         // And I think this solves the upside down issue...
00070         // Handle roll reversal when inverted
00071         /*if (Acc_data[2] < 0) {
00072             if (Acc_data[0] < 0) {
00073                 rangle[1] = (180 - rangle[1]);
00074             } else {
00075                 rangle[1] = (-180 - rangle[1]);
00076             }
00077         }*/
00078         
00079         for(int i=0; i<3; i++)  // angle in degree
00080             angle[i] = rangle[i] * 180 / PI;
00081     #endif 
00082 }
00083 
00084 void IMU_Filter::get_Acc_angle(const float * Acc_data)
00085 {
00086     // calculate the angles for roll and pitch (0,1)
00087     float R = sqrt(pow((float)Acc_data[0],2) + pow((float)Acc_data[1],2) + pow((float)Acc_data[2],2));
00088     float temp[3];
00089     
00090     temp[0] = -(Rad2Deg * acos(Acc_data[1] / R)-90);
00091     temp[1] =   Rad2Deg * acos(Acc_data[0] / R)-90;
00092     temp[2] =   Rad2Deg * acos(Acc_data[2] / R);
00093     
00094     for(int i = 0;i < 3; i++)
00095         if (temp[i] > -360 && temp[i] < 360)
00096             Acc_angle[i] = temp[i];
00097 }
00098 
00099 // MARG
00100 void IMU_Filter::IMUupdate(float halfT, float gx, float gy, float gz, float ax, float ay, float az)
00101 {
00102     float norm;
00103     float vx, vy, vz;
00104     float ex, ey, ez;         
00105     
00106     // normalise the measurements
00107     norm = sqrt(ax*ax + ay*ay + az*az);
00108     if(norm == 0.0f) return;   
00109     ax = ax / norm;
00110     ay = ay / norm;
00111     az = az / norm;      
00112     
00113     // estimated direction of gravity
00114     vx = 2*(q1*q3 - q0*q2);
00115     vy = 2*(q0*q1 + q2*q3);
00116     vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
00117     
00118     // error is sum of cross product between reference direction of field and direction measured by sensor
00119     ex = (ay*vz - az*vy);
00120     ey = (az*vx - ax*vz);
00121     ez = (ax*vy - ay*vx);
00122     
00123     // integral error scaled integral gain
00124     exInt = exInt + ex*Ki;
00125     eyInt = eyInt + ey*Ki;
00126     ezInt = ezInt + ez*Ki;
00127     
00128     // adjusted gyroscope measurements
00129     gx = gx + Kp*ex + exInt;
00130     gy = gy + Kp*ey + eyInt;
00131     gz = gz + Kp*ez + ezInt;
00132     
00133     // integrate quaternion rate and normalise
00134     q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
00135     q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
00136     q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
00137     q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;  
00138     
00139     // normalise quaternion
00140     norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
00141     q0 = q0 / norm;
00142     q1 = q1 / norm;
00143     q2 = q2 / norm;
00144     q3 = q3 / norm;
00145 }