diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_fusion_generated_compare.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_fusion_generated_compare.cpp deleted file mode 100644 index a6fa3d2484..0000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_fusion_generated_compare.cpp +++ /dev/null @@ -1,604 +0,0 @@ -#include -#include -#include -#include "../../../../../matrix/matrix/math.hpp" - -typedef matrix::Vector Vector24f; -typedef matrix::SquareMatrix SquareMatrix24f; - -float sq(float in) { - return in * in; -} - -int main() -{ - // Compare calculation of observation Jacobians and Kalman gains for sympy and matlab generated equations - - float Hfusion[24]; - Vector24f H_MAG; - Vector24f Kfusion; - float mag_innov_var; - - // quaternion inputs must be normalised - float q0 = 2.0f * ((float)rand() - 0.5f); - float q1 = 2.0f * ((float)rand() - 0.5f); - float q2 = 2.0f * ((float)rand() - 0.5f); - float q3 = 2.0f * ((float)rand() - 0.5f); - const float length = sqrtf(sq(q0) + sq(q1) + sq(q2) + sq(q3)); - q0 /= length; - q1 /= length; - q2 /= length; - q3 /= length; - - const float magN = 2.0f * ((float)rand() - 0.5f); - const float magE = 2.0f * ((float)rand() - 0.5f); - const float magD = 2.0f * ((float)rand() - 0.5f); - - const float R_MAG = sq(0.05f); - const bool update_all_states = true; - - // create a symmetrical positive dfinite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1 - SquareMatrix24f P; - for (int col=0; col<=23; col++) { - for (int row=0; row<=col; row++) { - if (row == col) { - P(row,col) = (float)rand(); - } else { - P(col,row) = P(row,col) = 2.0f * ((float)rand() - 0.5f); - } - } - } - - // common expressions used by sympy generated equations - // calculate intermediate variables used for X axis innovation variance, observation Jacobians and Kalman gainss - const float HKX0 = -magD*q2 + magE*q3 + magN*q0; - const float HKX1 = magD*q3 + magE*q2 + magN*q1; - const float HKX2 = magE*q1; - const float HKX3 = magD*q0; - const float HKX4 = magN*q2; - const float HKX5 = magD*q1 + magE*q0 - magN*q3; - const float HKX6 = powf(q0, 2) + powf(q1, 2) - powf(q2, 2) - powf(q3, 2); - const float HKX7 = q0*q3 + q1*q2; - const float HKX8 = q1*q3; - const float HKX9 = q0*q2; - const float HKX10 = 2*HKX7; - const float HKX11 = -2*HKX8 + 2*HKX9; - const float HKX12 = 2*HKX1; - const float HKX13 = 2*HKX0; - const float HKX14 = -2*HKX2 + 2*HKX3 + 2*HKX4; - const float HKX15 = 2*HKX5; - const float HKX16 = HKX10*P(0,17) - HKX11*P(0,18) + HKX12*P(0,1) + HKX13*P(0,0) - HKX14*P(0,2) + HKX15*P(0,3) + HKX6*P(0,16) + P(0,19); - const float HKX17 = HKX10*P(16,17) - HKX11*P(16,18) + HKX12*P(1,16) + HKX13*P(0,16) - HKX14*P(2,16) + HKX15*P(3,16) + HKX6*P(16,16) + P(16,19); - const float HKX18 = HKX10*P(17,18) - HKX11*P(18,18) + HKX12*P(1,18) + HKX13*P(0,18) - HKX14*P(2,18) + HKX15*P(3,18) + HKX6*P(16,18) + P(18,19); - const float HKX19 = HKX10*P(2,17) - HKX11*P(2,18) + HKX12*P(1,2) + HKX13*P(0,2) - HKX14*P(2,2) + HKX15*P(2,3) + HKX6*P(2,16) + P(2,19); - const float HKX20 = HKX10*P(17,17) - HKX11*P(17,18) + HKX12*P(1,17) + HKX13*P(0,17) - HKX14*P(2,17) + HKX15*P(3,17) + HKX6*P(16,17) + P(17,19); - const float HKX21 = HKX10*P(3,17) - HKX11*P(3,18) + HKX12*P(1,3) + HKX13*P(0,3) - HKX14*P(2,3) + HKX15*P(3,3) + HKX6*P(3,16) + P(3,19); - const float HKX22 = HKX10*P(1,17) - HKX11*P(1,18) + HKX12*P(1,1) + HKX13*P(0,1) - HKX14*P(1,2) + HKX15*P(1,3) + HKX6*P(1,16) + P(1,19); - const float HKX23 = HKX10*P(17,19) - HKX11*P(18,19) + HKX12*P(1,19) + HKX13*P(0,19) - HKX14*P(2,19) + HKX15*P(3,19) + HKX6*P(16,19) + P(19,19); - const float HKX24 = 1.0F/(HKX10*HKX20 - HKX11*HKX18 + HKX12*HKX22 + HKX13*HKX16 - HKX14*HKX19 + HKX15*HKX21 + HKX17*HKX6 + HKX23 + R_MAG); - - // common expressions used by matlab generated equations - float SH_MAG[9]; - SH_MAG[0] = 2.0f*magD*q3 + 2.0f*magE*q2 + 2.0f*magN*q1; - SH_MAG[1] = 2.0f*magD*q0 - 2.0f*magE*q1 + 2.0f*magN*q2; - SH_MAG[2] = 2.0f*magD*q1 + 2.0f*magE*q0 - 2.0f*magN*q3; - SH_MAG[3] = sq(q3); - SH_MAG[4] = sq(q2); - SH_MAG[5] = sq(q1); - SH_MAG[6] = sq(q0); - SH_MAG[7] = 2.0f*magN*q0; - SH_MAG[8] = 2.0f*magE*q3; - - // Compare X axis equations - { - mag_innov_var = (HKX10*HKX20 - HKX11*HKX18 + HKX12*HKX22 + HKX13*HKX16 - HKX14*HKX19 + HKX15*HKX21 + HKX17*HKX6 + HKX23 + R_MAG); - float HK50 = 1.0F/mag_innov_var; - - // Calculate X axis observation jacobians - memset(Hfusion, 0, sizeof(Hfusion)); - Hfusion[0] = 2*HKX0; - Hfusion[1] = 2*HKX1; - Hfusion[2] = 2*HKX2 - 2*HKX3 - 2*HKX4; - Hfusion[3] = 2*HKX5; - Hfusion[16] = HKX6; - Hfusion[17] = 2*HKX7; - Hfusion[18] = 2*HKX8 - 2*HKX9; - Hfusion[19] = 1; - - // Calculate X axis Kalman gains - if (update_all_states) { - Kfusion(0) = HKX16*HKX24; - Kfusion(1) = HKX22*HKX24; - Kfusion(2) = HKX19*HKX24; - Kfusion(3) = HKX21*HKX24; - - for (unsigned row = 4; row <= 15; row++) { - Kfusion(row) = HKX24*(HKX10*P(row,17) - HKX11*P(row,18) + HKX12*P(1,row) + HKX13*P(0,row) - HKX14*P(2,row) + HKX15*P(3,row) + HKX6*P(row,16) + P(row,19)); - } - - for (unsigned row = 22; row <= 23; row++) { - Kfusion(row) = HKX24*(HKX10*P(17,row) - HKX11*P(18,row) + HKX12*P(1,row) + HKX13*P(0,row) - HKX14*P(2,row) + HKX15*P(3,row) + HKX6*P(16,row) + P(19,row)); - } - } - - Kfusion(16) = HKX17*HKX24; - Kfusion(17) = HKX20*HKX24; - Kfusion(18) = HKX18*HKX24; - Kfusion(19) = HKX23*HKX24; - - for (unsigned row = 20; row <= 21; row++) { - Kfusion(row) = HKX24*(HKX10*P(17,row) - HKX11*P(18,row) + HKX12*P(1,row) + HKX13*P(0,row) - HKX14*P(2,row) + HKX15*P(3,row) + HKX6*P(16,row) + P(19,row)); - } - - // save output and repeat calculation using legacy matlab generated code - float Hfusion_sympy[24]; - Vector24f Kfusion_sympy; - for (int row=0; row<24; row++) { - Hfusion_sympy[row] = Hfusion[row]; - Kfusion_sympy(row) = Kfusion(row); - } - - // repeat calculation using matlab generated equations - // X axis innovation variance - mag_innov_var = (P(19,19) + R_MAG + P(1,19)*SH_MAG[0] - P(2,19)*SH_MAG[1] + P(3,19)*SH_MAG[2] - P(16,19)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + (2.0f*q0*q3 + 2.0f*q1*q2)*(P(19,17) + P(1,17)*SH_MAG[0] - P(2,17)*SH_MAG[1] + P(3,17)*SH_MAG[2] - P(16,17)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,17)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,17)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,17)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (2.0f*q0*q2 - 2.0f*q1*q3)*(P(19,18) + P(1,18)*SH_MAG[0] - P(2,18)*SH_MAG[1] + P(3,18)*SH_MAG[2] - P(16,18)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,18)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,18)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,18)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)*(P(19,0) + P(1,0)*SH_MAG[0] - P(2,0)*SH_MAG[1] + P(3,0)*SH_MAG[2] - P(16,0)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,0)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,0)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,0)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P(17,19)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,19)*(2.0f*q0*q2 - 2.0f*q1*q3) + SH_MAG[0]*(P(19,1) + P(1,1)*SH_MAG[0] - P(2,1)*SH_MAG[1] + P(3,1)*SH_MAG[2] - P(16,1)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,1)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,1)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,1)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - SH_MAG[1]*(P(19,2) + P(1,2)*SH_MAG[0] - P(2,2)*SH_MAG[1] + P(3,2)*SH_MAG[2] - P(16,2)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,2)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,2)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,2)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[2]*(P(19,3) + P(1,3)*SH_MAG[0] - P(2,3)*SH_MAG[1] + P(3,3)*SH_MAG[2] - P(16,3)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,3)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,3)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,3)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P(19,16) + P(1,16)*SH_MAG[0] - P(2,16)*SH_MAG[1] + P(3,16)*SH_MAG[2] - P(16,16)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,16)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,16)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,16)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P(0,19)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)); - - // Calculate X axis observation jacobians - H_MAG.setZero(); - H_MAG(0) = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2; - H_MAG(1) = SH_MAG[0]; - H_MAG(2) = -SH_MAG[1]; - H_MAG(3) = SH_MAG[2]; - H_MAG(16) = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6]; - H_MAG(17) = 2.0f*q0*q3 + 2.0f*q1*q2; - H_MAG(18) = 2.0f*q1*q3 - 2.0f*q0*q2; - H_MAG(19) = 1.0f; - - // Calculate X axis Kalman gains - float SK_MX[5]; - SK_MX[0] = 1.0f / mag_innov_var; - SK_MX[1] = SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]; - SK_MX[2] = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2; - SK_MX[3] = 2.0f*q0*q2 - 2.0f*q1*q3; - SK_MX[4] = 2.0f*q0*q3 + 2.0f*q1*q2; - - if (update_all_states) { - Kfusion(0) = SK_MX[0]*(P(0,19) + P(0,1)*SH_MAG[0] - P(0,2)*SH_MAG[1] + P(0,3)*SH_MAG[2] + P(0,0)*SK_MX[2] - P(0,16)*SK_MX[1] + P(0,17)*SK_MX[4] - P(0,18)*SK_MX[3]); - Kfusion(1) = SK_MX[0]*(P(1,19) + P(1,1)*SH_MAG[0] - P(1,2)*SH_MAG[1] + P(1,3)*SH_MAG[2] + P(1,0)*SK_MX[2] - P(1,16)*SK_MX[1] + P(1,17)*SK_MX[4] - P(1,18)*SK_MX[3]); - Kfusion(2) = SK_MX[0]*(P(2,19) + P(2,1)*SH_MAG[0] - P(2,2)*SH_MAG[1] + P(2,3)*SH_MAG[2] + P(2,0)*SK_MX[2] - P(2,16)*SK_MX[1] + P(2,17)*SK_MX[4] - P(2,18)*SK_MX[3]); - Kfusion(3) = SK_MX[0]*(P(3,19) + P(3,1)*SH_MAG[0] - P(3,2)*SH_MAG[1] + P(3,3)*SH_MAG[2] + P(3,0)*SK_MX[2] - P(3,16)*SK_MX[1] + P(3,17)*SK_MX[4] - P(3,18)*SK_MX[3]); - Kfusion(4) = SK_MX[0]*(P(4,19) + P(4,1)*SH_MAG[0] - P(4,2)*SH_MAG[1] + P(4,3)*SH_MAG[2] + P(4,0)*SK_MX[2] - P(4,16)*SK_MX[1] + P(4,17)*SK_MX[4] - P(4,18)*SK_MX[3]); - Kfusion(5) = SK_MX[0]*(P(5,19) + P(5,1)*SH_MAG[0] - P(5,2)*SH_MAG[1] + P(5,3)*SH_MAG[2] + P(5,0)*SK_MX[2] - P(5,16)*SK_MX[1] + P(5,17)*SK_MX[4] - P(5,18)*SK_MX[3]); - Kfusion(6) = SK_MX[0]*(P(6,19) + P(6,1)*SH_MAG[0] - P(6,2)*SH_MAG[1] + P(6,3)*SH_MAG[2] + P(6,0)*SK_MX[2] - P(6,16)*SK_MX[1] + P(6,17)*SK_MX[4] - P(6,18)*SK_MX[3]); - Kfusion(7) = SK_MX[0]*(P(7,19) + P(7,1)*SH_MAG[0] - P(7,2)*SH_MAG[1] + P(7,3)*SH_MAG[2] + P(7,0)*SK_MX[2] - P(7,16)*SK_MX[1] + P(7,17)*SK_MX[4] - P(7,18)*SK_MX[3]); - Kfusion(8) = SK_MX[0]*(P(8,19) + P(8,1)*SH_MAG[0] - P(8,2)*SH_MAG[1] + P(8,3)*SH_MAG[2] + P(8,0)*SK_MX[2] - P(8,16)*SK_MX[1] + P(8,17)*SK_MX[4] - P(8,18)*SK_MX[3]); - Kfusion(9) = SK_MX[0]*(P(9,19) + P(9,1)*SH_MAG[0] - P(9,2)*SH_MAG[1] + P(9,3)*SH_MAG[2] + P(9,0)*SK_MX[2] - P(9,16)*SK_MX[1] + P(9,17)*SK_MX[4] - P(9,18)*SK_MX[3]); - Kfusion(10) = SK_MX[0]*(P(10,19) + P(10,1)*SH_MAG[0] - P(10,2)*SH_MAG[1] + P(10,3)*SH_MAG[2] + P(10,0)*SK_MX[2] - P(10,16)*SK_MX[1] + P(10,17)*SK_MX[4] - P(10,18)*SK_MX[3]); - Kfusion(11) = SK_MX[0]*(P(11,19) + P(11,1)*SH_MAG[0] - P(11,2)*SH_MAG[1] + P(11,3)*SH_MAG[2] + P(11,0)*SK_MX[2] - P(11,16)*SK_MX[1] + P(11,17)*SK_MX[4] - P(11,18)*SK_MX[3]); - Kfusion(12) = SK_MX[0]*(P(12,19) + P(12,1)*SH_MAG[0] - P(12,2)*SH_MAG[1] + P(12,3)*SH_MAG[2] + P(12,0)*SK_MX[2] - P(12,16)*SK_MX[1] + P(12,17)*SK_MX[4] - P(12,18)*SK_MX[3]); - Kfusion(13) = SK_MX[0]*(P(13,19) + P(13,1)*SH_MAG[0] - P(13,2)*SH_MAG[1] + P(13,3)*SH_MAG[2] + P(13,0)*SK_MX[2] - P(13,16)*SK_MX[1] + P(13,17)*SK_MX[4] - P(13,18)*SK_MX[3]); - Kfusion(14) = SK_MX[0]*(P(14,19) + P(14,1)*SH_MAG[0] - P(14,2)*SH_MAG[1] + P(14,3)*SH_MAG[2] + P(14,0)*SK_MX[2] - P(14,16)*SK_MX[1] + P(14,17)*SK_MX[4] - P(14,18)*SK_MX[3]); - Kfusion(15) = SK_MX[0]*(P(15,19) + P(15,1)*SH_MAG[0] - P(15,2)*SH_MAG[1] + P(15,3)*SH_MAG[2] + P(15,0)*SK_MX[2] - P(15,16)*SK_MX[1] + P(15,17)*SK_MX[4] - P(15,18)*SK_MX[3]); - Kfusion(22) = SK_MX[0]*(P(22,19) + P(22,1)*SH_MAG[0] - P(22,2)*SH_MAG[1] + P(22,3)*SH_MAG[2] + P(22,0)*SK_MX[2] - P(22,16)*SK_MX[1] + P(22,17)*SK_MX[4] - P(22,18)*SK_MX[3]); - Kfusion(23) = SK_MX[0]*(P(23,19) + P(23,1)*SH_MAG[0] - P(23,2)*SH_MAG[1] + P(23,3)*SH_MAG[2] + P(23,0)*SK_MX[2] - P(23,16)*SK_MX[1] + P(23,17)*SK_MX[4] - P(23,18)*SK_MX[3]); - } - - Kfusion(16) = SK_MX[0]*(P(16,19) + P(16,1)*SH_MAG[0] - P(16,2)*SH_MAG[1] + P(16,3)*SH_MAG[2] + P(16,0)*SK_MX[2] - P(16,16)*SK_MX[1] + P(16,17)*SK_MX[4] - P(16,18)*SK_MX[3]); - Kfusion(17) = SK_MX[0]*(P(17,19) + P(17,1)*SH_MAG[0] - P(17,2)*SH_MAG[1] + P(17,3)*SH_MAG[2] + P(17,0)*SK_MX[2] - P(17,16)*SK_MX[1] + P(17,17)*SK_MX[4] - P(17,18)*SK_MX[3]); - Kfusion(18) = SK_MX[0]*(P(18,19) + P(18,1)*SH_MAG[0] - P(18,2)*SH_MAG[1] + P(18,3)*SH_MAG[2] + P(18,0)*SK_MX[2] - P(18,16)*SK_MX[1] + P(18,17)*SK_MX[4] - P(18,18)*SK_MX[3]); - Kfusion(19) = SK_MX[0]*(P(19,19) + P(19,1)*SH_MAG[0] - P(19,2)*SH_MAG[1] + P(19,3)*SH_MAG[2] + P(19,0)*SK_MX[2] - P(19,16)*SK_MX[1] + P(19,17)*SK_MX[4] - P(19,18)*SK_MX[3]); - Kfusion(20) = SK_MX[0]*(P(20,19) + P(20,1)*SH_MAG[0] - P(20,2)*SH_MAG[1] + P(20,3)*SH_MAG[2] + P(20,0)*SK_MX[2] - P(20,16)*SK_MX[1] + P(20,17)*SK_MX[4] - P(20,18)*SK_MX[3]); - Kfusion(21) = SK_MX[0]*(P(21,19) + P(21,1)*SH_MAG[0] - P(21,2)*SH_MAG[1] + P(21,3)*SH_MAG[2] + P(21,0)*SK_MX[2] - P(21,16)*SK_MX[1] + P(21,17)*SK_MX[4] - P(21,18)*SK_MX[3]); - - // find largest observation variance difference as a fraction of the matlab value - float max_diff_fraction = 0.0f; - int max_row; - float max_old, max_new; - for (int row=0; row<24; row++) { - float diff_fraction; - if (H_MAG(row) != 0.0f) { - diff_fraction = fabsf(Hfusion_sympy[row] - H_MAG(row)) / fabsf(H_MAG(row)); - } else if (Hfusion_sympy[row] != 0.0f) { - diff_fraction = fabsf(Hfusion_sympy[row] - H_MAG(row)) / fabsf(Hfusion_sympy[row]); - } else { - diff_fraction = 0.0f; - } - if (diff_fraction > max_diff_fraction) { - max_diff_fraction = diff_fraction; - max_row = row; - max_old = H_MAG(row); - max_new = Hfusion_sympy[row]; - } - } - - if (max_diff_fraction > 1e-5f) { - printf("Fail: Magnetomer X axis Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row); - } else { - printf("Pass: Magnetomer X axis Hfusion max diff fraction = %e\n",max_diff_fraction); - } - - // find largest Kalman gain difference as a fraction of the matlab value - max_diff_fraction = 0.0f; - for (int row=0; row<4; row++) { - float diff_fraction; - if (Kfusion(row) != 0.0f) { - diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion(row)) / fabsf(Kfusion(row)); - } else if (Kfusion_sympy(row) != 0.0f) { - diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion(row)) / fabsf(Kfusion_sympy(row)); - } else { - diff_fraction = 0.0f; - } - if (diff_fraction > max_diff_fraction) { - max_diff_fraction = diff_fraction; - max_row = row; - max_old = Kfusion(row); - max_new = Kfusion_sympy(row); - } - } - - if (max_diff_fraction > 1e-5f) { - printf("Fail: Magnetomer X axis Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row); - } else { - printf("Pass: Magnetomer X axis Kfusion max diff fraction = %e\n",max_diff_fraction); - } - - } - - // Compare Y axis equations - { - // recalculate innovation variance becasue states and covariances have changed due to previous fusion - const float HKY0 = magD*q1 + magE*q0 - magN*q3; - const float HKY1 = magD*q0 - magE*q1 + magN*q2; - const float HKY2 = magD*q3 + magE*q2 + magN*q1; - const float HKY3 = magD*q2; - const float HKY4 = magE*q3; - const float HKY5 = magN*q0; - const float HKY6 = q1*q2; - const float HKY7 = q0*q3; - const float HKY8 = powf(q0, 2) - powf(q1, 2) + powf(q2, 2) - powf(q3, 2); - const float HKY9 = q0*q1 + q2*q3; - const float HKY10 = 2*HKY9; - const float HKY11 = -2*HKY6 + 2*HKY7; - const float HKY12 = 2*HKY2; - const float HKY13 = 2*HKY0; - const float HKY14 = 2*HKY1; - const float HKY15 = -2*HKY3 + 2*HKY4 + 2*HKY5; - const float HKY16 = HKY10*P(0,18) - HKY11*P(0,16) + HKY12*P(0,2) + HKY13*P(0,0) + HKY14*P(0,1) - HKY15*P(0,3) + HKY8*P(0,17) + P(0,20); - const float HKY17 = HKY10*P(17,18) - HKY11*P(16,17) + HKY12*P(2,17) + HKY13*P(0,17) + HKY14*P(1,17) - HKY15*P(3,17) + HKY8*P(17,17) + P(17,20); - const float HKY18 = HKY10*P(16,18) - HKY11*P(16,16) + HKY12*P(2,16) + HKY13*P(0,16) + HKY14*P(1,16) - HKY15*P(3,16) + HKY8*P(16,17) + P(16,20); - const float HKY19 = HKY10*P(3,18) - HKY11*P(3,16) + HKY12*P(2,3) + HKY13*P(0,3) + HKY14*P(1,3) - HKY15*P(3,3) + HKY8*P(3,17) + P(3,20); - const float HKY20 = HKY10*P(18,18) - HKY11*P(16,18) + HKY12*P(2,18) + HKY13*P(0,18) + HKY14*P(1,18) - HKY15*P(3,18) + HKY8*P(17,18) + P(18,20); - const float HKY21 = HKY10*P(1,18) - HKY11*P(1,16) + HKY12*P(1,2) + HKY13*P(0,1) + HKY14*P(1,1) - HKY15*P(1,3) + HKY8*P(1,17) + P(1,20); - const float HKY22 = HKY10*P(2,18) - HKY11*P(2,16) + HKY12*P(2,2) + HKY13*P(0,2) + HKY14*P(1,2) - HKY15*P(2,3) + HKY8*P(2,17) + P(2,20); - const float HKY23 = HKY10*P(18,20) - HKY11*P(16,20) + HKY12*P(2,20) + HKY13*P(0,20) + HKY14*P(1,20) - HKY15*P(3,20) + HKY8*P(17,20) + P(20,20); - const float HKY24 = 1.0F/(HKY10*HKY20 - HKY11*HKY18 + HKY12*HKY22 + HKY13*HKY16 + HKY14*HKY21 - HKY15*HKY19 + HKY17*HKY8 + HKY23 + R_MAG); - - // Calculate Y axis observation jacobians - memset(Hfusion, 0, sizeof(Hfusion)); - Hfusion[0] = 2*HKY0; - Hfusion[1] = 2*HKY1; - Hfusion[2] = 2*HKY2; - Hfusion[3] = 2*HKY3 - 2*HKY4 - 2*HKY5; - Hfusion[16] = 2*HKY6 - 2*HKY7; - Hfusion[17] = HKY8; - Hfusion[18] = 2*HKY9; - Hfusion[20] = 1; - - // Calculate Y axis Kalman gains - if (update_all_states) { - Kfusion(0) = HKY16*HKY24; - Kfusion(1) = HKY21*HKY24; - Kfusion(2) = HKY22*HKY24; - Kfusion(3) = HKY19*HKY24; - - for (unsigned row = 4; row <= 15; row++) { - Kfusion(row) = HKY24*(HKY10*P(row,18) - HKY11*P(row,16) + HKY12*P(2,row) + HKY13*P(0,row) + HKY14*P(1,row) - HKY15*P(3,row) + HKY8*P(row,17) + P(row,20)); - } - - for (unsigned row = 22; row <= 23; row++) { - Kfusion(row) = HKY24*(HKY10*P(18,row) - HKY11*P(16,row) + HKY12*P(2,row) + HKY13*P(0,row) + HKY14*P(1,row) - HKY15*P(3,row) + HKY8*P(17,row) + P(20,row)); - } - } - - Kfusion(16) = HKY18*HKY24; - Kfusion(17) = HKY17*HKY24; - Kfusion(18) = HKY20*HKY24; - Kfusion(19) = HKY24*(HKY10*P(18,19) - HKY11*P(16,19) + HKY12*P(2,19) + HKY13*P(0,19) + HKY14*P(1,19) - HKY15*P(3,19) + HKY8*P(17,19) + P(19,20)); - Kfusion(20) = HKY23*HKY24; - Kfusion(21) = HKY24*(HKY10*P(18,21) - HKY11*P(16,21) + HKY12*P(2,21) + HKY13*P(0,21) + HKY14*P(1,21) - HKY15*P(3,21) + HKY8*P(17,21) + P(20,21)); - - // save output and repeat calculation using legacy matlab generated code - float Hfusion_sympy[24]; - Vector24f Kfusion_sympy; - for (int row=0; row<24; row++) { - Hfusion_sympy[row] = Hfusion[row]; - Kfusion_sympy(row) = Kfusion(row); - } - - // repeat calculation using matlab generated equations - - // Y axis innovation variance - mag_innov_var = (P(20,20) + R_MAG + P(0,20)*SH_MAG[2] + P(1,20)*SH_MAG[1] + P(2,20)*SH_MAG[0] - P(17,20)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2.0f*q0*q3 - 2.0f*q1*q2)*(P(20,16) + P(0,16)*SH_MAG[2] + P(1,16)*SH_MAG[1] + P(2,16)*SH_MAG[0] - P(17,16)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P(16,16)*(2.0f*q0*q3 - 2.0f*q1*q2) + P(18,16)*(2.0f*q0*q1 + 2.0f*q2*q3) - P(3,16)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (2.0f*q0*q1 + 2.0f*q2*q3)*(P(20,18) + P(0,18)*SH_MAG[2] + P(1,18)*SH_MAG[1] + P(2,18)*SH_MAG[0] - P(17,18)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P(16,18)*(2.0f*q0*q3 - 2.0f*q1*q2) + P(18,18)*(2.0f*q0*q1 + 2.0f*q2*q3) - P(3,18)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)*(P(20,3) + P(0,3)*SH_MAG[2] + P(1,3)*SH_MAG[1] + P(2,3)*SH_MAG[0] - P(17,3)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P(16,3)*(2.0f*q0*q3 - 2.0f*q1*q2) + P(18,3)*(2.0f*q0*q1 + 2.0f*q2*q3) - P(3,3)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - P(16,20)*(2.0f*q0*q3 - 2.0f*q1*q2) + P(18,20)*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_MAG[2]*(P(20,0) + P(0,0)*SH_MAG[2] + P(1,0)*SH_MAG[1] + P(2,0)*SH_MAG[0] - P(17,0)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P(16,0)*(2.0f*q0*q3 - 2.0f*q1*q2) + P(18,0)*(2.0f*q0*q1 + 2.0f*q2*q3) - P(3,0)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[1]*(P(20,1) + P(0,1)*SH_MAG[2] + P(1,1)*SH_MAG[1] + P(2,1)*SH_MAG[0] - P(17,1)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P(16,1)*(2.0f*q0*q3 - 2.0f*q1*q2) + P(18,1)*(2.0f*q0*q1 + 2.0f*q2*q3) - P(3,1)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[0]*(P(20,2) + P(0,2)*SH_MAG[2] + P(1,2)*SH_MAG[1] + P(2,2)*SH_MAG[0] - P(17,2)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P(16,2)*(2.0f*q0*q3 - 2.0f*q1*q2) + P(18,2)*(2.0f*q0*q1 + 2.0f*q2*q3) - P(3,2)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6])*(P(20,17) + P(0,17)*SH_MAG[2] + P(1,17)*SH_MAG[1] + P(2,17)*SH_MAG[0] - P(17,17)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P(16,17)*(2.0f*q0*q3 - 2.0f*q1*q2) + P(18,17)*(2.0f*q0*q1 + 2.0f*q2*q3) - P(3,17)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - P(3,20)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)); - - // Calculate Y axis observation jacobians - H_MAG.setZero(); - H_MAG(0) = SH_MAG[2]; - H_MAG(1) = SH_MAG[1]; - H_MAG(2) = SH_MAG[0]; - H_MAG(3) = 2.0f*magD*q2 - SH_MAG[8] - SH_MAG[7]; - H_MAG(16) = 2.0f*q1*q2 - 2.0f*q0*q3; - H_MAG(17) = SH_MAG[4] - SH_MAG[3] - SH_MAG[5] + SH_MAG[6]; - H_MAG(18) = 2.0f*q0*q1 + 2.0f*q2*q3; - H_MAG(20) = 1.0f; - - // Calculate Y axis Kalman gains - float SK_MY[5]; - SK_MY[0] = 1.0f / mag_innov_var; - SK_MY[1] = SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]; - SK_MY[2] = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2; - SK_MY[3] = 2.0f*q0*q3 - 2.0f*q1*q2; - SK_MY[4] = 2.0f*q0*q1 + 2.0f*q2*q3; - - if (update_all_states) { - Kfusion(0) = SK_MY[0]*(P(0,20) + P(0,0)*SH_MAG[2] + P(0,1)*SH_MAG[1] + P(0,2)*SH_MAG[0] - P(0,3)*SK_MY[2] - P(0,17)*SK_MY[1] - P(0,16)*SK_MY[3] + P(0,18)*SK_MY[4]); - Kfusion(1) = SK_MY[0]*(P(1,20) + P(1,0)*SH_MAG[2] + P(1,1)*SH_MAG[1] + P(1,2)*SH_MAG[0] - P(1,3)*SK_MY[2] - P(1,17)*SK_MY[1] - P(1,16)*SK_MY[3] + P(1,18)*SK_MY[4]); - Kfusion(2) = SK_MY[0]*(P(2,20) + P(2,0)*SH_MAG[2] + P(2,1)*SH_MAG[1] + P(2,2)*SH_MAG[0] - P(2,3)*SK_MY[2] - P(2,17)*SK_MY[1] - P(2,16)*SK_MY[3] + P(2,18)*SK_MY[4]); - Kfusion(3) = SK_MY[0]*(P(3,20) + P(3,0)*SH_MAG[2] + P(3,1)*SH_MAG[1] + P(3,2)*SH_MAG[0] - P(3,3)*SK_MY[2] - P(3,17)*SK_MY[1] - P(3,16)*SK_MY[3] + P(3,18)*SK_MY[4]); - Kfusion(4) = SK_MY[0]*(P(4,20) + P(4,0)*SH_MAG[2] + P(4,1)*SH_MAG[1] + P(4,2)*SH_MAG[0] - P(4,3)*SK_MY[2] - P(4,17)*SK_MY[1] - P(4,16)*SK_MY[3] + P(4,18)*SK_MY[4]); - Kfusion(5) = SK_MY[0]*(P(5,20) + P(5,0)*SH_MAG[2] + P(5,1)*SH_MAG[1] + P(5,2)*SH_MAG[0] - P(5,3)*SK_MY[2] - P(5,17)*SK_MY[1] - P(5,16)*SK_MY[3] + P(5,18)*SK_MY[4]); - Kfusion(6) = SK_MY[0]*(P(6,20) + P(6,0)*SH_MAG[2] + P(6,1)*SH_MAG[1] + P(6,2)*SH_MAG[0] - P(6,3)*SK_MY[2] - P(6,17)*SK_MY[1] - P(6,16)*SK_MY[3] + P(6,18)*SK_MY[4]); - Kfusion(7) = SK_MY[0]*(P(7,20) + P(7,0)*SH_MAG[2] + P(7,1)*SH_MAG[1] + P(7,2)*SH_MAG[0] - P(7,3)*SK_MY[2] - P(7,17)*SK_MY[1] - P(7,16)*SK_MY[3] + P(7,18)*SK_MY[4]); - Kfusion(8) = SK_MY[0]*(P(8,20) + P(8,0)*SH_MAG[2] + P(8,1)*SH_MAG[1] + P(8,2)*SH_MAG[0] - P(8,3)*SK_MY[2] - P(8,17)*SK_MY[1] - P(8,16)*SK_MY[3] + P(8,18)*SK_MY[4]); - Kfusion(9) = SK_MY[0]*(P(9,20) + P(9,0)*SH_MAG[2] + P(9,1)*SH_MAG[1] + P(9,2)*SH_MAG[0] - P(9,3)*SK_MY[2] - P(9,17)*SK_MY[1] - P(9,16)*SK_MY[3] + P(9,18)*SK_MY[4]); - Kfusion(10) = SK_MY[0]*(P(10,20) + P(10,0)*SH_MAG[2] + P(10,1)*SH_MAG[1] + P(10,2)*SH_MAG[0] - P(10,3)*SK_MY[2] - P(10,17)*SK_MY[1] - P(10,16)*SK_MY[3] + P(10,18)*SK_MY[4]); - Kfusion(11) = SK_MY[0]*(P(11,20) + P(11,0)*SH_MAG[2] + P(11,1)*SH_MAG[1] + P(11,2)*SH_MAG[0] - P(11,3)*SK_MY[2] - P(11,17)*SK_MY[1] - P(11,16)*SK_MY[3] + P(11,18)*SK_MY[4]); - Kfusion(12) = SK_MY[0]*(P(12,20) + P(12,0)*SH_MAG[2] + P(12,1)*SH_MAG[1] + P(12,2)*SH_MAG[0] - P(12,3)*SK_MY[2] - P(12,17)*SK_MY[1] - P(12,16)*SK_MY[3] + P(12,18)*SK_MY[4]); - Kfusion(13) = SK_MY[0]*(P(13,20) + P(13,0)*SH_MAG[2] + P(13,1)*SH_MAG[1] + P(13,2)*SH_MAG[0] - P(13,3)*SK_MY[2] - P(13,17)*SK_MY[1] - P(13,16)*SK_MY[3] + P(13,18)*SK_MY[4]); - Kfusion(14) = SK_MY[0]*(P(14,20) + P(14,0)*SH_MAG[2] + P(14,1)*SH_MAG[1] + P(14,2)*SH_MAG[0] - P(14,3)*SK_MY[2] - P(14,17)*SK_MY[1] - P(14,16)*SK_MY[3] + P(14,18)*SK_MY[4]); - Kfusion(15) = SK_MY[0]*(P(15,20) + P(15,0)*SH_MAG[2] + P(15,1)*SH_MAG[1] + P(15,2)*SH_MAG[0] - P(15,3)*SK_MY[2] - P(15,17)*SK_MY[1] - P(15,16)*SK_MY[3] + P(15,18)*SK_MY[4]); - Kfusion(22) = SK_MY[0]*(P(22,20) + P(22,0)*SH_MAG[2] + P(22,1)*SH_MAG[1] + P(22,2)*SH_MAG[0] - P(22,3)*SK_MY[2] - P(22,17)*SK_MY[1] - P(22,16)*SK_MY[3] + P(22,18)*SK_MY[4]); - Kfusion(23) = SK_MY[0]*(P(23,20) + P(23,0)*SH_MAG[2] + P(23,1)*SH_MAG[1] + P(23,2)*SH_MAG[0] - P(23,3)*SK_MY[2] - P(23,17)*SK_MY[1] - P(23,16)*SK_MY[3] + P(23,18)*SK_MY[4]); - } - - Kfusion(16) = SK_MY[0]*(P(16,20) + P(16,0)*SH_MAG[2] + P(16,1)*SH_MAG[1] + P(16,2)*SH_MAG[0] - P(16,3)*SK_MY[2] - P(16,17)*SK_MY[1] - P(16,16)*SK_MY[3] + P(16,18)*SK_MY[4]); - Kfusion(17) = SK_MY[0]*(P(17,20) + P(17,0)*SH_MAG[2] + P(17,1)*SH_MAG[1] + P(17,2)*SH_MAG[0] - P(17,3)*SK_MY[2] - P(17,17)*SK_MY[1] - P(17,16)*SK_MY[3] + P(17,18)*SK_MY[4]); - Kfusion(18) = SK_MY[0]*(P(18,20) + P(18,0)*SH_MAG[2] + P(18,1)*SH_MAG[1] + P(18,2)*SH_MAG[0] - P(18,3)*SK_MY[2] - P(18,17)*SK_MY[1] - P(18,16)*SK_MY[3] + P(18,18)*SK_MY[4]); - Kfusion(19) = SK_MY[0]*(P(19,20) + P(19,0)*SH_MAG[2] + P(19,1)*SH_MAG[1] + P(19,2)*SH_MAG[0] - P(19,3)*SK_MY[2] - P(19,17)*SK_MY[1] - P(19,16)*SK_MY[3] + P(19,18)*SK_MY[4]); - Kfusion(20) = SK_MY[0]*(P(20,20) + P(20,0)*SH_MAG[2] + P(20,1)*SH_MAG[1] + P(20,2)*SH_MAG[0] - P(20,3)*SK_MY[2] - P(20,17)*SK_MY[1] - P(20,16)*SK_MY[3] + P(20,18)*SK_MY[4]); - Kfusion(21) = SK_MY[0]*(P(21,20) + P(21,0)*SH_MAG[2] + P(21,1)*SH_MAG[1] + P(21,2)*SH_MAG[0] - P(21,3)*SK_MY[2] - P(21,17)*SK_MY[1] - P(21,16)*SK_MY[3] + P(21,18)*SK_MY[4]); - - // find largest observation variance difference as a fraction of the matlab value - float max_diff_fraction = 0.0f; - int max_row; - float max_old, max_new; - for (int row=0; row<24; row++) { - float diff_fraction; - if (H_MAG(row) != 0.0f) { - diff_fraction = fabsf(Hfusion_sympy[row] - H_MAG(row)) / fabsf(H_MAG(row)); - } else if (Hfusion_sympy[row] != 0.0f) { - diff_fraction = fabsf(Hfusion_sympy[row] - H_MAG(row)) / fabsf(Hfusion_sympy[row]); - } else { - diff_fraction = 0.0f; - } - if (diff_fraction > max_diff_fraction) { - max_diff_fraction = diff_fraction; - max_row = row; - max_old = H_MAG(row); - max_new = Hfusion_sympy[row]; - } - } - - if (max_diff_fraction > 1e-5f) { - printf("Fail: Magnetomer Y axis Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row); - } else { - printf("Pass: Magnetomer Y axis Hfusion max diff fraction = %e\n",max_diff_fraction); - } - - // find largest Kalman gain difference as a fraction of the matlab value - max_diff_fraction = 0.0f; - for (int row=0; row<4; row++) { - float diff_fraction; - if (Kfusion(row) != 0.0f) { - diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion(row)) / fabsf(Kfusion(row)); - } else if (Kfusion_sympy(row) != 0.0f) { - diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion(row)) / fabsf(Kfusion_sympy(row)); - } else { - diff_fraction = 0.0f; - } - if (diff_fraction > max_diff_fraction) { - max_diff_fraction = diff_fraction; - max_row = row; - max_old = Kfusion(row); - max_new = Kfusion_sympy(row); - } - } - - if (max_diff_fraction > 1e-5f) { - printf("Fail: Magnetomer Y axis Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row); - } else { - printf("Pass: Magnetomer Y axis Kfusion max diff fraction = %e\n",max_diff_fraction); - } - - } - - // Compare Z axis equations - { - // recalculate innovation variance becasue states and covariances have changed due to previous fusion - const float HKZ0 = magD*q0 - magE*q1 + magN*q2; - const float HKZ1 = magN*q3; - const float HKZ2 = magD*q1; - const float HKZ3 = magE*q0; - const float HKZ4 = -magD*q2 + magE*q3 + magN*q0; - const float HKZ5 = magD*q3 + magE*q2 + magN*q1; - const float HKZ6 = q0*q2 + q1*q3; - const float HKZ7 = q2*q3; - const float HKZ8 = q0*q1; - const float HKZ9 = powf(q0, 2) - powf(q1, 2) - powf(q2, 2) + powf(q3, 2); - const float HKZ10 = 2*HKZ6; - const float HKZ11 = -2*HKZ7 + 2*HKZ8; - const float HKZ12 = 2*HKZ5; - const float HKZ13 = 2*HKZ0; - const float HKZ14 = -2*HKZ1 + 2*HKZ2 + 2*HKZ3; - const float HKZ15 = 2*HKZ4; - const float HKZ16 = HKZ10*P(0,16) - HKZ11*P(0,17) + HKZ12*P(0,3) + HKZ13*P(0,0) - HKZ14*P(0,1) + HKZ15*P(0,2) + HKZ9*P(0,18) + P(0,21); - const float HKZ17 = HKZ10*P(16,18) - HKZ11*P(17,18) + HKZ12*P(3,18) + HKZ13*P(0,18) - HKZ14*P(1,18) + HKZ15*P(2,18) + HKZ9*P(18,18) + P(18,21); - const float HKZ18 = HKZ10*P(16,17) - HKZ11*P(17,17) + HKZ12*P(3,17) + HKZ13*P(0,17) - HKZ14*P(1,17) + HKZ15*P(2,17) + HKZ9*P(17,18) + P(17,21); - const float HKZ19 = HKZ10*P(1,16) - HKZ11*P(1,17) + HKZ12*P(1,3) + HKZ13*P(0,1) - HKZ14*P(1,1) + HKZ15*P(1,2) + HKZ9*P(1,18) + P(1,21); - const float HKZ20 = HKZ10*P(16,16) - HKZ11*P(16,17) + HKZ12*P(3,16) + HKZ13*P(0,16) - HKZ14*P(1,16) + HKZ15*P(2,16) + HKZ9*P(16,18) + P(16,21); - const float HKZ21 = HKZ10*P(3,16) - HKZ11*P(3,17) + HKZ12*P(3,3) + HKZ13*P(0,3) - HKZ14*P(1,3) + HKZ15*P(2,3) + HKZ9*P(3,18) + P(3,21); - const float HKZ22 = HKZ10*P(2,16) - HKZ11*P(2,17) + HKZ12*P(2,3) + HKZ13*P(0,2) - HKZ14*P(1,2) + HKZ15*P(2,2) + HKZ9*P(2,18) + P(2,21); - const float HKZ23 = HKZ10*P(16,21) - HKZ11*P(17,21) + HKZ12*P(3,21) + HKZ13*P(0,21) - HKZ14*P(1,21) + HKZ15*P(2,21) + HKZ9*P(18,21) + P(21,21); - const float HKZ24 = 1.0F/(HKZ10*HKZ20 - HKZ11*HKZ18 + HKZ12*HKZ21 + HKZ13*HKZ16 - HKZ14*HKZ19 + HKZ15*HKZ22 + HKZ17*HKZ9 + HKZ23 + R_MAG); - - // calculate Z axis observation jacobians - memset(Hfusion, 0, sizeof(Hfusion)); - Hfusion[0] = 2*HKZ0; - Hfusion[1] = 2*HKZ1 - 2*HKZ2 - 2*HKZ3; - Hfusion[2] = 2*HKZ4; - Hfusion[3] = 2*HKZ5; - Hfusion[16] = 2*HKZ6; - Hfusion[17] = 2*HKZ7 - 2*HKZ8; - Hfusion[18] = HKZ9; - Hfusion[21] = 1; - - // Calculate Z axis Kalman gains - if (update_all_states) { - Kfusion(0) = HKZ16*HKZ24; - Kfusion(1) = HKZ19*HKZ24; - Kfusion(2) = HKZ22*HKZ24; - Kfusion(3) = HKZ21*HKZ24; - - for (unsigned row = 4; row <= 15; row++) { - Kfusion(row) = HKZ24*(HKZ10*P(row,16) - HKZ11*P(row,17) + HKZ12*P(3,row) + HKZ13*P(0,row) - HKZ14*P(1,row) + HKZ15*P(2,row) + HKZ9*P(row,18) + P(row,21)); - } - - for (unsigned row = 22; row <= 23; row++) { - Kfusion(row) = HKZ24*(HKZ10*P(16,row) - HKZ11*P(17,row) + HKZ12*P(3,row) + HKZ13*P(0,row) - HKZ14*P(1,row) + HKZ15*P(2,row) + HKZ9*P(18,row) + P(21,row)); - } - } - - Kfusion(16) = HKZ20*HKZ24; - Kfusion(17) = HKZ18*HKZ24; - Kfusion(18) = HKZ17*HKZ24; - - for (unsigned row = 19; row <= 20; row++) { - Kfusion(row) = HKZ24*(HKZ10*P(16,row) - HKZ11*P(17,row) + HKZ12*P(3,row) + HKZ13*P(0,row) - HKZ14*P(1,row) + HKZ15*P(2,row) + HKZ9*P(18,row) + P(row,21)); - } - - Kfusion(21) = HKZ23*HKZ24; - - // save output and repeat calculation using legacy matlab generated code - float Hfusion_sympy[24]; - Vector24f Kfusion_sympy; - for (int row=0; row<24; row++) { - Hfusion_sympy[row] = Hfusion[row]; - Kfusion_sympy(row) = Kfusion(row); - } - - // repeat calculation using matlab generated equations - - // Z axis innovation variance - mag_innov_var = (P(21,21) + R_MAG + P(0,21)*SH_MAG[1] - P(1,21)*SH_MAG[2] + P(3,21)*SH_MAG[0] + P(18,21)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + (2.0f*q0*q2 + 2.0f*q1*q3)*(P(21,16) + P(0,16)*SH_MAG[1] - P(1,16)*SH_MAG[2] + P(3,16)*SH_MAG[0] + P(18,16)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P(16,16)*(2.0f*q0*q2 + 2.0f*q1*q3) - P(17,16)*(2.0f*q0*q1 - 2.0f*q2*q3) + P(2,16)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (2.0f*q0*q1 - 2.0f*q2*q3)*(P(21,17) + P(0,17)*SH_MAG[1] - P(1,17)*SH_MAG[2] + P(3,17)*SH_MAG[0] + P(18,17)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P(16,17)*(2.0f*q0*q2 + 2.0f*q1*q3) - P(17,17)*(2.0f*q0*q1 - 2.0f*q2*q3) + P(2,17)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)*(P(21,2) + P(0,2)*SH_MAG[1] - P(1,2)*SH_MAG[2] + P(3,2)*SH_MAG[0] + P(18,2)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P(16,2)*(2.0f*q0*q2 + 2.0f*q1*q3) - P(17,2)*(2.0f*q0*q1 - 2.0f*q2*q3) + P(2,2)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P(16,21)*(2.0f*q0*q2 + 2.0f*q1*q3) - P(17,21)*(2.0f*q0*q1 - 2.0f*q2*q3) + SH_MAG[1]*(P(21,0) + P(0,0)*SH_MAG[1] - P(1,0)*SH_MAG[2] + P(3,0)*SH_MAG[0] + P(18,0)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P(16,0)*(2.0f*q0*q2 + 2.0f*q1*q3) - P(17,0)*(2.0f*q0*q1 - 2.0f*q2*q3) + P(2,0)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - SH_MAG[2]*(P(21,1) + P(0,1)*SH_MAG[1] - P(1,1)*SH_MAG[2] + P(3,1)*SH_MAG[0] + P(18,1)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P(16,1)*(2.0f*q0*q2 + 2.0f*q1*q3) - P(17,1)*(2.0f*q0*q1 - 2.0f*q2*q3) + P(2,1)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[0]*(P(21,3) + P(0,3)*SH_MAG[1] - P(1,3)*SH_MAG[2] + P(3,3)*SH_MAG[0] + P(18,3)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P(16,3)*(2.0f*q0*q2 + 2.0f*q1*q3) - P(17,3)*(2.0f*q0*q1 - 2.0f*q2*q3) + P(2,3)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6])*(P(21,18) + P(0,18)*SH_MAG[1] - P(1,18)*SH_MAG[2] + P(3,18)*SH_MAG[0] + P(18,18)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P(16,18)*(2.0f*q0*q2 + 2.0f*q1*q3) - P(17,18)*(2.0f*q0*q1 - 2.0f*q2*q3) + P(2,18)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P(2,21)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)); - - // calculate Z axis observation jacobians - H_MAG.setZero(); - H_MAG(0) = SH_MAG[1]; - H_MAG(1) = -SH_MAG[2]; - H_MAG(2) = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2; - H_MAG(3) = SH_MAG[0]; - H_MAG(16) = 2.0f*q0*q2 + 2.0f*q1*q3; - H_MAG(17) = 2.0f*q2*q3 - 2.0f*q0*q1; - H_MAG(18) = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; - H_MAG(21) = 1.0f; - - // Calculate Z axis Kalman gains - float SK_MZ[5]; - SK_MZ[0] = 1.0f / mag_innov_var; - SK_MZ[1] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; - SK_MZ[2] = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2; - SK_MZ[3] = 2.0f*q0*q1 - 2.0f*q2*q3; - SK_MZ[4] = 2.0f*q0*q2 + 2.0f*q1*q3; - - if (update_all_states) { - Kfusion(0) = SK_MZ[0]*(P(0,21) + P(0,0)*SH_MAG[1] - P(0,1)*SH_MAG[2] + P(0,3)*SH_MAG[0] + P(0,2)*SK_MZ[2] + P(0,18)*SK_MZ[1] + P(0,16)*SK_MZ[4] - P(0,17)*SK_MZ[3]); - Kfusion(1) = SK_MZ[0]*(P(1,21) + P(1,0)*SH_MAG[1] - P(1,1)*SH_MAG[2] + P(1,3)*SH_MAG[0] + P(1,2)*SK_MZ[2] + P(1,18)*SK_MZ[1] + P(1,16)*SK_MZ[4] - P(1,17)*SK_MZ[3]); - Kfusion(2) = SK_MZ[0]*(P(2,21) + P(2,0)*SH_MAG[1] - P(2,1)*SH_MAG[2] + P(2,3)*SH_MAG[0] + P(2,2)*SK_MZ[2] + P(2,18)*SK_MZ[1] + P(2,16)*SK_MZ[4] - P(2,17)*SK_MZ[3]); - Kfusion(3) = SK_MZ[0]*(P(3,21) + P(3,0)*SH_MAG[1] - P(3,1)*SH_MAG[2] + P(3,3)*SH_MAG[0] + P(3,2)*SK_MZ[2] + P(3,18)*SK_MZ[1] + P(3,16)*SK_MZ[4] - P(3,17)*SK_MZ[3]); - Kfusion(4) = SK_MZ[0]*(P(4,21) + P(4,0)*SH_MAG[1] - P(4,1)*SH_MAG[2] + P(4,3)*SH_MAG[0] + P(4,2)*SK_MZ[2] + P(4,18)*SK_MZ[1] + P(4,16)*SK_MZ[4] - P(4,17)*SK_MZ[3]); - Kfusion(5) = SK_MZ[0]*(P(5,21) + P(5,0)*SH_MAG[1] - P(5,1)*SH_MAG[2] + P(5,3)*SH_MAG[0] + P(5,2)*SK_MZ[2] + P(5,18)*SK_MZ[1] + P(5,16)*SK_MZ[4] - P(5,17)*SK_MZ[3]); - Kfusion(6) = SK_MZ[0]*(P(6,21) + P(6,0)*SH_MAG[1] - P(6,1)*SH_MAG[2] + P(6,3)*SH_MAG[0] + P(6,2)*SK_MZ[2] + P(6,18)*SK_MZ[1] + P(6,16)*SK_MZ[4] - P(6,17)*SK_MZ[3]); - Kfusion(7) = SK_MZ[0]*(P(7,21) + P(7,0)*SH_MAG[1] - P(7,1)*SH_MAG[2] + P(7,3)*SH_MAG[0] + P(7,2)*SK_MZ[2] + P(7,18)*SK_MZ[1] + P(7,16)*SK_MZ[4] - P(7,17)*SK_MZ[3]); - Kfusion(8) = SK_MZ[0]*(P(8,21) + P(8,0)*SH_MAG[1] - P(8,1)*SH_MAG[2] + P(8,3)*SH_MAG[0] + P(8,2)*SK_MZ[2] + P(8,18)*SK_MZ[1] + P(8,16)*SK_MZ[4] - P(8,17)*SK_MZ[3]); - Kfusion(9) = SK_MZ[0]*(P(9,21) + P(9,0)*SH_MAG[1] - P(9,1)*SH_MAG[2] + P(9,3)*SH_MAG[0] + P(9,2)*SK_MZ[2] + P(9,18)*SK_MZ[1] + P(9,16)*SK_MZ[4] - P(9,17)*SK_MZ[3]); - Kfusion(10) = SK_MZ[0]*(P(10,21) + P(10,0)*SH_MAG[1] - P(10,1)*SH_MAG[2] + P(10,3)*SH_MAG[0] + P(10,2)*SK_MZ[2] + P(10,18)*SK_MZ[1] + P(10,16)*SK_MZ[4] - P(10,17)*SK_MZ[3]); - Kfusion(11) = SK_MZ[0]*(P(11,21) + P(11,0)*SH_MAG[1] - P(11,1)*SH_MAG[2] + P(11,3)*SH_MAG[0] + P(11,2)*SK_MZ[2] + P(11,18)*SK_MZ[1] + P(11,16)*SK_MZ[4] - P(11,17)*SK_MZ[3]); - Kfusion(12) = SK_MZ[0]*(P(12,21) + P(12,0)*SH_MAG[1] - P(12,1)*SH_MAG[2] + P(12,3)*SH_MAG[0] + P(12,2)*SK_MZ[2] + P(12,18)*SK_MZ[1] + P(12,16)*SK_MZ[4] - P(12,17)*SK_MZ[3]); - Kfusion(13) = SK_MZ[0]*(P(13,21) + P(13,0)*SH_MAG[1] - P(13,1)*SH_MAG[2] + P(13,3)*SH_MAG[0] + P(13,2)*SK_MZ[2] + P(13,18)*SK_MZ[1] + P(13,16)*SK_MZ[4] - P(13,17)*SK_MZ[3]); - Kfusion(14) = SK_MZ[0]*(P(14,21) + P(14,0)*SH_MAG[1] - P(14,1)*SH_MAG[2] + P(14,3)*SH_MAG[0] + P(14,2)*SK_MZ[2] + P(14,18)*SK_MZ[1] + P(14,16)*SK_MZ[4] - P(14,17)*SK_MZ[3]); - Kfusion(15) = SK_MZ[0]*(P(15,21) + P(15,0)*SH_MAG[1] - P(15,1)*SH_MAG[2] + P(15,3)*SH_MAG[0] + P(15,2)*SK_MZ[2] + P(15,18)*SK_MZ[1] + P(15,16)*SK_MZ[4] - P(15,17)*SK_MZ[3]); - Kfusion(22) = SK_MZ[0]*(P(22,21) + P(22,0)*SH_MAG[1] - P(22,1)*SH_MAG[2] + P(22,3)*SH_MAG[0] + P(22,2)*SK_MZ[2] + P(22,18)*SK_MZ[1] + P(22,16)*SK_MZ[4] - P(22,17)*SK_MZ[3]); - Kfusion(23) = SK_MZ[0]*(P(23,21) + P(23,0)*SH_MAG[1] - P(23,1)*SH_MAG[2] + P(23,3)*SH_MAG[0] + P(23,2)*SK_MZ[2] + P(23,18)*SK_MZ[1] + P(23,16)*SK_MZ[4] - P(23,17)*SK_MZ[3]); - } - - Kfusion(16) = SK_MZ[0]*(P(16,21) + P(16,0)*SH_MAG[1] - P(16,1)*SH_MAG[2] + P(16,3)*SH_MAG[0] + P(16,2)*SK_MZ[2] + P(16,18)*SK_MZ[1] + P(16,16)*SK_MZ[4] - P(16,17)*SK_MZ[3]); - Kfusion(17) = SK_MZ[0]*(P(17,21) + P(17,0)*SH_MAG[1] - P(17,1)*SH_MAG[2] + P(17,3)*SH_MAG[0] + P(17,2)*SK_MZ[2] + P(17,18)*SK_MZ[1] + P(17,16)*SK_MZ[4] - P(17,17)*SK_MZ[3]); - Kfusion(18) = SK_MZ[0]*(P(18,21) + P(18,0)*SH_MAG[1] - P(18,1)*SH_MAG[2] + P(18,3)*SH_MAG[0] + P(18,2)*SK_MZ[2] + P(18,18)*SK_MZ[1] + P(18,16)*SK_MZ[4] - P(18,17)*SK_MZ[3]); - Kfusion(19) = SK_MZ[0]*(P(19,21) + P(19,0)*SH_MAG[1] - P(19,1)*SH_MAG[2] + P(19,3)*SH_MAG[0] + P(19,2)*SK_MZ[2] + P(19,18)*SK_MZ[1] + P(19,16)*SK_MZ[4] - P(19,17)*SK_MZ[3]); - Kfusion(20) = SK_MZ[0]*(P(20,21) + P(20,0)*SH_MAG[1] - P(20,1)*SH_MAG[2] + P(20,3)*SH_MAG[0] + P(20,2)*SK_MZ[2] + P(20,18)*SK_MZ[1] + P(20,16)*SK_MZ[4] - P(20,17)*SK_MZ[3]); - Kfusion(21) = SK_MZ[0]*(P(21,21) + P(21,0)*SH_MAG[1] - P(21,1)*SH_MAG[2] + P(21,3)*SH_MAG[0] + P(21,2)*SK_MZ[2] + P(21,18)*SK_MZ[1] + P(21,16)*SK_MZ[4] - P(21,17)*SK_MZ[3]); - - // find largest observation variance difference as a fraction of the matlab value - float max_diff_fraction = 0.0f; - int max_row; - float max_old, max_new; - for (int row=0; row<24; row++) { - float diff_fraction; - if (H_MAG(row) != 0.0f) { - diff_fraction = fabsf(Hfusion_sympy[row] - H_MAG(row)) / fabsf(H_MAG(row)); - } else if (Hfusion_sympy[row] != 0.0f) { - diff_fraction = fabsf(Hfusion_sympy[row] - H_MAG(row)) / fabsf(Hfusion_sympy[row]); - } else { - diff_fraction = 0.0f; - } - if (diff_fraction > max_diff_fraction) { - max_diff_fraction = diff_fraction; - max_row = row; - max_old = H_MAG(row); - max_new = Hfusion_sympy[row]; - } - } - - if (max_diff_fraction > 1e-5f) { - printf("Fail: Magnetomer Z axis Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row); - } else { - printf("Pass: Magnetomer Z axis Hfusion max diff fraction = %e\n",max_diff_fraction); - } - - // find largest Kalman gain difference as a fraction of the matlab value - max_diff_fraction = 0.0f; - for (int row=0; row<4; row++) { - float diff_fraction; - if (Kfusion(row) != 0.0f) { - diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion(row)) / fabsf(Kfusion(row)); - } else if (Kfusion_sympy(row) != 0.0f) { - diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion(row)) / fabsf(Kfusion_sympy(row)); - } else { - diff_fraction = 0.0f; - } - if (diff_fraction > max_diff_fraction) { - max_diff_fraction = diff_fraction; - max_row = row; - max_old = Kfusion(row); - max_new = Kfusion_sympy(row); - } - } - - if (max_diff_fraction > 1e-5f) { - printf("Fail: Magnetomer Z axis Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row); - } else { - printf("Pass: Magnetomer Z axis Kfusion max diff fraction = %e\n",max_diff_fraction); - } - - } - - return 0; -} diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated.cpp deleted file mode 100644 index ca880c9570..0000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated.cpp +++ /dev/null @@ -1,210 +0,0 @@ -// Axis 0 equations -// Sub Expressions -const float HKX0 = -magD*q2 + magE*q3 + magN*q0; -const float HKX1 = magD*q3 + magE*q2 + magN*q1; -const float HKX2 = magD*q0 - magE*q1 + magN*q2; -const float HKX3 = magD*q1 + magE*q0 - magN*q3; -const float HKX4 = (q0)*(q0) + (q1)*(q1) - (q2)*(q2) - (q3)*(q3); -const float HKX5 = q0*q3 + q1*q2; -const float HKX6 = q0*q2 - q1*q3; -const float HKX7 = 2*HKX5; -const float HKX8 = 2*HKX6; -const float HKX9 = 2*HKX1; -const float HKX10 = 2*HKX0; -const float HKX11 = 2*HKX2; -const float HKX12 = 2*HKX3; -const float HKX13 = HKX10*P(0,0) - HKX11*P(0,2) + HKX12*P(0,3) + HKX4*P(0,16) + HKX7*P(0,17) - HKX8*P(0,18) + HKX9*P(0,1) + P(0,19); -const float HKX14 = HKX10*P(0,16) - HKX11*P(2,16) + HKX12*P(3,16) + HKX4*P(16,16) + HKX7*P(16,17) - HKX8*P(16,18) + HKX9*P(1,16) + P(16,19); -const float HKX15 = HKX10*P(0,18) - HKX11*P(2,18) + HKX12*P(3,18) + HKX4*P(16,18) + HKX7*P(17,18) - HKX8*P(18,18) + HKX9*P(1,18) + P(18,19); -const float HKX16 = HKX10*P(0,2) - HKX11*P(2,2) + HKX12*P(2,3) + HKX4*P(2,16) + HKX7*P(2,17) - HKX8*P(2,18) + HKX9*P(1,2) + P(2,19); -const float HKX17 = HKX10*P(0,17) - HKX11*P(2,17) + HKX12*P(3,17) + HKX4*P(16,17) + HKX7*P(17,17) - HKX8*P(17,18) + HKX9*P(1,17) + P(17,19); -const float HKX18 = HKX10*P(0,3) - HKX11*P(2,3) + HKX12*P(3,3) + HKX4*P(3,16) + HKX7*P(3,17) - HKX8*P(3,18) + HKX9*P(1,3) + P(3,19); -const float HKX19 = HKX10*P(0,1) - HKX11*P(1,2) + HKX12*P(1,3) + HKX4*P(1,16) + HKX7*P(1,17) - HKX8*P(1,18) + HKX9*P(1,1) + P(1,19); -const float HKX20 = HKX10*P(0,19) - HKX11*P(2,19) + HKX12*P(3,19) + HKX4*P(16,19) + HKX7*P(17,19) - HKX8*P(18,19) + HKX9*P(1,19) + P(19,19); -const float HKX21 = 1.0F/(HKX10*HKX13 - HKX11*HKX16 + HKX12*HKX18 + HKX14*HKX4 - HKX15*HKX8 + HKX17*HKX7 + HKX19*HKX9 + HKX20 + R_MAG); - - -// Observation Jacobians -Hfusion.at<0>() = 2*HKX0; -Hfusion.at<1>() = 2*HKX1; -Hfusion.at<2>() = -2*HKX2; -Hfusion.at<3>() = 2*HKX3; -Hfusion.at<16>() = HKX4; -Hfusion.at<17>() = 2*HKX5; -Hfusion.at<18>() = -2*HKX6; -Hfusion.at<19>() = 1; - - -// Kalman gains -Kfusion(0) = HKX13*HKX21; -Kfusion(1) = HKX19*HKX21; -Kfusion(2) = HKX16*HKX21; -Kfusion(3) = HKX18*HKX21; -Kfusion(4) = HKX21*(HKX10*P(0,4) - HKX11*P(2,4) + HKX12*P(3,4) + HKX4*P(4,16) + HKX7*P(4,17) - HKX8*P(4,18) + HKX9*P(1,4) + P(4,19)); -Kfusion(5) = HKX21*(HKX10*P(0,5) - HKX11*P(2,5) + HKX12*P(3,5) + HKX4*P(5,16) + HKX7*P(5,17) - HKX8*P(5,18) + HKX9*P(1,5) + P(5,19)); -Kfusion(6) = HKX21*(HKX10*P(0,6) - HKX11*P(2,6) + HKX12*P(3,6) + HKX4*P(6,16) + HKX7*P(6,17) - HKX8*P(6,18) + HKX9*P(1,6) + P(6,19)); -Kfusion(7) = HKX21*(HKX10*P(0,7) - HKX11*P(2,7) + HKX12*P(3,7) + HKX4*P(7,16) + HKX7*P(7,17) - HKX8*P(7,18) + HKX9*P(1,7) + P(7,19)); -Kfusion(8) = HKX21*(HKX10*P(0,8) - HKX11*P(2,8) + HKX12*P(3,8) + HKX4*P(8,16) + HKX7*P(8,17) - HKX8*P(8,18) + HKX9*P(1,8) + P(8,19)); -Kfusion(9) = HKX21*(HKX10*P(0,9) - HKX11*P(2,9) + HKX12*P(3,9) + HKX4*P(9,16) + HKX7*P(9,17) - HKX8*P(9,18) + HKX9*P(1,9) + P(9,19)); -Kfusion(10) = HKX21*(HKX10*P(0,10) - HKX11*P(2,10) + HKX12*P(3,10) + HKX4*P(10,16) + HKX7*P(10,17) - HKX8*P(10,18) + HKX9*P(1,10) + P(10,19)); -Kfusion(11) = HKX21*(HKX10*P(0,11) - HKX11*P(2,11) + HKX12*P(3,11) + HKX4*P(11,16) + HKX7*P(11,17) - HKX8*P(11,18) + HKX9*P(1,11) + P(11,19)); -Kfusion(12) = HKX21*(HKX10*P(0,12) - HKX11*P(2,12) + HKX12*P(3,12) + HKX4*P(12,16) + HKX7*P(12,17) - HKX8*P(12,18) + HKX9*P(1,12) + P(12,19)); -Kfusion(13) = HKX21*(HKX10*P(0,13) - HKX11*P(2,13) + HKX12*P(3,13) + HKX4*P(13,16) + HKX7*P(13,17) - HKX8*P(13,18) + HKX9*P(1,13) + P(13,19)); -Kfusion(14) = HKX21*(HKX10*P(0,14) - HKX11*P(2,14) + HKX12*P(3,14) + HKX4*P(14,16) + HKX7*P(14,17) - HKX8*P(14,18) + HKX9*P(1,14) + P(14,19)); -Kfusion(15) = HKX21*(HKX10*P(0,15) - HKX11*P(2,15) + HKX12*P(3,15) + HKX4*P(15,16) + HKX7*P(15,17) - HKX8*P(15,18) + HKX9*P(1,15) + P(15,19)); -Kfusion(16) = HKX14*HKX21; -Kfusion(17) = HKX17*HKX21; -Kfusion(18) = HKX15*HKX21; -Kfusion(19) = HKX20*HKX21; -Kfusion(20) = HKX21*(HKX10*P(0,20) - HKX11*P(2,20) + HKX12*P(3,20) + HKX4*P(16,20) + HKX7*P(17,20) - HKX8*P(18,20) + HKX9*P(1,20) + P(19,20)); -Kfusion(21) = HKX21*(HKX10*P(0,21) - HKX11*P(2,21) + HKX12*P(3,21) + HKX4*P(16,21) + HKX7*P(17,21) - HKX8*P(18,21) + HKX9*P(1,21) + P(19,21)); -Kfusion(22) = HKX21*(HKX10*P(0,22) - HKX11*P(2,22) + HKX12*P(3,22) + HKX4*P(16,22) + HKX7*P(17,22) - HKX8*P(18,22) + HKX9*P(1,22) + P(19,22)); -Kfusion(23) = HKX21*(HKX10*P(0,23) - HKX11*P(2,23) + HKX12*P(3,23) + HKX4*P(16,23) + HKX7*P(17,23) - HKX8*P(18,23) + HKX9*P(1,23) + P(19,23)); - - -// Predicted observation - - -// Innovation variance - - -// Axis 1 equations -// Sub Expressions -const float HKY0 = magD*q1 + magE*q0 - magN*q3; -const float HKY1 = magD*q0 - magE*q1 + magN*q2; -const float HKY2 = magD*q3 + magE*q2 + magN*q1; -const float HKY3 = -magD*q2 + magE*q3 + magN*q0; -const float HKY4 = q0*q3 - q1*q2; -const float HKY5 = (q0)*(q0) - (q1)*(q1) + (q2)*(q2) - (q3)*(q3); -const float HKY6 = q0*q1 + q2*q3; -const float HKY7 = 2*HKY6; -const float HKY8 = 2*HKY4; -const float HKY9 = 2*HKY2; -const float HKY10 = 2*HKY0; -const float HKY11 = 2*HKY1; -const float HKY12 = 2*HKY3; -const float HKY13 = HKY10*P(0,0) + HKY11*P(0,1) - HKY12*P(0,3) + HKY5*P(0,17) + HKY7*P(0,18) - HKY8*P(0,16) + HKY9*P(0,2) + P(0,20); -const float HKY14 = HKY10*P(0,17) + HKY11*P(1,17) - HKY12*P(3,17) + HKY5*P(17,17) + HKY7*P(17,18) - HKY8*P(16,17) + HKY9*P(2,17) + P(17,20); -const float HKY15 = HKY10*P(0,16) + HKY11*P(1,16) - HKY12*P(3,16) + HKY5*P(16,17) + HKY7*P(16,18) - HKY8*P(16,16) + HKY9*P(2,16) + P(16,20); -const float HKY16 = HKY10*P(0,3) + HKY11*P(1,3) - HKY12*P(3,3) + HKY5*P(3,17) + HKY7*P(3,18) - HKY8*P(3,16) + HKY9*P(2,3) + P(3,20); -const float HKY17 = HKY10*P(0,18) + HKY11*P(1,18) - HKY12*P(3,18) + HKY5*P(17,18) + HKY7*P(18,18) - HKY8*P(16,18) + HKY9*P(2,18) + P(18,20); -const float HKY18 = HKY10*P(0,1) + HKY11*P(1,1) - HKY12*P(1,3) + HKY5*P(1,17) + HKY7*P(1,18) - HKY8*P(1,16) + HKY9*P(1,2) + P(1,20); -const float HKY19 = HKY10*P(0,2) + HKY11*P(1,2) - HKY12*P(2,3) + HKY5*P(2,17) + HKY7*P(2,18) - HKY8*P(2,16) + HKY9*P(2,2) + P(2,20); -const float HKY20 = HKY10*P(0,20) + HKY11*P(1,20) - HKY12*P(3,20) + HKY5*P(17,20) + HKY7*P(18,20) - HKY8*P(16,20) + HKY9*P(2,20) + P(20,20); -const float HKY21 = 1.0F/(HKY10*HKY13 + HKY11*HKY18 - HKY12*HKY16 + HKY14*HKY5 - HKY15*HKY8 + HKY17*HKY7 + HKY19*HKY9 + HKY20 + R_MAG); - - -// Observation Jacobians -Hfusion.at<0>() = 2*HKY0; -Hfusion.at<1>() = 2*HKY1; -Hfusion.at<2>() = 2*HKY2; -Hfusion.at<3>() = -2*HKY3; -Hfusion.at<16>() = -2*HKY4; -Hfusion.at<17>() = HKY5; -Hfusion.at<18>() = 2*HKY6; -Hfusion.at<20>() = 1; - - -// Kalman gains -Kfusion(0) = HKY13*HKY21; -Kfusion(1) = HKY18*HKY21; -Kfusion(2) = HKY19*HKY21; -Kfusion(3) = HKY16*HKY21; -Kfusion(4) = HKY21*(HKY10*P(0,4) + HKY11*P(1,4) - HKY12*P(3,4) + HKY5*P(4,17) + HKY7*P(4,18) - HKY8*P(4,16) + HKY9*P(2,4) + P(4,20)); -Kfusion(5) = HKY21*(HKY10*P(0,5) + HKY11*P(1,5) - HKY12*P(3,5) + HKY5*P(5,17) + HKY7*P(5,18) - HKY8*P(5,16) + HKY9*P(2,5) + P(5,20)); -Kfusion(6) = HKY21*(HKY10*P(0,6) + HKY11*P(1,6) - HKY12*P(3,6) + HKY5*P(6,17) + HKY7*P(6,18) - HKY8*P(6,16) + HKY9*P(2,6) + P(6,20)); -Kfusion(7) = HKY21*(HKY10*P(0,7) + HKY11*P(1,7) - HKY12*P(3,7) + HKY5*P(7,17) + HKY7*P(7,18) - HKY8*P(7,16) + HKY9*P(2,7) + P(7,20)); -Kfusion(8) = HKY21*(HKY10*P(0,8) + HKY11*P(1,8) - HKY12*P(3,8) + HKY5*P(8,17) + HKY7*P(8,18) - HKY8*P(8,16) + HKY9*P(2,8) + P(8,20)); -Kfusion(9) = HKY21*(HKY10*P(0,9) + HKY11*P(1,9) - HKY12*P(3,9) + HKY5*P(9,17) + HKY7*P(9,18) - HKY8*P(9,16) + HKY9*P(2,9) + P(9,20)); -Kfusion(10) = HKY21*(HKY10*P(0,10) + HKY11*P(1,10) - HKY12*P(3,10) + HKY5*P(10,17) + HKY7*P(10,18) - HKY8*P(10,16) + HKY9*P(2,10) + P(10,20)); -Kfusion(11) = HKY21*(HKY10*P(0,11) + HKY11*P(1,11) - HKY12*P(3,11) + HKY5*P(11,17) + HKY7*P(11,18) - HKY8*P(11,16) + HKY9*P(2,11) + P(11,20)); -Kfusion(12) = HKY21*(HKY10*P(0,12) + HKY11*P(1,12) - HKY12*P(3,12) + HKY5*P(12,17) + HKY7*P(12,18) - HKY8*P(12,16) + HKY9*P(2,12) + P(12,20)); -Kfusion(13) = HKY21*(HKY10*P(0,13) + HKY11*P(1,13) - HKY12*P(3,13) + HKY5*P(13,17) + HKY7*P(13,18) - HKY8*P(13,16) + HKY9*P(2,13) + P(13,20)); -Kfusion(14) = HKY21*(HKY10*P(0,14) + HKY11*P(1,14) - HKY12*P(3,14) + HKY5*P(14,17) + HKY7*P(14,18) - HKY8*P(14,16) + HKY9*P(2,14) + P(14,20)); -Kfusion(15) = HKY21*(HKY10*P(0,15) + HKY11*P(1,15) - HKY12*P(3,15) + HKY5*P(15,17) + HKY7*P(15,18) - HKY8*P(15,16) + HKY9*P(2,15) + P(15,20)); -Kfusion(16) = HKY15*HKY21; -Kfusion(17) = HKY14*HKY21; -Kfusion(18) = HKY17*HKY21; -Kfusion(19) = HKY21*(HKY10*P(0,19) + HKY11*P(1,19) - HKY12*P(3,19) + HKY5*P(17,19) + HKY7*P(18,19) - HKY8*P(16,19) + HKY9*P(2,19) + P(19,20)); -Kfusion(20) = HKY20*HKY21; -Kfusion(21) = HKY21*(HKY10*P(0,21) + HKY11*P(1,21) - HKY12*P(3,21) + HKY5*P(17,21) + HKY7*P(18,21) - HKY8*P(16,21) + HKY9*P(2,21) + P(20,21)); -Kfusion(22) = HKY21*(HKY10*P(0,22) + HKY11*P(1,22) - HKY12*P(3,22) + HKY5*P(17,22) + HKY7*P(18,22) - HKY8*P(16,22) + HKY9*P(2,22) + P(20,22)); -Kfusion(23) = HKY21*(HKY10*P(0,23) + HKY11*P(1,23) - HKY12*P(3,23) + HKY5*P(17,23) + HKY7*P(18,23) - HKY8*P(16,23) + HKY9*P(2,23) + P(20,23)); - - -// Predicted observation - - -// Innovation variance - - -// Axis 2 equations -// Sub Expressions -const float HKZ0 = magD*q0 - magE*q1 + magN*q2; -const float HKZ1 = magD*q1 + magE*q0 - magN*q3; -const float HKZ2 = -magD*q2 + magE*q3 + magN*q0; -const float HKZ3 = magD*q3 + magE*q2 + magN*q1; -const float HKZ4 = q0*q2 + q1*q3; -const float HKZ5 = q0*q1 - q2*q3; -const float HKZ6 = (q0)*(q0) - (q1)*(q1) - (q2)*(q2) + (q3)*(q3); -const float HKZ7 = 2*HKZ4; -const float HKZ8 = 2*HKZ5; -const float HKZ9 = 2*HKZ3; -const float HKZ10 = 2*HKZ0; -const float HKZ11 = 2*HKZ1; -const float HKZ12 = 2*HKZ2; -const float HKZ13 = HKZ10*P(0,0) - HKZ11*P(0,1) + HKZ12*P(0,2) + HKZ6*P(0,18) + HKZ7*P(0,16) - HKZ8*P(0,17) + HKZ9*P(0,3) + P(0,21); -const float HKZ14 = HKZ10*P(0,18) - HKZ11*P(1,18) + HKZ12*P(2,18) + HKZ6*P(18,18) + HKZ7*P(16,18) - HKZ8*P(17,18) + HKZ9*P(3,18) + P(18,21); -const float HKZ15 = HKZ10*P(0,17) - HKZ11*P(1,17) + HKZ12*P(2,17) + HKZ6*P(17,18) + HKZ7*P(16,17) - HKZ8*P(17,17) + HKZ9*P(3,17) + P(17,21); -const float HKZ16 = HKZ10*P(0,1) - HKZ11*P(1,1) + HKZ12*P(1,2) + HKZ6*P(1,18) + HKZ7*P(1,16) - HKZ8*P(1,17) + HKZ9*P(1,3) + P(1,21); -const float HKZ17 = HKZ10*P(0,16) - HKZ11*P(1,16) + HKZ12*P(2,16) + HKZ6*P(16,18) + HKZ7*P(16,16) - HKZ8*P(16,17) + HKZ9*P(3,16) + P(16,21); -const float HKZ18 = HKZ10*P(0,3) - HKZ11*P(1,3) + HKZ12*P(2,3) + HKZ6*P(3,18) + HKZ7*P(3,16) - HKZ8*P(3,17) + HKZ9*P(3,3) + P(3,21); -const float HKZ19 = HKZ10*P(0,2) - HKZ11*P(1,2) + HKZ12*P(2,2) + HKZ6*P(2,18) + HKZ7*P(2,16) - HKZ8*P(2,17) + HKZ9*P(2,3) + P(2,21); -const float HKZ20 = HKZ10*P(0,21) - HKZ11*P(1,21) + HKZ12*P(2,21) + HKZ6*P(18,21) + HKZ7*P(16,21) - HKZ8*P(17,21) + HKZ9*P(3,21) + P(21,21); -const float HKZ21 = 1.0F/(HKZ10*HKZ13 - HKZ11*HKZ16 + HKZ12*HKZ19 + HKZ14*HKZ6 - HKZ15*HKZ8 + HKZ17*HKZ7 + HKZ18*HKZ9 + HKZ20 + R_MAG); - - -// Observation Jacobians -Hfusion.at<0>() = 2*HKZ0; -Hfusion.at<1>() = -2*HKZ1; -Hfusion.at<2>() = 2*HKZ2; -Hfusion.at<3>() = 2*HKZ3; -Hfusion.at<16>() = 2*HKZ4; -Hfusion.at<17>() = -2*HKZ5; -Hfusion.at<18>() = HKZ6; -Hfusion.at<21>() = 1; - - -// Kalman gains -Kfusion(0) = HKZ13*HKZ21; -Kfusion(1) = HKZ16*HKZ21; -Kfusion(2) = HKZ19*HKZ21; -Kfusion(3) = HKZ18*HKZ21; -Kfusion(4) = HKZ21*(HKZ10*P(0,4) - HKZ11*P(1,4) + HKZ12*P(2,4) + HKZ6*P(4,18) + HKZ7*P(4,16) - HKZ8*P(4,17) + HKZ9*P(3,4) + P(4,21)); -Kfusion(5) = HKZ21*(HKZ10*P(0,5) - HKZ11*P(1,5) + HKZ12*P(2,5) + HKZ6*P(5,18) + HKZ7*P(5,16) - HKZ8*P(5,17) + HKZ9*P(3,5) + P(5,21)); -Kfusion(6) = HKZ21*(HKZ10*P(0,6) - HKZ11*P(1,6) + HKZ12*P(2,6) + HKZ6*P(6,18) + HKZ7*P(6,16) - HKZ8*P(6,17) + HKZ9*P(3,6) + P(6,21)); -Kfusion(7) = HKZ21*(HKZ10*P(0,7) - HKZ11*P(1,7) + HKZ12*P(2,7) + HKZ6*P(7,18) + HKZ7*P(7,16) - HKZ8*P(7,17) + HKZ9*P(3,7) + P(7,21)); -Kfusion(8) = HKZ21*(HKZ10*P(0,8) - HKZ11*P(1,8) + HKZ12*P(2,8) + HKZ6*P(8,18) + HKZ7*P(8,16) - HKZ8*P(8,17) + HKZ9*P(3,8) + P(8,21)); -Kfusion(9) = HKZ21*(HKZ10*P(0,9) - HKZ11*P(1,9) + HKZ12*P(2,9) + HKZ6*P(9,18) + HKZ7*P(9,16) - HKZ8*P(9,17) + HKZ9*P(3,9) + P(9,21)); -Kfusion(10) = HKZ21*(HKZ10*P(0,10) - HKZ11*P(1,10) + HKZ12*P(2,10) + HKZ6*P(10,18) + HKZ7*P(10,16) - HKZ8*P(10,17) + HKZ9*P(3,10) + P(10,21)); -Kfusion(11) = HKZ21*(HKZ10*P(0,11) - HKZ11*P(1,11) + HKZ12*P(2,11) + HKZ6*P(11,18) + HKZ7*P(11,16) - HKZ8*P(11,17) + HKZ9*P(3,11) + P(11,21)); -Kfusion(12) = HKZ21*(HKZ10*P(0,12) - HKZ11*P(1,12) + HKZ12*P(2,12) + HKZ6*P(12,18) + HKZ7*P(12,16) - HKZ8*P(12,17) + HKZ9*P(3,12) + P(12,21)); -Kfusion(13) = HKZ21*(HKZ10*P(0,13) - HKZ11*P(1,13) + HKZ12*P(2,13) + HKZ6*P(13,18) + HKZ7*P(13,16) - HKZ8*P(13,17) + HKZ9*P(3,13) + P(13,21)); -Kfusion(14) = HKZ21*(HKZ10*P(0,14) - HKZ11*P(1,14) + HKZ12*P(2,14) + HKZ6*P(14,18) + HKZ7*P(14,16) - HKZ8*P(14,17) + HKZ9*P(3,14) + P(14,21)); -Kfusion(15) = HKZ21*(HKZ10*P(0,15) - HKZ11*P(1,15) + HKZ12*P(2,15) + HKZ6*P(15,18) + HKZ7*P(15,16) - HKZ8*P(15,17) + HKZ9*P(3,15) + P(15,21)); -Kfusion(16) = HKZ17*HKZ21; -Kfusion(17) = HKZ15*HKZ21; -Kfusion(18) = HKZ14*HKZ21; -Kfusion(19) = HKZ21*(HKZ10*P(0,19) - HKZ11*P(1,19) + HKZ12*P(2,19) + HKZ6*P(18,19) + HKZ7*P(16,19) - HKZ8*P(17,19) + HKZ9*P(3,19) + P(19,21)); -Kfusion(20) = HKZ21*(HKZ10*P(0,20) - HKZ11*P(1,20) + HKZ12*P(2,20) + HKZ6*P(18,20) + HKZ7*P(16,20) - HKZ8*P(17,20) + HKZ9*P(3,20) + P(20,21)); -Kfusion(21) = HKZ20*HKZ21; -Kfusion(22) = HKZ21*(HKZ10*P(0,22) - HKZ11*P(1,22) + HKZ12*P(2,22) + HKZ6*P(18,22) + HKZ7*P(16,22) - HKZ8*P(17,22) + HKZ9*P(3,22) + P(21,22)); -Kfusion(23) = HKZ21*(HKZ10*P(0,23) - HKZ11*P(1,23) + HKZ12*P(2,23) + HKZ6*P(18,23) + HKZ7*P(16,23) - HKZ8*P(17,23) + HKZ9*P(3,23) + P(21,23)); - - -// Predicted observation - - -// Innovation variance - - diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated_alt.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated_alt.cpp deleted file mode 100644 index 8335eaf49c..0000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated_alt.cpp +++ /dev/null @@ -1,193 +0,0 @@ -// Sub Expressions -const float HK0 = -magD*q2 + magE*q3 + magN*q0; -const float HK1 = 2*HK0; -const float HK2 = magD*q3 + magE*q2 + magN*q1; -const float HK3 = 2*HK2; -const float HK4 = magD*q0 - magE*q1 + magN*q2; -const float HK5 = magD*q1 + magE*q0 - magN*q3; -const float HK6 = 2*HK5; -const float HK7 = (q1)*(q1); -const float HK8 = (q2)*(q2); -const float HK9 = -HK8; -const float HK10 = (q0)*(q0); -const float HK11 = (q3)*(q3); -const float HK12 = HK10 - HK11; -const float HK13 = HK12 + HK7 + HK9; -const float HK14 = q0*q3; -const float HK15 = HK14 + q1*q2; -const float HK16 = q0*q2; -const float HK17 = HK16 - q1*q3; -const float HK18 = 2*HK15; -const float HK19 = 2*HK17; -const float HK20 = 2*HK2; -const float HK21 = 2*HK0; -const float HK22 = 2*HK4; -const float HK23 = HK22*P(0,2); -const float HK24 = 2*HK5; -const float HK25 = HK24*P(0,3); -const float HK26 = HK13*P(0,16) + HK18*P(0,17) - HK19*P(0,18) + HK20*P(0,1) + HK21*P(0,0) - HK23 + HK25 + P(0,19); -const float HK27 = HK13*P(16,16) + HK18*P(16,17) - HK19*P(16,18) + HK20*P(1,16) + HK21*P(0,16) - HK22*P(2,16) + HK24*P(3,16) + P(16,19); -const float HK28 = HK13*P(16,18) + HK18*P(17,18) - HK19*P(18,18) + HK20*P(1,18) + HK21*P(0,18) - HK22*P(2,18) + HK24*P(3,18) + P(18,19); -const float HK29 = HK20*P(1,2); -const float HK30 = HK21*P(0,2); -const float HK31 = HK13*P(2,16) + HK18*P(2,17) - HK19*P(2,18) - HK22*P(2,2) + HK24*P(2,3) + HK29 + HK30 + P(2,19); -const float HK32 = HK13*P(16,17) + HK18*P(17,17) - HK19*P(17,18) + HK20*P(1,17) + HK21*P(0,17) - HK22*P(2,17) + HK24*P(3,17) + P(17,19); -const float HK33 = HK20*P(1,3); -const float HK34 = HK21*P(0,3); -const float HK35 = HK13*P(3,16) + HK18*P(3,17) - HK19*P(3,18) - HK22*P(2,3) + HK24*P(3,3) + HK33 + HK34 + P(3,19); -const float HK36 = HK22*P(1,2); -const float HK37 = HK24*P(1,3); -const float HK38 = HK13*P(1,16) + HK18*P(1,17) - HK19*P(1,18) + HK20*P(1,1) + HK21*P(0,1) - HK36 + HK37 + P(1,19); -const float HK39 = HK13*P(16,19) + HK18*P(17,19) - HK19*P(18,19) + HK20*P(1,19) + HK21*P(0,19) - HK22*P(2,19) + HK24*P(3,19) + P(19,19); -const float HK40 = 1.0F/(HK13*HK27 + HK18*HK32 - HK19*HK28 + HK20*HK38 + HK21*HK26 - HK22*HK31 + HK24*HK35 + HK39 + R_MAG); -const float HK41 = 2*HK4; -const float HK42 = HK14 - q1*q2; -const float HK43 = -HK7; -const float HK44 = HK12 + HK43 + HK8; -const float HK45 = q0*q1; -const float HK46 = HK45 + q2*q3; -const float HK47 = 2*HK46; -const float HK48 = 2*HK42; -const float HK49 = HK22*P(0,1); -const float HK50 = HK20*P(0,2) + HK24*P(0,0) - HK34 + HK44*P(0,17) + HK47*P(0,18) - HK48*P(0,16) + HK49 + P(0,20); -const float HK51 = HK20*P(2,17) - HK21*P(3,17) + HK22*P(1,17) + HK24*P(0,17) + HK44*P(17,17) + HK47*P(17,18) - HK48*P(16,17) + P(17,20); -const float HK52 = HK20*P(2,16) - HK21*P(3,16) + HK22*P(1,16) + HK24*P(0,16) + HK44*P(16,17) + HK47*P(16,18) - HK48*P(16,16) + P(16,20); -const float HK53 = HK20*P(2,3); -const float HK54 = -HK21*P(3,3) + HK22*P(1,3) + HK25 + HK44*P(3,17) + HK47*P(3,18) - HK48*P(3,16) + HK53 + P(3,20); -const float HK55 = HK20*P(2,18) - HK21*P(3,18) + HK22*P(1,18) + HK24*P(0,18) + HK44*P(17,18) + HK47*P(18,18) - HK48*P(16,18) + P(18,20); -const float HK56 = HK24*P(0,1); -const float HK57 = -HK21*P(1,3) + HK22*P(1,1) + HK29 + HK44*P(1,17) + HK47*P(1,18) - HK48*P(1,16) + HK56 + P(1,20); -const float HK58 = HK21*P(2,3); -const float HK59 = HK20*P(2,2) + HK24*P(0,2) + HK36 + HK44*P(2,17) + HK47*P(2,18) - HK48*P(2,16) - HK58 + P(2,20); -const float HK60 = HK20*P(2,20) - HK21*P(3,20) + HK22*P(1,20) + HK24*P(0,20) + HK44*P(17,20) + HK47*P(18,20) - HK48*P(16,20) + P(20,20); -const float HK61 = 1.0F/(HK20*HK59 - HK21*HK54 + HK22*HK57 + HK24*HK50 + HK44*HK51 + HK47*HK55 - HK48*HK52 + HK60 + R_MAG); -const float HK62 = HK16 + q1*q3; -const float HK63 = HK45 - q2*q3; -const float HK64 = HK10 + HK11 + HK43 + HK9; -const float HK65 = 2*HK62; -const float HK66 = 2*HK63; -const float HK67 = HK20*P(0,3) + HK22*P(0,0) + HK30 - HK56 + HK64*P(0,18) + HK65*P(0,16) - HK66*P(0,17) + P(0,21); -const float HK68 = HK20*P(3,18) + HK21*P(2,18) + HK22*P(0,18) - HK24*P(1,18) + HK64*P(18,18) + HK65*P(16,18) - HK66*P(17,18) + P(18,21); -const float HK69 = HK20*P(3,17) + HK21*P(2,17) + HK22*P(0,17) - HK24*P(1,17) + HK64*P(17,18) + HK65*P(16,17) - HK66*P(17,17) + P(17,21); -const float HK70 = HK21*P(1,2) - HK24*P(1,1) + HK33 + HK49 + HK64*P(1,18) + HK65*P(1,16) - HK66*P(1,17) + P(1,21); -const float HK71 = HK20*P(3,16) + HK21*P(2,16) + HK22*P(0,16) - HK24*P(1,16) + HK64*P(16,18) + HK65*P(16,16) - HK66*P(16,17) + P(16,21); -const float HK72 = HK20*P(3,3) + HK22*P(0,3) - HK37 + HK58 + HK64*P(3,18) + HK65*P(3,16) - HK66*P(3,17) + P(3,21); -const float HK73 = HK21*P(2,2) + HK23 - HK24*P(1,2) + HK53 + HK64*P(2,18) + HK65*P(2,16) - HK66*P(2,17) + P(2,21); -const float HK74 = HK20*P(3,21) + HK21*P(2,21) + HK22*P(0,21) - HK24*P(1,21) + HK64*P(18,21) + HK65*P(16,21) - HK66*P(17,21) + P(21,21); -const float HK75 = 1.0F/(HK20*HK72 + HK21*HK73 + HK22*HK67 - HK24*HK70 + HK64*HK68 + HK65*HK71 - HK66*HK69 + HK74 + R_MAG); - - -// Observation Jacobians - axis 0 -Hfusion.at<0>() = HK1; -Hfusion.at<1>() = HK3; -Hfusion.at<2>() = -2*HK4; -Hfusion.at<3>() = HK6; -Hfusion.at<16>() = HK13; -Hfusion.at<17>() = 2*HK15; -Hfusion.at<18>() = -2*HK17; -Hfusion.at<19>() = 1; - - -// Kalman gains - axis 0 -Kfusion(0) = HK26*HK40; -Kfusion(1) = HK38*HK40; -Kfusion(2) = HK31*HK40; -Kfusion(3) = HK35*HK40; -Kfusion(4) = HK40*(HK13*P(4,16) + HK18*P(4,17) - HK19*P(4,18) + HK20*P(1,4) + HK21*P(0,4) - HK22*P(2,4) + HK24*P(3,4) + P(4,19)); -Kfusion(5) = HK40*(HK13*P(5,16) + HK18*P(5,17) - HK19*P(5,18) + HK20*P(1,5) + HK21*P(0,5) - HK22*P(2,5) + HK24*P(3,5) + P(5,19)); -Kfusion(6) = HK40*(HK13*P(6,16) + HK18*P(6,17) - HK19*P(6,18) + HK20*P(1,6) + HK21*P(0,6) - HK22*P(2,6) + HK24*P(3,6) + P(6,19)); -Kfusion(7) = HK40*(HK13*P(7,16) + HK18*P(7,17) - HK19*P(7,18) + HK20*P(1,7) + HK21*P(0,7) - HK22*P(2,7) + HK24*P(3,7) + P(7,19)); -Kfusion(8) = HK40*(HK13*P(8,16) + HK18*P(8,17) - HK19*P(8,18) + HK20*P(1,8) + HK21*P(0,8) - HK22*P(2,8) + HK24*P(3,8) + P(8,19)); -Kfusion(9) = HK40*(HK13*P(9,16) + HK18*P(9,17) - HK19*P(9,18) + HK20*P(1,9) + HK21*P(0,9) - HK22*P(2,9) + HK24*P(3,9) + P(9,19)); -Kfusion(10) = HK40*(HK13*P(10,16) + HK18*P(10,17) - HK19*P(10,18) + HK20*P(1,10) + HK21*P(0,10) - HK22*P(2,10) + HK24*P(3,10) + P(10,19)); -Kfusion(11) = HK40*(HK13*P(11,16) + HK18*P(11,17) - HK19*P(11,18) + HK20*P(1,11) + HK21*P(0,11) - HK22*P(2,11) + HK24*P(3,11) + P(11,19)); -Kfusion(12) = HK40*(HK13*P(12,16) + HK18*P(12,17) - HK19*P(12,18) + HK20*P(1,12) + HK21*P(0,12) - HK22*P(2,12) + HK24*P(3,12) + P(12,19)); -Kfusion(13) = HK40*(HK13*P(13,16) + HK18*P(13,17) - HK19*P(13,18) + HK20*P(1,13) + HK21*P(0,13) - HK22*P(2,13) + HK24*P(3,13) + P(13,19)); -Kfusion(14) = HK40*(HK13*P(14,16) + HK18*P(14,17) - HK19*P(14,18) + HK20*P(1,14) + HK21*P(0,14) - HK22*P(2,14) + HK24*P(3,14) + P(14,19)); -Kfusion(15) = HK40*(HK13*P(15,16) + HK18*P(15,17) - HK19*P(15,18) + HK20*P(1,15) + HK21*P(0,15) - HK22*P(2,15) + HK24*P(3,15) + P(15,19)); -Kfusion(16) = HK27*HK40; -Kfusion(17) = HK32*HK40; -Kfusion(18) = HK28*HK40; -Kfusion(19) = HK39*HK40; -Kfusion(20) = HK40*(HK13*P(16,20) + HK18*P(17,20) - HK19*P(18,20) + HK20*P(1,20) + HK21*P(0,20) - HK22*P(2,20) + HK24*P(3,20) + P(19,20)); -Kfusion(21) = HK40*(HK13*P(16,21) + HK18*P(17,21) - HK19*P(18,21) + HK20*P(1,21) + HK21*P(0,21) - HK22*P(2,21) + HK24*P(3,21) + P(19,21)); -Kfusion(22) = HK40*(HK13*P(16,22) + HK18*P(17,22) - HK19*P(18,22) + HK20*P(1,22) + HK21*P(0,22) - HK22*P(2,22) + HK24*P(3,22) + P(19,22)); -Kfusion(23) = HK40*(HK13*P(16,23) + HK18*P(17,23) - HK19*P(18,23) + HK20*P(1,23) + HK21*P(0,23) - HK22*P(2,23) + HK24*P(3,23) + P(19,23)); - - -// Observation Jacobians - axis 1 -Hfusion.at<0>() = HK6; -Hfusion.at<1>() = HK41; -Hfusion.at<2>() = HK3; -Hfusion.at<3>() = -2*HK0; -Hfusion.at<16>() = -2*HK42; -Hfusion.at<17>() = HK44; -Hfusion.at<18>() = 2*HK46; -Hfusion.at<20>() = 1; - - -// Kalman gains - axis 1 -Kfusion(0) = HK50*HK61; -Kfusion(1) = HK57*HK61; -Kfusion(2) = HK59*HK61; -Kfusion(3) = HK54*HK61; -Kfusion(4) = HK61*(HK20*P(2,4) - HK21*P(3,4) + HK22*P(1,4) + HK24*P(0,4) + HK44*P(4,17) + HK47*P(4,18) - HK48*P(4,16) + P(4,20)); -Kfusion(5) = HK61*(HK20*P(2,5) - HK21*P(3,5) + HK22*P(1,5) + HK24*P(0,5) + HK44*P(5,17) + HK47*P(5,18) - HK48*P(5,16) + P(5,20)); -Kfusion(6) = HK61*(HK20*P(2,6) - HK21*P(3,6) + HK22*P(1,6) + HK24*P(0,6) + HK44*P(6,17) + HK47*P(6,18) - HK48*P(6,16) + P(6,20)); -Kfusion(7) = HK61*(HK20*P(2,7) - HK21*P(3,7) + HK22*P(1,7) + HK24*P(0,7) + HK44*P(7,17) + HK47*P(7,18) - HK48*P(7,16) + P(7,20)); -Kfusion(8) = HK61*(HK20*P(2,8) - HK21*P(3,8) + HK22*P(1,8) + HK24*P(0,8) + HK44*P(8,17) + HK47*P(8,18) - HK48*P(8,16) + P(8,20)); -Kfusion(9) = HK61*(HK20*P(2,9) - HK21*P(3,9) + HK22*P(1,9) + HK24*P(0,9) + HK44*P(9,17) + HK47*P(9,18) - HK48*P(9,16) + P(9,20)); -Kfusion(10) = HK61*(HK20*P(2,10) - HK21*P(3,10) + HK22*P(1,10) + HK24*P(0,10) + HK44*P(10,17) + HK47*P(10,18) - HK48*P(10,16) + P(10,20)); -Kfusion(11) = HK61*(HK20*P(2,11) - HK21*P(3,11) + HK22*P(1,11) + HK24*P(0,11) + HK44*P(11,17) + HK47*P(11,18) - HK48*P(11,16) + P(11,20)); -Kfusion(12) = HK61*(HK20*P(2,12) - HK21*P(3,12) + HK22*P(1,12) + HK24*P(0,12) + HK44*P(12,17) + HK47*P(12,18) - HK48*P(12,16) + P(12,20)); -Kfusion(13) = HK61*(HK20*P(2,13) - HK21*P(3,13) + HK22*P(1,13) + HK24*P(0,13) + HK44*P(13,17) + HK47*P(13,18) - HK48*P(13,16) + P(13,20)); -Kfusion(14) = HK61*(HK20*P(2,14) - HK21*P(3,14) + HK22*P(1,14) + HK24*P(0,14) + HK44*P(14,17) + HK47*P(14,18) - HK48*P(14,16) + P(14,20)); -Kfusion(15) = HK61*(HK20*P(2,15) - HK21*P(3,15) + HK22*P(1,15) + HK24*P(0,15) + HK44*P(15,17) + HK47*P(15,18) - HK48*P(15,16) + P(15,20)); -Kfusion(16) = HK52*HK61; -Kfusion(17) = HK51*HK61; -Kfusion(18) = HK55*HK61; -Kfusion(19) = HK61*(HK20*P(2,19) - HK21*P(3,19) + HK22*P(1,19) + HK24*P(0,19) + HK44*P(17,19) + HK47*P(18,19) - HK48*P(16,19) + P(19,20)); -Kfusion(20) = HK60*HK61; -Kfusion(21) = HK61*(HK20*P(2,21) - HK21*P(3,21) + HK22*P(1,21) + HK24*P(0,21) + HK44*P(17,21) + HK47*P(18,21) - HK48*P(16,21) + P(20,21)); -Kfusion(22) = HK61*(HK20*P(2,22) - HK21*P(3,22) + HK22*P(1,22) + HK24*P(0,22) + HK44*P(17,22) + HK47*P(18,22) - HK48*P(16,22) + P(20,22)); -Kfusion(23) = HK61*(HK20*P(2,23) - HK21*P(3,23) + HK22*P(1,23) + HK24*P(0,23) + HK44*P(17,23) + HK47*P(18,23) - HK48*P(16,23) + P(20,23)); - - -// Observation Jacobians - axis 2 -Hfusion.at<0>() = HK41; -Hfusion.at<1>() = -2*HK5; -Hfusion.at<2>() = HK1; -Hfusion.at<3>() = HK3; -Hfusion.at<16>() = 2*HK62; -Hfusion.at<17>() = -2*HK63; -Hfusion.at<18>() = HK64; -Hfusion.at<21>() = 1; - - -// Kalman gains - axis 2 -Kfusion(0) = HK67*HK75; -Kfusion(1) = HK70*HK75; -Kfusion(2) = HK73*HK75; -Kfusion(3) = HK72*HK75; -Kfusion(4) = HK75*(HK20*P(3,4) + HK21*P(2,4) + HK22*P(0,4) - HK24*P(1,4) + HK64*P(4,18) + HK65*P(4,16) - HK66*P(4,17) + P(4,21)); -Kfusion(5) = HK75*(HK20*P(3,5) + HK21*P(2,5) + HK22*P(0,5) - HK24*P(1,5) + HK64*P(5,18) + HK65*P(5,16) - HK66*P(5,17) + P(5,21)); -Kfusion(6) = HK75*(HK20*P(3,6) + HK21*P(2,6) + HK22*P(0,6) - HK24*P(1,6) + HK64*P(6,18) + HK65*P(6,16) - HK66*P(6,17) + P(6,21)); -Kfusion(7) = HK75*(HK20*P(3,7) + HK21*P(2,7) + HK22*P(0,7) - HK24*P(1,7) + HK64*P(7,18) + HK65*P(7,16) - HK66*P(7,17) + P(7,21)); -Kfusion(8) = HK75*(HK20*P(3,8) + HK21*P(2,8) + HK22*P(0,8) - HK24*P(1,8) + HK64*P(8,18) + HK65*P(8,16) - HK66*P(8,17) + P(8,21)); -Kfusion(9) = HK75*(HK20*P(3,9) + HK21*P(2,9) + HK22*P(0,9) - HK24*P(1,9) + HK64*P(9,18) + HK65*P(9,16) - HK66*P(9,17) + P(9,21)); -Kfusion(10) = HK75*(HK20*P(3,10) + HK21*P(2,10) + HK22*P(0,10) - HK24*P(1,10) + HK64*P(10,18) + HK65*P(10,16) - HK66*P(10,17) + P(10,21)); -Kfusion(11) = HK75*(HK20*P(3,11) + HK21*P(2,11) + HK22*P(0,11) - HK24*P(1,11) + HK64*P(11,18) + HK65*P(11,16) - HK66*P(11,17) + P(11,21)); -Kfusion(12) = HK75*(HK20*P(3,12) + HK21*P(2,12) + HK22*P(0,12) - HK24*P(1,12) + HK64*P(12,18) + HK65*P(12,16) - HK66*P(12,17) + P(12,21)); -Kfusion(13) = HK75*(HK20*P(3,13) + HK21*P(2,13) + HK22*P(0,13) - HK24*P(1,13) + HK64*P(13,18) + HK65*P(13,16) - HK66*P(13,17) + P(13,21)); -Kfusion(14) = HK75*(HK20*P(3,14) + HK21*P(2,14) + HK22*P(0,14) - HK24*P(1,14) + HK64*P(14,18) + HK65*P(14,16) - HK66*P(14,17) + P(14,21)); -Kfusion(15) = HK75*(HK20*P(3,15) + HK21*P(2,15) + HK22*P(0,15) - HK24*P(1,15) + HK64*P(15,18) + HK65*P(15,16) - HK66*P(15,17) + P(15,21)); -Kfusion(16) = HK71*HK75; -Kfusion(17) = HK69*HK75; -Kfusion(18) = HK68*HK75; -Kfusion(19) = HK75*(HK20*P(3,19) + HK21*P(2,19) + HK22*P(0,19) - HK24*P(1,19) + HK64*P(18,19) + HK65*P(16,19) - HK66*P(17,19) + P(19,21)); -Kfusion(20) = HK75*(HK20*P(3,20) + HK21*P(2,20) + HK22*P(0,20) - HK24*P(1,20) + HK64*P(18,20) + HK65*P(16,20) - HK66*P(17,20) + P(20,21)); -Kfusion(21) = HK74*HK75; -Kfusion(22) = HK75*(HK20*P(3,22) + HK21*P(2,22) + HK22*P(0,22) - HK24*P(1,22) + HK64*P(18,22) + HK65*P(16,22) - HK66*P(17,22) + P(21,22)); -Kfusion(23) = HK75*(HK20*P(3,23) + HK21*P(2,23) + HK22*P(0,23) - HK24*P(1,23) + HK64*P(18,23) + HK65*P(16,23) - HK66*P(17,23) + P(21,23)); - - diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_innov_var_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_innov_var_generated.cpp deleted file mode 100644 index a02d0af8c7..0000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_innov_var_generated.cpp +++ /dev/null @@ -1,43 +0,0 @@ -// Sub Expressions -const float IV0 = q0*q1; -const float IV1 = q2*q3; -const float IV2 = 2*IV0 + 2*IV1; -const float IV3 = 2*q0*q3 - 2*q1*q2; -const float IV4 = 2*magD*q3 + 2*magE*q2 + 2*magN*q1; -const float IV5 = 2*magD*q1 + 2*magE*q0 - 2*magN*q3; -const float IV6 = 2*magD*q0 - 2*magE*q1 + 2*magN*q2; -const float IV7 = -2*magD*q2 + 2*magE*q3 + 2*magN*q0; -const float IV8 = (q2)*(q2); -const float IV9 = (q3)*(q3); -const float IV10 = (q0)*(q0) - (q1)*(q1); -const float IV11 = IV10 + IV8 - IV9; -const float IV12 = IV7*P(2,3); -const float IV13 = IV5*P(0,1); -const float IV14 = IV6*P(0,1); -const float IV15 = IV4*P(2,3); -const float IV16 = 2*q0*q2 + 2*q1*q3; -const float IV17 = 2*IV0 - 2*IV1; -const float IV18 = IV10 - IV8 + IV9; - - -// Observation Jacobians - axis 0 -Hfusion.at<0>() = R_MAG; -Hfusion.at<1>() = IV11*P(17,20) + IV11*(IV11*P(17,17) + IV2*P(17,18) - IV3*P(16,17) + IV4*P(2,17) + IV5*P(0,17) + IV6*P(1,17) - IV7*P(3,17) + P(17,20)) + IV2*P(18,20) + IV2*(IV11*P(17,18) + IV2*P(18,18) - IV3*P(16,18) + IV4*P(2,18) + IV5*P(0,18) + IV6*P(1,18) - IV7*P(3,18) + P(18,20)) - IV3*P(16,20) - IV3*(IV11*P(16,17) + IV2*P(16,18) - IV3*P(16,16) + IV4*P(2,16) + IV5*P(0,16) + IV6*P(1,16) - IV7*P(3,16) + P(16,20)) + IV4*P(2,20) + IV4*(IV11*P(2,17) - IV12 + IV2*P(2,18) - IV3*P(2,16) + IV4*P(2,2) + IV5*P(0,2) + IV6*P(1,2) + P(2,20)) + IV5*P(0,20) + IV5*(IV11*P(0,17) + IV14 + IV2*P(0,18) - IV3*P(0,16) + IV4*P(0,2) + IV5*P(0,0) - IV7*P(0,3) + P(0,20)) + IV6*P(1,20) + IV6*(IV11*P(1,17) + IV13 + IV2*P(1,18) - IV3*P(1,16) + IV4*P(1,2) + IV6*P(1,1) - IV7*P(1,3) + P(1,20)) - IV7*P(3,20) - IV7*(IV11*P(3,17) + IV15 + IV2*P(3,18) - IV3*P(3,16) + IV5*P(0,3) + IV6*P(1,3) - IV7*P(3,3) + P(3,20)) + P(20,20) + R_MAG; -Hfusion.at<2>() = IV16*P(16,21) + IV16*(IV16*P(16,16) - IV17*P(16,17) + IV18*P(16,18) + IV4*P(3,16) - IV5*P(1,16) + IV6*P(0,16) + IV7*P(2,16) + P(16,21)) - IV17*P(17,21) - IV17*(IV16*P(16,17) - IV17*P(17,17) + IV18*P(17,18) + IV4*P(3,17) - IV5*P(1,17) + IV6*P(0,17) + IV7*P(2,17) + P(17,21)) + IV18*P(18,21) + IV18*(IV16*P(16,18) - IV17*P(17,18) + IV18*P(18,18) + IV4*P(3,18) - IV5*P(1,18) + IV6*P(0,18) + IV7*P(2,18) + P(18,21)) + IV4*P(3,21) + IV4*(IV12 + IV16*P(3,16) - IV17*P(3,17) + IV18*P(3,18) + IV4*P(3,3) - IV5*P(1,3) + IV6*P(0,3) + P(3,21)) - IV5*P(1,21) - IV5*(IV14 + IV16*P(1,16) - IV17*P(1,17) + IV18*P(1,18) + IV4*P(1,3) - IV5*P(1,1) + IV7*P(1,2) + P(1,21)) + IV6*P(0,21) + IV6*(-IV13 + IV16*P(0,16) - IV17*P(0,17) + IV18*P(0,18) + IV4*P(0,3) + IV6*P(0,0) + IV7*P(0,2) + P(0,21)) + IV7*P(2,21) + IV7*(IV15 + IV16*P(2,16) - IV17*P(2,17) + IV18*P(2,18) - IV5*P(1,2) + IV6*P(0,2) + IV7*P(2,2) + P(2,21)) + P(21,21) + R_MAG; - - -// Kalman gains - axis 0 - - -// Observation Jacobians - axis 1 - - -// Kalman gains - axis 1 - - -// Observation Jacobians - axis 2 - - -// Kalman gains - axis 2 - -