Alberto Petrucci
Simple example to show how to get an estimation of the attitude with a 9DOF IMU and the Kalman filter
Dependencies: L3GD20 LSM303DLHC mbed-dsp mbed
Fork of
minimu-9v2
by 
brian claus
Embed:
(wiki syntax)
Show/hide line numbers
sensfusion9.c
00001 #include "sensfusion9.h" 00002 #include "math.h" 00003 #include "arm_math.h" 00004 00005 #define M_PI_F 3.14159265358979323846f /* pi */ 00006 00007 static inline void mat_trans(const arm_matrix_instance_f32 * pSrc, arm_matrix_instance_f32 * pDst) 00008 { 00009 arm_mat_trans_f32(pSrc, pDst); 00010 } 00011 static inline void mat_inv(const arm_matrix_instance_f32 * pSrc, arm_matrix_instance_f32 * pDst) 00012 { 00013 arm_mat_inverse_f32(pSrc, pDst); 00014 } 00015 static inline void mat_mult(const arm_matrix_instance_f32 * pSrcA, const arm_matrix_instance_f32 * pSrcB, arm_matrix_instance_f32 * pDst) 00016 { 00017 arm_mat_mult_f32(pSrcA, pSrcB, pDst); 00018 } 00019 static inline float arm_sqrt(float32_t in) 00020 { 00021 float pOut = 0; 00022 arm_status result = arm_sqrt_f32(in, &pOut); 00023 return pOut; 00024 } 00025 00026 #define STATE_SIZE 4 00027 #define OUTPUT_SIZE 6 00028 static float Q_VARIANCE = 0.01f; 00029 static float R_VARIANCE_ACC = 25.0f; 00030 static float R_VARIANCE_MAG = 50.0f; 00031 00032 // The covariance matrix 00033 static float P[STATE_SIZE][STATE_SIZE] = {{0}}; 00034 static float R[OUTPUT_SIZE] = {0}; 00035 static arm_matrix_instance_f32 Pm = {STATE_SIZE, STATE_SIZE, (float *)P}; 00036 00037 // The state update matrix 00038 static float A[STATE_SIZE][STATE_SIZE]; 00039 static arm_matrix_instance_f32 Am = {STATE_SIZE, STATE_SIZE, (float *)A}; // linearized dynamics for covariance update; 00040 00041 // Temporary matrices for the covariance updates 00042 static float tmpNN1d[STATE_SIZE][STATE_SIZE]; 00043 static arm_matrix_instance_f32 tmpNN1m = { STATE_SIZE, STATE_SIZE, (float *)tmpNN1d}; 00044 00045 static float tmpNN2d[STATE_SIZE][STATE_SIZE]; 00046 static arm_matrix_instance_f32 tmpNN2m = { STATE_SIZE, STATE_SIZE, (float *)tmpNN2d}; 00047 00048 // quaternion of sensor frame relative to auxiliary frame 00049 static float q0 = 1.0f; 00050 static float q1 = 0.0f; 00051 static float q2 = 0.0f; 00052 static float q3 = 0.0f; 00053 00054 // Unit vector in the estimated gravity direction 00055 static float gravX, gravY, gravZ; 00056 00057 // The acc in Z for static position (g) 00058 // Set on first update, assuming we are in a static position since the sensors were just calibrates. 00059 // This value will be better the more level the copter is at calibration time 00060 static float baseZacc = 1.0; 00061 00062 static bool isInit; 00063 00064 static bool isCalibrated = false; 00065 00066 static void sensfusion9UpdateQImpl(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float dt); 00067 static float sensfusion9GetAccZ(const float ax, const float ay, const float az); 00068 static void estimatedGravityDirection(float* gx, float* gy, float* gz); 00069 00070 // TODO: Make math util file 00071 static float invSqrt(float x); 00072 00073 void sensfusion9Init() 00074 { 00075 if(isInit) 00076 return; 00077 for(int i = 0; i < STATE_SIZE; ++i) 00078 P[i][i] = Q_VARIANCE; 00079 R[0] = R_VARIANCE_ACC; 00080 R[1] = R_VARIANCE_ACC; 00081 R[2] = R_VARIANCE_ACC; 00082 R[3] = R_VARIANCE_MAG; 00083 R[4] = R_VARIANCE_MAG; 00084 R[5] = R_VARIANCE_MAG; 00085 isInit = true; 00086 } 00087 00088 bool sensfusion9Test(void) 00089 { 00090 return isInit; 00091 } 00092 00093 void sensfusion9UpdateQ(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float dt) 00094 { 00095 //Cambio gli assi del magnetometro perchè rispetto a quelli del gyro/acc sono diversi: 00096 //In particolare: 00097 //Mx = (Gy = Ay) 00098 //My = (Gx = Ax) 00099 //-Mz = (Gz = Az) 00100 sensfusion9UpdateQImpl(gx, gy, gz, ax, ay, az, mx, my, mz, dt); 00101 estimatedGravityDirection(&gravX, &gravY, &gravZ); 00102 if (!isCalibrated) { 00103 baseZacc = sensfusion9GetAccZ(ax, ay, az); 00104 isCalibrated = true; 00105 } 00106 } 00107 00108 /* 00109 static void sensfusion9UpdateQImpl(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float dt) 00110 { 00111 float recipNorm; 00112 00113 gx = gx * M_PI_F / 180.0f; 00114 gy = gy * M_PI_F / 180.0f; 00115 gz = gz * M_PI_F / 180.0f; 00116 00117 // Normalise accelerometer measurement 00118 recipNorm = invSqrt(ax*ax + ay*ay + az*az); 00119 ax *= recipNorm; 00120 ay *= recipNorm; 00121 az *= recipNorm; 00122 00123 // Normalise magnetometer measurement 00124 recipNorm = invSqrt(mx*mx + my*my + mz*mz); 00125 mx *= recipNorm; 00126 my *= recipNorm; 00127 mz *= recipNorm; 00128 00129 A[0][0] = 1.0f; 00130 A[0][1] = gz*dt; 00131 A[0][2] = -gy*dt; 00132 A[1][0] = -gz*dt; 00133 A[1][1] = 1.0f; 00134 A[1][2] = gx*dt; 00135 A[2][0] = gy*dt; 00136 A[2][1] = -gx*dt; 00137 A[2][2] = 1.0f; 00138 00139 float q[4][1]; 00140 arm_matrix_instance_f32 qm = {4, 1, (float *)q}; 00141 q[0][0] = q0; 00142 q[1][0] = q1; 00143 q[2][0] = q2; 00144 q[3][0] = q3; 00145 00146 float W[4][4]; 00147 arm_matrix_instance_f32 Wm = {4, 4, (float *)W}; 00148 W[0][0] = 0.0f; 00149 W[0][1] = -gx; 00150 W[0][2] = -gy; 00151 W[0][3] = -gz; 00152 W[1][0] = gx; 00153 W[1][1] = 0.0f; 00154 W[1][2] = gz; 00155 W[1][3] = -gy; 00156 W[2][0] = gy; 00157 W[2][1] = -gz; 00158 W[2][2] = 0.0f; 00159 W[2][3] = gx; 00160 W[3][0] = gz; 00161 W[3][1] = gy; 00162 W[3][2] = -gx; 00163 W[3][3] = 0.0f; 00164 00165 //Aggiorno lo stato con runge kutta 4 00166 float m1[4][1]; 00167 float m2[4][1]; 00168 float m3[4][1]; 00169 float x1[4][1]; 00170 float x2[4][1]; 00171 arm_matrix_instance_f32 m1m = {4, 1, (float *)m1}; 00172 arm_matrix_instance_f32 m2m = {4, 1, (float *)m2}; 00173 arm_matrix_instance_f32 m3m = {4, 1, (float *)m3}; 00174 arm_matrix_instance_f32 x1m = {4, 1, (float *)x1}; 00175 arm_matrix_instance_f32 x2m = {4, 1, (float *)x2}; 00176 00177 //Primo step 00178 mat_mult(&Wm, &qm, &m1m); 00179 for(int i = 0; i < 4; ++i){ 00180 x1[i][0] = q[i][0] + 0.5f*m1[i][0]*dt; 00181 } 00182 00183 //Secondo step 00184 mat_mult(&Wm, &x1m, &m2m); 00185 for(int i = 0; i < 4; ++i){ 00186 x2[i][0] = q[i][0] + 0.5f*(m1[i][0] + m2[i][0])*dt/4.0f; 00187 } 00188 00189 //Terzo step 00190 mat_mult(&Wm, &x2m, &m3m); 00191 for(int i = 0; i < 4; ++i){ 00192 q[i][0] = q[i][0] + 0.5f*(m1[i][0] + m2[i][0] + 4.0f*m3[i][0])*(dt/6.0f); 00193 } 00194 00195 //Normalizzo la stima dei quaternioni 00196 recipNorm = invSqrt(q[0][0]*q[0][0] + q[1][0]*q[1][0] + q[2][0]*q[2][0] + q[3][0]*q[3][0]); 00197 for(int i = 0; i < 4; ++i){ 00198 q[i][0] *= recipNorm; 00199 } 00200 00201 q0 = q[0][0]; 00202 q1 = q[1][0]; 00203 q2 = q[2][0]; 00204 q3 = q[3][0]; 00205 00206 //Matrice di rotazione 00207 float QuaternionDCM[3][3]; 00208 arm_matrix_instance_f32 QuaternionDCMm = {3, 3, (float *)QuaternionDCM}; 00209 QuaternionDCM[0][0] = 2*q0*q0-1+2*q1*q1; 00210 QuaternionDCM[0][1] = 2*(q1*q2+q0*q3); 00211 QuaternionDCM[0][2] = 2*(q1*q3-q0*q2); 00212 QuaternionDCM[1][0] = 2*(q1*q2-q0*q3); 00213 QuaternionDCM[1][1] = 2*q0*q0-1+2*q2*q2; 00214 QuaternionDCM[1][2] = 2*(q2*q3+q0*q1); 00215 QuaternionDCM[2][0] = 2*(q1*q3+q0*q2); 00216 QuaternionDCM[2][1] = 2*(q2*q3-q0*q1); 00217 QuaternionDCM[2][2] = 2*q0*q0-1+2*q3*q3; 00218 00219 //Gravity Reference 00220 float G[3][1]; 00221 float Gr[3][1]; 00222 arm_matrix_instance_f32 Gm = {3, 1, (float *)G}; 00223 arm_matrix_instance_f32 Grm = {3, 1, (float *)Gr}; 00224 G[0][0] = 0; 00225 G[1][0] = 0; 00226 G[2][0] = 1; 00227 mat_mult(&QuaternionDCMm, &Gm, &Grm); 00228 00229 //Magnetic field Reference 00230 float M[3][1]; 00231 float Mr[3][1]; 00232 arm_matrix_instance_f32 Mm = {3, 1, (float *)M}; 00233 arm_matrix_instance_f32 Mrm = {3, 1, (float *)Mr}; 00234 float hx = mx*q0*q0 + 2.0f*mz*q0*q2 - 2.0f*my*q0*q3 + mx*q1*q1 + 2.0f*my*q1*q2 + 2.0f*mz*q1*q3 - mx*q2*q2 - mx*q3*q3; 00235 float hy = my*q0*q0 - 2.0f*mz*q0*q1 + 2.0f*mx*q0*q3 - my*q1*q1 + 2.0f*mx*q1*q2 + my*q2*q2 + 2.0f*mz*q2*q3 - my*q3*q3; 00236 float hz = mz*q0*q0 + 2.0f*my*q0*q1 - 2.0f*mx*q0*q2 - mz*q1*q1 + 2.0f*mx*q1*q3 - mz*q2*q1 + 2.0f*my*q2*q3 + mz*q3*q3; 00237 M[0][0] = arm_sqrt(hx*hx + hy*hy); 00238 M[1][0] = 0; 00239 M[2][0] = hz; 00240 mat_mult(&QuaternionDCMm, &Mm, &Mrm); 00241 00242 // Aggiorno la Covarianza 00243 float P0[STATE_SIZE][STATE_SIZE]; 00244 arm_matrix_instance_f32 P0m = {STATE_SIZE, STATE_SIZE, (float *)P0}; 00245 00246 mat_mult(&Am, &Pm, &tmpNN1m); // A P 00247 mat_trans(&Am, &tmpNN2m); // A' 00248 mat_mult(&tmpNN1m, &tmpNN2m, &P0m); // A P A' 00249 for(int i = 0; i < STATE_SIZE; ++i){ 00250 P0[i][i] += Q_VARIANCE; 00251 } 00252 00253 // Calcolo il guadagno 00254 float K[STATE_SIZE][OUTPUT_SIZE]; 00255 arm_matrix_instance_f32 Km = {STATE_SIZE, OUTPUT_SIZE, (float *)K}; 00256 00257 float h[OUTPUT_SIZE][STATE_SIZE]; 00258 arm_matrix_instance_f32 Hm = {OUTPUT_SIZE, STATE_SIZE, (float *)h}; 00259 00260 float hT[STATE_SIZE][OUTPUT_SIZE]; 00261 arm_matrix_instance_f32 HTm = {STATE_SIZE, OUTPUT_SIZE, (float *)hT}; 00262 00263 float P0hT[STATE_SIZE][OUTPUT_SIZE]; 00264 arm_matrix_instance_f32 P0HTm = {STATE_SIZE, OUTPUT_SIZE, (float *)P0hT}; 00265 00266 float hP0hT[OUTPUT_SIZE][OUTPUT_SIZE]; 00267 arm_matrix_instance_f32 HP0HTm = {OUTPUT_SIZE, OUTPUT_SIZE, (float *)hP0hT}; 00268 00269 float hP0hT_inv[OUTPUT_SIZE][OUTPUT_SIZE]; 00270 arm_matrix_instance_f32 HP0HT_INVm = {OUTPUT_SIZE, OUTPUT_SIZE, (float *)hP0hT_inv}; 00271 00272 //Accelerometer 00273 h[0][0] = 0; 00274 h[0][1] = -Gr[2][0]; 00275 h[0][2] = Gr[1][0]; 00276 h[1][0] = Gr[2][0]; 00277 h[1][1] = 0; 00278 h[1][2] = -Gr[0][0]; 00279 h[2][0] = -Gr[1][0]; 00280 h[2][1] = Gr[0][0]; 00281 h[2][2] = 0; 00282 00283 //Magnetometer 00284 h[3][0] = 0; 00285 h[3][1] = -Mr[2][0]; 00286 h[3][2] = Mr[1][0]; 00287 h[4][0] = Mr[2][0]; 00288 h[4][1] = 0; 00289 h[4][2] = -Mr[0][0]; 00290 h[5][0] = -Mr[1][0]; 00291 h[5][1] = Mr[0][0]; 00292 h[5][2] = 0; 00293 00294 00295 // ====== INNOVATION COVARIANCE ====== 00296 mat_trans(&Hm, &HTm); 00297 mat_mult(&P0m, &HTm, &P0HTm); // P0*H' 00298 mat_mult(&Hm, &P0HTm, &HP0HTm); // H*P0*H' 00299 for (int i = 0; i < OUTPUT_SIZE; ++i) { // Add the element of HPH' to the above 00300 hP0hT[i][i] += R[i]; 00301 } 00302 mat_inv(&HP0HTm, &HP0HT_INVm); // (H*P0*H' + R)^(-1) 00303 mat_mult(&P0HTm, &HP0HT_INVm, &Km); // K = P0*H'*(H*P0*H' + R)^(-1) 00304 00305 //Aggiorno la P 00306 mat_mult(&Km, &Hm, &tmpNN1m); // KH 00307 for (int i = 0; i < STATE_SIZE; ++i) { 00308 tmpNN1d[i][i] -= 1.0f; 00309 for (int j = 0; j < STATE_SIZE; ++j) { 00310 tmpNN1d[i][j] *= -1.0f; 00311 } 00312 } // -(KH - I) 00313 mat_mult(&tmpNN1m, &P0m, &Pm); // -(KH - I)*P0 00314 00315 float Error[OUTPUT_SIZE][1]; 00316 arm_matrix_instance_f32 Errorm = {STATE_SIZE, 1, (float *)Error}; 00317 float ae[STATE_SIZE][1]; 00318 arm_matrix_instance_f32 aem = {STATE_SIZE, 1, (float *)ae}; 00319 Error[0][0] = ax - Gr[0][0]; 00320 Error[1][0] = ay - Gr[1][0]; 00321 Error[2][0] = az - Gr[2][0]; 00322 Error[3][0] = mx - Mr[0][0]; 00323 Error[4][0] = my - Mr[1][0]; 00324 Error[5][0] = mz - Mr[2][0]; 00325 mat_mult(&Km, &Errorm, &aem); // K*error(k) 00326 00327 float qe0 = 1; 00328 float qe1 = ae[0][0]/2; 00329 float qe2 = ae[1][0]/2; 00330 float qe3 = ae[2][0]/2; 00331 00332 float q0tmp = q0; 00333 float q1tmp = q1; 00334 float q2tmp = q2; 00335 float q3tmp = q3; 00336 q0 = q0tmp*qe0 - q1tmp*qe1 - q2tmp*qe2 - q3tmp*qe3; 00337 q1 = q0tmp*qe1 + q1tmp*qe0 + q2tmp*qe3 - q3tmp*qe2; 00338 q2 = q0tmp*qe2 + q2tmp*qe0 - q1tmp*qe3 + q3tmp*qe1; 00339 q3 = q0tmp*qe3 + q1tmp*qe2 - q2tmp*qe1 + q3tmp*qe0; 00340 00341 // Normalise quaternion 00342 recipNorm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3); 00343 q0 *= recipNorm; 00344 q1 *= recipNorm; 00345 q2 *= recipNorm; 00346 q3 *= recipNorm; 00347 } 00348 */ 00349 static void sensfusion9UpdateQImpl(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float dt) 00350 { 00351 float recipNorm; 00352 00353 gx = gx * M_PI_F / 180.0f; 00354 gy = gy * M_PI_F / 180.0f; 00355 gz = gz * M_PI_F / 180.0f; 00356 00357 // Normalise accelerometer measurement 00358 recipNorm = invSqrt(ax*ax + ay*ay + az*az); 00359 ax *= recipNorm; 00360 ay *= recipNorm; 00361 az *= recipNorm; 00362 00363 // Normalise magnetometer measurement 00364 recipNorm = invSqrt(mx*mx + my*my + mz*mz); 00365 mx *= recipNorm; 00366 my *= recipNorm; 00367 mz *= recipNorm; 00368 00369 //Converto il vettore intensità campo magnetico 00370 //float h1 = mx*q1 + my*q2 + mz*q3; 00371 //float h2 = mx*q0 - my*q3 + mz*q2; 00372 //float h3 = mx*q3 + my*q0 - mz*q1; 00373 //float h4 = -mx*q2 + my*q1 + mz*q0; 00374 //float n2 = q0*h2 + q1*h1 + q2*h4 - q3*h3; 00375 //float n3 = q0*h3 - q1*h4 + q2*h1 + q3*h2; 00376 //float n4 = q0*h4 + q1*h3 - q2*h2 + q3*h1; 00377 //float b2 = arm_sqrt(n2*n2 + n3*n3); 00378 //float b4 = n4; 00379 //float b2 = mx; 00380 //float b4 = mz; 00381 float hx = mx*q0*q0 + 2.0f*mz*q0*q2 - 2.0f*my*q0*q3 + mx*q1*q1 + 2.0f*my*q1*q2 + 2.0f*mz*q1*q3 - mx*q2*q2 - mx*q3*q3; 00382 float hy = my*q0*q0 - 2.0f*mz*q0*q1 + 2.0f*mx*q0*q3 - my*q1*q1 + 2.0f*mx*q1*q2 + my*q2*q2 + 2.0f*mz*q2*q3 - my*q3*q3; 00383 float hz = mz*q0*q0 + 2.0f*my*q0*q1 - 2.0f*mx*q0*q2 - mz*q1*q1 + 2.0f*mx*q1*q3 - mz*q2*q1 + 2.0f*my*q2*q3 + mz*q3*q3; 00384 float b2 = arm_sqrt(hx*hx + hy*hy); 00385 float b4 = hz; 00386 00387 A[0][0] = 1.0f; 00388 A[0][1] = -gx*dt*0.5f; 00389 A[0][2] = -gy*dt*0.5f; 00390 A[0][3] = -gz*dt*0.5f; 00391 A[1][0] = gx*dt*0.5f; 00392 A[1][1] = 1.0f; 00393 A[1][2] = gz*dt*0.5f; 00394 A[1][3] = -gy*dt*0.5f; 00395 A[2][0] = gy*dt*0.5f; 00396 A[2][1] = -gz*dt*0.5f; 00397 A[2][2] = 1.0f; 00398 A[2][3] = gx*dt*0.5f; 00399 A[3][0] = gz*dt*0.5f; 00400 A[3][1] = gy*dt*0.5f; 00401 A[3][2] = -gx*dt*0.5f; 00402 A[3][3] = 1.0f; 00403 00404 // Aggiorno la Covarianza 00405 float P0[STATE_SIZE][STATE_SIZE]; 00406 arm_matrix_instance_f32 P0m = {STATE_SIZE, STATE_SIZE, (float *)P0}; 00407 00408 mat_mult(&Am, &Pm, &tmpNN1m); // A P 00409 mat_trans(&Am, &tmpNN2m); // A' 00410 mat_mult(&tmpNN1m, &tmpNN2m, &P0m); // A P A' 00411 for(int i = 0; i < STATE_SIZE; ++i){ 00412 P0[i][i] += Q_VARIANCE; 00413 } 00414 00415 // Calcolo il guadagno 00416 float K[STATE_SIZE][OUTPUT_SIZE]; 00417 arm_matrix_instance_f32 Km = {STATE_SIZE, OUTPUT_SIZE, (float *)K}; 00418 00419 float h[OUTPUT_SIZE][STATE_SIZE]; 00420 arm_matrix_instance_f32 Hm = {OUTPUT_SIZE, STATE_SIZE, (float *)h}; 00421 00422 float hT[STATE_SIZE][OUTPUT_SIZE]; 00423 arm_matrix_instance_f32 HTm = {STATE_SIZE, OUTPUT_SIZE, (float *)hT}; 00424 00425 float P0hT[STATE_SIZE][OUTPUT_SIZE]; 00426 arm_matrix_instance_f32 P0HTm = {STATE_SIZE, OUTPUT_SIZE, (float *)P0hT}; 00427 00428 float hP0hT[OUTPUT_SIZE][OUTPUT_SIZE]; 00429 arm_matrix_instance_f32 HP0HTm = {OUTPUT_SIZE, OUTPUT_SIZE, (float *)hP0hT}; 00430 00431 float hP0hT_inv[OUTPUT_SIZE][OUTPUT_SIZE]; 00432 arm_matrix_instance_f32 HP0HT_INVm = {OUTPUT_SIZE, OUTPUT_SIZE, (float *)hP0hT_inv}; 00433 00434 //Accelerometer 00435 h[0][0] = -2.0f*q2; 00436 h[0][1] = 2.0f*q3; 00437 h[0][2] = -2.0f*q0; 00438 h[0][3] = 2.0f*q1; 00439 h[1][0] = 2.0f*q1; 00440 h[1][1] = 2.0f*q0; 00441 h[1][2] = 2.0f*q3; 00442 h[1][3] = 2.0f*q2; 00443 h[2][0] = 4.0f*q0; 00444 h[2][1] = 0.0f; 00445 h[2][2] = 0.0f; 00446 h[2][3] = 4.0f*q3; 00447 //h[2][0] = 2.0f*q0; 00448 //h[2][1] = -2.0f*q1; 00449 //h[2][2] = -2.0f*q2; 00450 //h[2][3] = 2.0f*q3; 00451 00452 //Magnetometer 00453 h[3][0] = 4.0f*b2*q0 - 2.0f*b4*q2; 00454 h[3][1] = 4.0f*b2*q1 + 2.0f*b4*q3; 00455 h[3][2] = -2.0f*b4*q0; 00456 h[3][3] = 2.0f*b4*q1; 00457 h[4][0] = 2.0f*b4*q1 - 2.0f*b2*q3; 00458 h[4][1] = 2.0f*b2*q2 + 2.0f*b4*q0; 00459 h[4][2] = 2.0f*b2*q1 + 2.0f*b4*q3; 00460 h[4][3] = 2.0f*b4*q2 - 2.0f*b2*q0; 00461 h[5][0] = 2.0f*b2*q2 + 4.0f*b4*q0; 00462 h[5][1] = 2.0f*b2*q3; 00463 h[5][2] = 2.0f*b2*q0; 00464 h[5][3] = 2.0f*b2*q1 + 4.0f*b4*q3; 00465 00466 //h[3][0] = -2.0f*b4*q2; 00467 //h[3][1] = 2.0f*b4*q3; 00468 //h[3][2] = -4.0f*b2*q2 - 2.0f*b4*q0; 00469 //h[3][3] = -4.0f*b2*q3 + 2.0f*b4*q1; 00470 //h[4][0] = -2.0f*b2*q3 + 2.0f*b4*q1; 00471 //h[4][1] = 2.0f*b2*q2 + 2.0f*b4*q0; 00472 //h[4][2] = 2.0f*b2*q1 + 2.0f*b4*q3; 00473 //h[4][3] = -2.0f*b2*q0 + 2.0f*b4*q2; 00474 //h[5][0] = 2.0f*b2*q2; 00475 //h[5][1] = 2.0f*b2*q3 - 4.0f*b4*q1; 00476 //h[5][2] = 2.0f*b2*q0 - 4.0f*b4*q2; 00477 //h[5][3] = 2.0f*b2*q1; 00478 00479 00480 // ====== INNOVATION COVARIANCE ====== 00481 mat_trans(&Hm, &HTm); 00482 mat_mult(&P0m, &HTm, &P0HTm); // P0*H' 00483 mat_mult(&Hm, &P0HTm, &HP0HTm); // H*P0*H' 00484 for (int i = 0; i < OUTPUT_SIZE; ++i) { // Add the element of HPH' to the above 00485 hP0hT[i][i] += R[i]; 00486 } 00487 mat_inv(&HP0HTm, &HP0HT_INVm); // (H*P0*H' + R)^(-1) 00488 mat_mult(&P0HTm, &HP0HT_INVm, &Km); // K = P0*H'*(H*P0*H' + R)^(-1) 00489 00490 //Aggiorno Stato 00491 float State[STATE_SIZE][1]; 00492 arm_matrix_instance_f32 Statem = {STATE_SIZE, 1, (float *)State}; 00493 float StateTMP[STATE_SIZE][1]; 00494 arm_matrix_instance_f32 StateTMPm = {STATE_SIZE, 1, (float *)StateTMP}; 00495 float Error[OUTPUT_SIZE][1]; 00496 arm_matrix_instance_f32 Errorm = {STATE_SIZE, 1, (float *)Error}; 00497 float ErrorTMP[OUTPUT_SIZE][1]; 00498 arm_matrix_instance_f32 ErrorTMPm = {STATE_SIZE, 1, (float *)ErrorTMP}; 00499 State[0][0] = q0; 00500 State[1][0] = q1; 00501 State[2][0] = q2; 00502 State[3][0] = q3; 00503 mat_mult(&Am, &Statem, &StateTMPm); // A*x(k) 00504 00505 Error[0][0] = ax - 2.0f*(q1*q3 - q0*q2); 00506 Error[1][0] = ay - 2.0f*(q0*q1 + q2*q3); 00507 Error[2][0] = az - 2.0f*(q0*q0 + q3*q3 - 0.5f); 00508 Error[3][0] = mx - 2.0f*(b2*(q0*q0 + q1*q1 - 0.5f) - b4*(q0*q2 - q1*q3)); 00509 Error[4][0] = my - 2.0f*(b4*(q0*q1 + q2*q3) - b2*(q0*q3 - q1*q2)); 00510 Error[5][0] = mz - 2.0f*(b4*(q0*q0 + q3*q3 - 0.5f) + b2*(q0*q2 + q1*q3)); 00511 mat_mult(&Km, &Errorm, &ErrorTMPm); // K*error(k) 00512 00513 q0 = StateTMP[0][0] + ErrorTMP[0][0]; 00514 q1 = StateTMP[1][0] + ErrorTMP[1][0]; 00515 q2 = StateTMP[2][0] + ErrorTMP[2][0]; 00516 q3 = StateTMP[3][0] + ErrorTMP[3][0]; 00517 00518 //Aggiorno la P 00519 mat_mult(&Km, &Hm, &tmpNN1m); // KH 00520 for (int i = 0; i < STATE_SIZE; ++i) { 00521 tmpNN1d[i][i] -= 1.0f; 00522 for (int j = 0; j < STATE_SIZE; ++j) { 00523 tmpNN1d[i][j] *= -1.0f; 00524 } 00525 } // -(KH - I) 00526 mat_mult(&tmpNN1m, &P0m, &Pm); // -(KH - I)*P0 00527 00528 // Normalise quaternion 00529 recipNorm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3); 00530 q0 *= recipNorm; 00531 q1 *= recipNorm; 00532 q2 *= recipNorm; 00533 q3 *= recipNorm; 00534 } 00535 00536 void sensfusion9GetEulerRPY(float* roll, float* pitch, float* yaw) 00537 { 00538 float gx = gravX; 00539 float gy = gravY; 00540 float gz = gravZ; 00541 00542 if (gx > 1) gx = 1; 00543 if (gx < -1) gx = -1; 00544 00545 *yaw = atan2f(2.0f*(q0*q3 + q1*q2), q0*q0 + q1*q1 - q2*q2 - q3*q3) * 180.0f / M_PI_F; 00546 *pitch = asinf(gx) * 180.0f / M_PI_F; //Pitch seems to be inverted 00547 *roll = atan2f(gy, gz) * 180.0f / M_PI_F; 00548 } 00549 void sensfusion9GetQuaternion(float* Q0, float* Q1, float* Q2, float* Q3) 00550 { 00551 *Q0 = q0; 00552 *Q1 = q1; 00553 *Q2 = q2; 00554 *Q3 = q3; 00555 } 00556 00557 float sensfusion9GetAccZWithoutGravity(const float ax, const float ay, const float az) 00558 { 00559 return sensfusion9GetAccZ(ax, ay, az) - baseZacc; 00560 } 00561 00562 float sensfusion9GetInvThrustCompensationForTilt() 00563 { 00564 // Return the z component of the estimated gravity direction 00565 // (0, 0, 1) dot G 00566 return gravZ; 00567 } 00568 00569 //--------------------------------------------------------------------------------------------------- 00570 // Fast inverse square-root 00571 // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root 00572 float invSqrt(float x) 00573 { 00574 float halfx = 0.5f * x; 00575 float y = x; 00576 long i = *(long*)&y; 00577 i = 0x5f3759df - (i>>1); 00578 y = *(float*)&i; 00579 y = y * (1.5f - (halfx * y * y)); 00580 return y; 00581 } 00582 00583 static float sensfusion9GetAccZ(const float ax, const float ay, const float az) 00584 { 00585 // return vertical acceleration 00586 // (A dot G) / |G|, (|G| = 1) -> (A dot G) 00587 return (ax * gravX + ay * gravY + az * gravZ); 00588 } 00589 00590 static void estimatedGravityDirection(float* gx, float* gy, float* gz) 00591 { 00592 *gx = 2 * (q1 * q3 - q0 * q2); 00593 *gy = 2 * (q0 * q1 + q2 * q3); 00594 *gz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3; 00595 }
Generated on Tue Jul 12 2022 23:01:10 by
1.7.2