AHRS based on MatrixPilot DCM algorithm; ported from Pololu MinIMU-9 example code in turn based on ArduPilot 1.5
DCM.cpp
00001 /* 00002 MinIMU-9-mbed-AHRS 00003 Pololu MinIMU-9 + mbed AHRS (Attitude and Heading Reference System) 00004 00005 Modified and ported to mbed environment by Michael Shimniok 00006 http://www.bot-thoughts.com/ 00007 00008 Basedd on MinIMU-9-Arduino-AHRS 00009 Pololu MinIMU-9 + Arduino AHRS (Attitude and Heading Reference System) 00010 00011 Copyright (c) 2011 Pololu Corporation. 00012 http://www.pololu.com/ 00013 00014 MinIMU-9-Arduino-AHRS is based on sf9domahrs by Doug Weibel and Jose Julio: 00015 http://code.google.com/p/sf9domahrs/ 00016 00017 sf9domahrs is based on ArduIMU v1.5 by Jordi Munoz and William Premerlani, Jose 00018 Julio and Doug Weibel: 00019 http://code.google.com/p/ardu-imu/ 00020 00021 MinIMU-9-Arduino-AHRS is free software: you can redistribute it and/or modify it 00022 under the terms of the GNU Lesser General Public License as published by the 00023 Free Software Foundation, either version 3 of the License, or (at your option) 00024 any later version. 00025 00026 MinIMU-9-Arduino-AHRS is distributed in the hope that it will be useful, but 00027 WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 00028 FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for 00029 more details. 00030 00031 You should have received a copy of the GNU Lesser General Public License along 00032 with MinIMU-9-Arduino-AHRS. If not, see <http://www.gnu.org/licenses/>. 00033 00034 */ 00035 #include "DCM.h" 00036 #include "Matrix.h" 00037 #include "math.h" 00038 #include "Sensors.h" 00039 00040 #define MAG_SKIP 2 00041 00042 float DCM::constrain(float x, float a, float b) 00043 { 00044 float result = x; 00045 00046 if (x < a) result = a; 00047 if (x > b) result = b; 00048 00049 return result; 00050 } 00051 00052 00053 DCM::DCM(void): 00054 G_Dt(0.02), update_count(MAG_SKIP) 00055 { 00056 for (int m=0; m < 3; m++) { 00057 Accel_Vector[m] = 0; 00058 Gyro_Vector[m] = 0; 00059 Omega_Vector[m] = 0; 00060 Omega_P[m] = 0; 00061 Omega_I[m] = 0; 00062 Omega[m] = 0; 00063 errorRollPitch[m] = 0; 00064 errorYaw[m] = 0; 00065 for (int n=0; n < 3; n++) { 00066 dcm[m][n] = (m == n) ? 1 : 0; // dcm starts as identity matrix 00067 } 00068 } 00069 00070 } 00071 00072 00073 /**************************************************/ 00074 void DCM::Normalize(void) 00075 { 00076 float error=0; 00077 float temporary[3][3]; 00078 float renorm=0; 00079 00080 error= -Vector_Dot_Product(&dcm[0][0], &dcm[1][0])*.5; //eq.19 00081 00082 Vector_Scale(&temporary[0][0], &dcm[1][0], error); //eq.19 00083 Vector_Scale(&temporary[1][0], &dcm[0][0], error); //eq.19 00084 00085 Vector_Add(&temporary[0][0], &temporary[0][0], &dcm[0][0]);//eq.19 00086 Vector_Add(&temporary[1][0], &temporary[1][0], &dcm[1][0]);//eq.19 00087 00088 Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20 00089 00090 renorm= .5 *(3 - Vector_Dot_Product(&temporary[0][0],&temporary[0][0])); //eq.21 00091 Vector_Scale(&dcm[0][0], &temporary[0][0], renorm); 00092 00093 renorm= .5 *(3 - Vector_Dot_Product(&temporary[1][0],&temporary[1][0])); //eq.21 00094 Vector_Scale(&dcm[1][0], &temporary[1][0], renorm); 00095 00096 renorm= .5 *(3 - Vector_Dot_Product(&temporary[2][0],&temporary[2][0])); //eq.21 00097 Vector_Scale(&dcm[2][0], &temporary[2][0], renorm); 00098 } 00099 00100 /**************************************************/ 00101 void DCM::Drift_correction(void) 00102 { 00103 float mag_heading_x; 00104 float mag_heading_y; 00105 float errorCourse; 00106 //Compensation the Roll, Pitch and Yaw drift. 00107 static float Scaled_Omega_P[3]; 00108 static float Scaled_Omega_I[3]; 00109 float Accel_magnitude; 00110 float Accel_weight; 00111 00112 00113 //*****Roll and Pitch*************** 00114 00115 // Calculate the magnitude of the accelerometer vector 00116 Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]); 00117 Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity. 00118 // Dynamic weighting of accelerometer info (reliability filter) 00119 // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0) 00120 Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1); // 00121 00122 Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&dcm[2][0]); //adjust the ground of reference 00123 Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight); 00124 00125 Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight); 00126 Vector_Add(Omega_I,Omega_I,Scaled_Omega_I); 00127 00128 //*****YAW*************** 00129 // We make the gyro YAW drift correction based on compass magnetic heading 00130 00131 /* http://tinyurl.com/7bul438 00132 * William Premerlani: 00133 * 1. If you are treating the magnetometer as a tilt compensated compass, it will not work for pitch values near 90 degrees. 00134 * A better way to do it is to use the magnetometer measurement as a reference vector instead. Use the direction cosine 00135 * matrix to transform the magnetometer vector from body frame to earth frame, which works in any orientation, even with 00136 * 90 degree pitch. Then, extract the horizontal component of the magnetometer earth frame vector, and take the cross 00137 * product of it with the known horizontal component of the earth's magnetic field. The result is a rotation error vector 00138 * that you transform back into the body frame, and use it to compensate for gyro drift. That is technique we are using in 00139 * MatrixPilot, it works for any orientation. A combination of two reference vectors (magnetometer and accelerometer) will 00140 * provide a 3 axis lock. 00141 * 2. If you are using Euler angles to represent orientation, they do not work for 90 degree pitch. There is an effect known 00142 * as "gimbal lock" that throws off the roll. It is better to use either an angle and rotation axis representation, or 00143 * quaternions. 00144 * 00145 * ummm... we're no actually calculating MAG_Heading anywhere... so it's zero... 00146 * mag_earth[3x1] = mag[3x1] dot dcm[3x3] 00147 * earth_rotation_error_vector = mag_earth[x and y] cross known_earth_mag[??] 00148 * gyro drift error aka body_rotation_error_vector = earth_rotation_error_Vector times dcm[3x3] 00149 float mag_earth[3], mag_sensor[3]; 00150 mag_sensor[0] = magnetom_x; 00151 mag_sensor[1] = magnetom_y; 00152 mag_sensor[2] = magnetom_z; 00153 mag_earth[0] = VectorDotProduct( &dcm[0] , mag_sensor ) << 1; 00154 mag_earth[1] = VectorDotProduct( &dcm[1] , mag_sensor ) << 1; 00155 mag_earth[2] = VectorDotProduct( &dcm[2] , mag_sensor ) << 1; 00156 mag_error = 100 * VectorDotProduct( 2 , mag_earth , declinationVector ) ; // Dotgain = 1/2 00157 VectorScale( 3 , errorYawplane , &rmat[6] , mag_error ) ; // Scalegain = 1/2 00158 */ 00159 00160 mag_heading_x = cos(MAG_Heading); 00161 mag_heading_y = sin(MAG_Heading); 00162 errorCourse=(dcm[0][0]*mag_heading_y) - (dcm[1][0]*mag_heading_x); //Calculating YAW error 00163 Vector_Scale(errorYaw,&dcm[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position. 00164 00165 Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);//.01proportional of YAW. 00166 Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional. 00167 00168 Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);//.00001Integrator 00169 Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I 00170 } 00171 00172 /**************************************************/ 00173 void DCM::Accel_adjust(void) 00174 { 00175 Accel_Vector[1] += Accel_Scale(speed_3d*Omega[2]); // Centrifugal force on Acc_y = GPS_speed*GyroZ 00176 Accel_Vector[2] -= Accel_Scale(speed_3d*Omega[1]); // Centrifugal force on Acc_z = GPS_speed*GyroY 00177 // Add linear (x-axis) acceleration correction here 00178 00179 // from MatrixPilot 00180 // total (3D) airspeed in cm/sec is used to adjust for acceleration 00181 //gplane[0]=gplane[0]- omegaSOG( omegaAccum[2] , air_speed_3DGPS ) ; 00182 //gplane[2]=gplane[2]+ omegaSOG( omegaAccum[0] , air_speed_3DGPS ) ; 00183 //gplane[1]=gplane[1]+ ((unsigned int)(ACCELSCALE))*forward_acceleration 00184 } 00185 00186 /**************************************************/ 00187 void DCM::Matrix_update(void) 00188 { 00189 // TODO: Hardware-specific routines to convert gyro to units; this (probably) should be handled elsewhere 00190 00191 Gyro_Vector[0]=Gyro_Scaled_X(gyro_x); //gyro x roll 00192 Gyro_Vector[1]=Gyro_Scaled_Y(gyro_y); //gyro y pitch 00193 Gyro_Vector[2]=Gyro_Scaled_Z(gyro_z); //gyro Z yaw 00194 00195 // Why aren't we scaling accelerometer? I think the DCM paper talks a little about this... ?? 00196 Accel_Vector[0]=accel_x; 00197 Accel_Vector[1]=accel_y; 00198 Accel_Vector[2]=accel_z; 00199 00200 Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]); //adding proportional term 00201 Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term 00202 00203 // Remove centrifugal & linear acceleration. 00204 Accel_adjust(); 00205 00206 #if OUTPUTMODE==1 00207 Update_Matrix[0][0]=0; 00208 Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z 00209 Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y 00210 Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z 00211 Update_Matrix[1][1]=0; 00212 Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x 00213 Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y 00214 Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x 00215 Update_Matrix[2][2]=0; 00216 #else // Uncorrected data (no drift correction) 00217 Update_Matrix[0][0]=0; 00218 Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z 00219 Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y 00220 Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z 00221 Update_Matrix[1][1]=0; 00222 Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0]; 00223 Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1]; 00224 Update_Matrix[2][1]=G_Dt*Gyro_Vector[0]; 00225 Update_Matrix[2][2]=0; 00226 #endif 00227 00228 Matrix_Multiply(Temporary_Matrix, dcm, Update_Matrix); //c=a*b 00229 00230 // ??? Matrix_Add(dcm, dcm, Temporary_Matrix); ??? 00231 for(int x=0; x<3; x++) { //Matrix Addition (update) 00232 for(int y=0; y<3; y++) { 00233 dcm[x][y] += Temporary_Matrix[x][y]; 00234 } 00235 } 00236 } 00237 00238 void DCM::Euler_angles(void) 00239 { 00240 pitch = -asin(dcm[2][0]); 00241 roll = atan2(dcm[2][1],dcm[2][2]); 00242 yaw = atan2(dcm[1][0],dcm[0][0]); 00243 } 00244 00245 void DCM::Update_step() 00246 { 00247 Read_Gyro(); 00248 Read_Accel(); 00249 if (--update_count == 0) { 00250 Update_mag(); 00251 update_count = MAG_SKIP; 00252 } 00253 Matrix_update(); 00254 Normalize(); 00255 Drift_correction(); 00256 //Accel_adjust(); 00257 Euler_angles(); 00258 } 00259 00260 void DCM::Update_mag() 00261 { 00262 Read_Compass(); 00263 Compass_Heading(); 00264 }
Generated on Sat Jul 16 2022 20:25:06 by 1.7.2