diff --git a/EKF/python/ekf_derivation/generated/3Dmag_generated.cpp b/EKF/python/ekf_derivation/generated/3Dmag_generated.cpp index 23f60186ab..47dd3b1b45 100644 --- a/EKF/python/ekf_derivation/generated/3Dmag_generated.cpp +++ b/EKF/python/ekf_derivation/generated/3Dmag_generated.cpp @@ -1,37 +1,83 @@ // 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 = 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); +const float HKX0 = magE*q3; +const float HKX1 = magD*q2; +const float HKX2 = magD*q3 + magE*q2; +const float HKX3 = magE*q1; +const float HKX4 = magD*q0; +const float HKX5 = 2*magN; +const float HKX6 = HKX5*q2; +const float HKX7 = -HKX5*q3 + magD*q1 + magE*q0; +const float HKX8 = 2*powf(q2, 2); +const float HKX9 = 2*powf(q3, 2); +const float HKX10 = q0*q3 + q1*q2; +const float HKX11 = q1*q3; +const float HKX12 = q0*q2; +const float HKX13 = 2*HKX2; +const float HKX14 = HKX13*P(0,1); +const float HKX15 = 2*HKX10; +const float HKX16 = HKX15*P(0,17); +const float HKX17 = -2*HKX0 + 2*HKX1; +const float HKX18 = HKX17*P(0,0); +const float HKX19 = -2*HKX11 + 2*HKX12; +const float HKX20 = HKX19*P(0,18); +const float HKX21 = HKX8 + HKX9 - 1; +const float HKX22 = HKX21*P(0,16); +const float HKX23 = 2*HKX7; +const float HKX24 = HKX23*P(0,3); +const float HKX25 = -2*HKX3 + 2*HKX4 + 2*HKX6; +const float HKX26 = HKX25*P(0,2); +const float HKX27 = HKX13*P(1,16); +const float HKX28 = HKX15*P(16,17); +const float HKX29 = HKX17*P(0,16); +const float HKX30 = HKX19*P(16,18); +const float HKX31 = HKX23*P(3,16); +const float HKX32 = HKX21*P(16,16); +const float HKX33 = HKX25*P(2,16); +const float HKX34 = HKX13*P(1,1); +const float HKX35 = HKX15*P(1,17); +const float HKX36 = HKX17*P(0,1); +const float HKX37 = HKX19*P(1,18); +const float HKX38 = HKX23*P(1,3); +const float HKX39 = HKX21*P(1,16); +const float HKX40 = HKX25*P(1,2); +const float HKX41 = HKX13*P(1,17); +const float HKX42 = HKX15*P(17,17); +const float HKX43 = HKX17*P(0,17); +const float HKX44 = HKX19*P(17,18); +const float HKX45 = HKX23*P(3,17); +const float HKX46 = HKX21*P(16,17); +const float HKX47 = HKX25*P(2,17); +const float HKX48 = HKX13*P(1,3); +const float HKX49 = HKX15*P(3,17); +const float HKX50 = HKX17*P(0,3); +const float HKX51 = HKX19*P(3,18); +const float HKX52 = HKX23*P(3,3); +const float HKX53 = HKX25*P(2,3); +const float HKX54 = HKX21*P(3,16); +const float HKX55 = HKX13*P(1,18); +const float HKX56 = HKX15*P(17,18); +const float HKX57 = HKX17*P(0,18); +const float HKX58 = HKX19*P(18,18); +const float HKX59 = HKX23*P(3,18); +const float HKX60 = HKX21*P(16,18); +const float HKX61 = HKX25*P(2,18); +const float HKX62 = HKX13*P(1,2); +const float HKX63 = HKX15*P(2,17); +const float HKX64 = HKX17*P(0,2); +const float HKX65 = HKX19*P(2,18); +const float HKX66 = HKX23*P(2,3); +const float HKX67 = HKX21*P(2,16); +const float HKX68 = HKX25*P(2,2); +const float HKX69 = -HKX13*P(1,19) - HKX15*P(17,19) + HKX17*P(0,19) + HKX19*P(18,19) + HKX21*P(16,19) - HKX23*P(3,19) + HKX25*P(2,19) - P(19,19); +const float HKX70 = 1.0F/(-HKX13*(HKX34 + HKX35 - HKX36 - HKX37 + HKX38 - HKX39 - HKX40 + P(1,19)) - HKX15*(HKX41 + HKX42 - HKX43 - HKX44 + HKX45 - HKX46 - HKX47 + P(17,19)) + HKX17*(HKX14 + HKX16 - HKX18 - HKX20 - HKX22 + HKX24 - HKX26 + P(0,19)) + HKX19*(HKX55 + HKX56 - HKX57 - HKX58 + HKX59 - HKX60 - HKX61 + P(18,19)) + HKX21*(HKX27 + HKX28 - HKX29 - HKX30 + HKX31 - HKX32 - HKX33 + P(16,19)) - HKX23*(HKX48 + HKX49 - HKX50 - HKX51 + HKX52 - HKX53 - HKX54 + P(3,19)) + HKX25*(HKX62 + HKX63 - HKX64 - HKX65 + HKX66 - HKX67 - HKX68 + P(2,19)) + HKX69 - R_MAG); // Observation Jacobians -Hfusion.at<0>() = 2*HKX0; -Hfusion.at<1>() = 2*HKX1; -Hfusion.at<2>() = 2*HKX2 - 2*HKX3 - 2*HKX4; -Hfusion.at<3>() = 2*HKX5; +Hfusion.at<0>() = 2*HKX0 - 2*HKX1; +Hfusion.at<1>() = 2*HKX2; +Hfusion.at<2>() = 2*HKX3 - 2*HKX4 - 2*HKX6; +Hfusion.at<3>() = 2*HKX7; Hfusion.at<4>() = 0; Hfusion.at<5>() = 0; Hfusion.at<6>() = 0; @@ -44,9 +90,9 @@ Hfusion.at<12>() = 0; Hfusion.at<13>() = 0; Hfusion.at<14>() = 0; Hfusion.at<15>() = 0; -Hfusion.at<16>() = HKX6; -Hfusion.at<17>() = 2*HKX7; -Hfusion.at<18>() = 2*HKX8 - 2*HKX9; +Hfusion.at<16>() = -HKX8 - HKX9 + 1; +Hfusion.at<17>() = 2*HKX10; +Hfusion.at<18>() = 2*HKX11 - 2*HKX12; Hfusion.at<19>() = 1; Hfusion.at<20>() = 0; Hfusion.at<21>() = 0; @@ -55,66 +101,69 @@ Hfusion.at<23>() = 0; // Kalman gains -Kfusion(0) = HKX16*HKX24; -Kfusion(1) = HKX22*HKX24; -Kfusion(2) = HKX19*HKX24; -Kfusion(3) = HKX21*HKX24; -Kfusion(4) = HKX24*(HKX10*P(4,17) - HKX11*P(4,18) + HKX12*P(1,4) + HKX13*P(0,4) - HKX14*P(2,4) + HKX15*P(3,4) + HKX6*P(4,16) + P(4,19)); -Kfusion(5) = HKX24*(HKX10*P(5,17) - HKX11*P(5,18) + HKX12*P(1,5) + HKX13*P(0,5) - HKX14*P(2,5) + HKX15*P(3,5) + HKX6*P(5,16) + P(5,19)); -Kfusion(6) = HKX24*(HKX10*P(6,17) - HKX11*P(6,18) + HKX12*P(1,6) + HKX13*P(0,6) - HKX14*P(2,6) + HKX15*P(3,6) + HKX6*P(6,16) + P(6,19)); -Kfusion(7) = HKX24*(HKX10*P(7,17) - HKX11*P(7,18) + HKX12*P(1,7) + HKX13*P(0,7) - HKX14*P(2,7) + HKX15*P(3,7) + HKX6*P(7,16) + P(7,19)); -Kfusion(8) = HKX24*(HKX10*P(8,17) - HKX11*P(8,18) + HKX12*P(1,8) + HKX13*P(0,8) - HKX14*P(2,8) + HKX15*P(3,8) + HKX6*P(8,16) + P(8,19)); -Kfusion(9) = HKX24*(HKX10*P(9,17) - HKX11*P(9,18) + HKX12*P(1,9) + HKX13*P(0,9) - HKX14*P(2,9) + HKX15*P(3,9) + HKX6*P(9,16) + P(9,19)); -Kfusion(10) = HKX24*(HKX10*P(10,17) - HKX11*P(10,18) + HKX12*P(1,10) + HKX13*P(0,10) - HKX14*P(2,10) + HKX15*P(3,10) + HKX6*P(10,16) + P(10,19)); -Kfusion(11) = HKX24*(HKX10*P(11,17) - HKX11*P(11,18) + HKX12*P(1,11) + HKX13*P(0,11) - HKX14*P(2,11) + HKX15*P(3,11) + HKX6*P(11,16) + P(11,19)); -Kfusion(12) = HKX24*(HKX10*P(12,17) - HKX11*P(12,18) + HKX12*P(1,12) + HKX13*P(0,12) - HKX14*P(2,12) + HKX15*P(3,12) + HKX6*P(12,16) + P(12,19)); -Kfusion(13) = HKX24*(HKX10*P(13,17) - HKX11*P(13,18) + HKX12*P(1,13) + HKX13*P(0,13) - HKX14*P(2,13) + HKX15*P(3,13) + HKX6*P(13,16) + P(13,19)); -Kfusion(14) = HKX24*(HKX10*P(14,17) - HKX11*P(14,18) + HKX12*P(1,14) + HKX13*P(0,14) - HKX14*P(2,14) + HKX15*P(3,14) + HKX6*P(14,16) + P(14,19)); -Kfusion(15) = HKX24*(HKX10*P(15,17) - HKX11*P(15,18) + HKX12*P(1,15) + HKX13*P(0,15) - HKX14*P(2,15) + HKX15*P(3,15) + HKX6*P(15,16) + P(15,19)); -Kfusion(16) = HKX17*HKX24; -Kfusion(17) = HKX20*HKX24; -Kfusion(18) = HKX18*HKX24; -Kfusion(19) = HKX23*HKX24; -Kfusion(20) = HKX24*(HKX10*P(17,20) - HKX11*P(18,20) + HKX12*P(1,20) + HKX13*P(0,20) - HKX14*P(2,20) + HKX15*P(3,20) + HKX6*P(16,20) + P(19,20)); -Kfusion(21) = HKX24*(HKX10*P(17,21) - HKX11*P(18,21) + HKX12*P(1,21) + HKX13*P(0,21) - HKX14*P(2,21) + HKX15*P(3,21) + HKX6*P(16,21) + P(19,21)); -Kfusion(22) = HKX24*(HKX10*P(17,22) - HKX11*P(18,22) + HKX12*P(1,22) + HKX13*P(0,22) - HKX14*P(2,22) + HKX15*P(3,22) + HKX6*P(16,22) + P(19,22)); -Kfusion(23) = HKX24*(HKX10*P(17,23) - HKX11*P(18,23) + HKX12*P(1,23) + HKX13*P(0,23) - HKX14*P(2,23) + HKX15*P(3,23) + HKX6*P(16,23) + P(19,23)); +Kfusion(0) = HKX70*(-HKX14 - HKX16 + HKX18 + HKX20 + HKX22 - HKX24 + HKX26 - P(0,19)); +Kfusion(1) = HKX70*(-HKX34 - HKX35 + HKX36 + HKX37 - HKX38 + HKX39 + HKX40 - P(1,19)); +Kfusion(2) = HKX70*(-HKX62 - HKX63 + HKX64 + HKX65 - HKX66 + HKX67 + HKX68 - P(2,19)); +Kfusion(3) = HKX70*(-HKX48 - HKX49 + HKX50 + HKX51 - HKX52 + HKX53 + HKX54 - P(3,19)); +Kfusion(4) = HKX70*(-HKX13*P(1,4) - HKX15*P(4,17) + HKX17*P(0,4) + HKX19*P(4,18) + HKX21*P(4,16) - HKX23*P(3,4) + HKX25*P(2,4) - P(4,19)); +Kfusion(5) = HKX70*(-HKX13*P(1,5) - HKX15*P(5,17) + HKX17*P(0,5) + HKX19*P(5,18) + HKX21*P(5,16) - HKX23*P(3,5) + HKX25*P(2,5) - P(5,19)); +Kfusion(6) = HKX70*(-HKX13*P(1,6) - HKX15*P(6,17) + HKX17*P(0,6) + HKX19*P(6,18) + HKX21*P(6,16) - HKX23*P(3,6) + HKX25*P(2,6) - P(6,19)); +Kfusion(7) = HKX70*(-HKX13*P(1,7) - HKX15*P(7,17) + HKX17*P(0,7) + HKX19*P(7,18) + HKX21*P(7,16) - HKX23*P(3,7) + HKX25*P(2,7) - P(7,19)); +Kfusion(8) = HKX70*(-HKX13*P(1,8) - HKX15*P(8,17) + HKX17*P(0,8) + HKX19*P(8,18) + HKX21*P(8,16) - HKX23*P(3,8) + HKX25*P(2,8) - P(8,19)); +Kfusion(9) = HKX70*(-HKX13*P(1,9) - HKX15*P(9,17) + HKX17*P(0,9) + HKX19*P(9,18) + HKX21*P(9,16) - HKX23*P(3,9) + HKX25*P(2,9) - P(9,19)); +Kfusion(10) = HKX70*(-HKX13*P(1,10) - HKX15*P(10,17) + HKX17*P(0,10) + HKX19*P(10,18) + HKX21*P(10,16) - HKX23*P(3,10) + HKX25*P(2,10) - P(10,19)); +Kfusion(11) = HKX70*(-HKX13*P(1,11) - HKX15*P(11,17) + HKX17*P(0,11) + HKX19*P(11,18) + HKX21*P(11,16) - HKX23*P(3,11) + HKX25*P(2,11) - P(11,19)); +Kfusion(12) = HKX70*(-HKX13*P(1,12) - HKX15*P(12,17) + HKX17*P(0,12) + HKX19*P(12,18) + HKX21*P(12,16) - HKX23*P(3,12) + HKX25*P(2,12) - P(12,19)); +Kfusion(13) = HKX70*(-HKX13*P(1,13) - HKX15*P(13,17) + HKX17*P(0,13) + HKX19*P(13,18) + HKX21*P(13,16) - HKX23*P(3,13) + HKX25*P(2,13) - P(13,19)); +Kfusion(14) = HKX70*(-HKX13*P(1,14) - HKX15*P(14,17) + HKX17*P(0,14) + HKX19*P(14,18) + HKX21*P(14,16) - HKX23*P(3,14) + HKX25*P(2,14) - P(14,19)); +Kfusion(15) = HKX70*(-HKX13*P(1,15) - HKX15*P(15,17) + HKX17*P(0,15) + HKX19*P(15,18) + HKX21*P(15,16) - HKX23*P(3,15) + HKX25*P(2,15) - P(15,19)); +Kfusion(16) = HKX70*(-HKX27 - HKX28 + HKX29 + HKX30 - HKX31 + HKX32 + HKX33 - P(16,19)); +Kfusion(17) = HKX70*(-HKX41 - HKX42 + HKX43 + HKX44 - HKX45 + HKX46 + HKX47 - P(17,19)); +Kfusion(18) = HKX70*(-HKX55 - HKX56 + HKX57 + HKX58 - HKX59 + HKX60 + HKX61 - P(18,19)); +Kfusion(19) = HKX69*HKX70; +Kfusion(20) = HKX70*(-HKX13*P(1,20) - HKX15*P(17,20) + HKX17*P(0,20) + HKX19*P(18,20) + HKX21*P(16,20) - HKX23*P(3,20) + HKX25*P(2,20) - P(19,20)); +Kfusion(21) = HKX70*(-HKX13*P(1,21) - HKX15*P(17,21) + HKX17*P(0,21) + HKX19*P(18,21) + HKX21*P(16,21) - HKX23*P(3,21) + HKX25*P(2,21) - P(19,21)); +Kfusion(22) = HKX70*(-HKX13*P(1,22) - HKX15*P(17,22) + HKX17*P(0,22) + HKX19*P(18,22) + HKX21*P(16,22) - HKX23*P(3,22) + HKX25*P(2,22) - P(19,22)); +Kfusion(23) = HKX70*(-HKX13*P(1,23) - HKX15*P(17,23) + HKX17*P(0,23) + HKX19*P(18,23) + HKX21*P(16,23) - HKX23*P(3,23) + HKX25*P(2,23) - P(19,23)); // 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; -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); +const float HKY0 = magD*q1 - magN*q3; +const float HKY1 = 2*magE; +const float HKY2 = -HKY1*q1 + magD*q0 + magN*q2; +const float HKY3 = magD*q3 + magN*q1; +const float HKY4 = magD*q2; +const float HKY5 = HKY1*q3; +const float HKY6 = magN*q0; +const float HKY7 = q1*q2; +const float HKY8 = q0*q3; +const float HKY9 = 2*powf(q1, 2); +const float HKY10 = 2*powf(q3, 2); +const float HKY11 = q0*q1 + q2*q3; +const float HKY12 = 2*HKY11; +const float HKY13 = 2*HKY3; +const float HKY14 = 2*HKY0; +const float HKY15 = -2*HKY7 + 2*HKY8; +const float HKY16 = 2*HKY2; +const float HKY17 = HKY10 + HKY9 - 1; +const float HKY18 = -2*HKY4 + 2*HKY5 + 2*HKY6; +const float HKY19 = HKY12*P(0,18) + HKY13*P(0,2) + HKY14*P(0,0) - HKY15*P(0,16) + HKY16*P(0,1) - HKY17*P(0,17) - HKY18*P(0,3) + P(0,20); +const float HKY20 = HKY12*P(17,18) + HKY13*P(2,17) + HKY14*P(0,17) - HKY15*P(16,17) + HKY16*P(1,17) - HKY17*P(17,17) - HKY18*P(3,17) + P(17,20); +const float HKY21 = HKY12*P(16,18) + HKY13*P(2,16) + HKY14*P(0,16) - HKY15*P(16,16) + HKY16*P(1,16) - HKY17*P(16,17) - HKY18*P(3,16) + P(16,20); +const float HKY22 = HKY12*P(3,18) + HKY13*P(2,3) + HKY14*P(0,3) - HKY15*P(3,16) + HKY16*P(1,3) - HKY17*P(3,17) - HKY18*P(3,3) + P(3,20); +const float HKY23 = HKY12*P(2,18) + HKY13*P(2,2) + HKY14*P(0,2) - HKY15*P(2,16) + HKY16*P(1,2) - HKY17*P(2,17) - HKY18*P(2,3) + P(2,20); +const float HKY24 = HKY12*P(18,18) + HKY13*P(2,18) + HKY14*P(0,18) - HKY15*P(16,18) + HKY16*P(1,18) - HKY17*P(17,18) - HKY18*P(3,18) + P(18,20); +const float HKY25 = HKY12*P(1,18) + HKY13*P(1,2) + HKY14*P(0,1) - HKY15*P(1,16) + HKY16*P(1,1) - HKY17*P(1,17) - HKY18*P(1,3) + P(1,20); +const float HKY26 = HKY12*P(18,20) + HKY13*P(2,20) + HKY14*P(0,20) - HKY15*P(16,20) + HKY16*P(1,20) - HKY17*P(17,20) - HKY18*P(3,20) + P(20,20); +const float HKY27 = 1.0F/(HKY12*HKY24 + HKY13*HKY23 + HKY14*HKY19 - HKY15*HKY21 + HKY16*HKY25 - HKY17*HKY20 - HKY18*HKY22 + HKY26 + R_MAG); // Observation Jacobians Hfusion.at<0>() = 2*HKY0; -Hfusion.at<1>() = 2*HKY1; -Hfusion.at<2>() = 2*HKY2; -Hfusion.at<3>() = 2*HKY3 - 2*HKY4 - 2*HKY5; +Hfusion.at<1>() = 2*HKY2; +Hfusion.at<2>() = 2*HKY3; +Hfusion.at<3>() = 2*HKY4 - 2*HKY5 - 2*HKY6; Hfusion.at<4>() = 0; Hfusion.at<5>() = 0; Hfusion.at<6>() = 0; @@ -127,9 +176,9 @@ Hfusion.at<12>() = 0; Hfusion.at<13>() = 0; Hfusion.at<14>() = 0; Hfusion.at<15>() = 0; -Hfusion.at<16>() = 2*HKY6 - 2*HKY7; -Hfusion.at<17>() = HKY8; -Hfusion.at<18>() = 2*HKY9; +Hfusion.at<16>() = 2*HKY7 - 2*HKY8; +Hfusion.at<17>() = -HKY10 - HKY9 + 1; +Hfusion.at<18>() = 2*HKY11; Hfusion.at<19>() = 0; Hfusion.at<20>() = 1; Hfusion.at<21>() = 0; @@ -138,66 +187,112 @@ Hfusion.at<23>() = 0; // Kalman gains -Kfusion(0) = HKY16*HKY24; -Kfusion(1) = HKY21*HKY24; -Kfusion(2) = HKY22*HKY24; -Kfusion(3) = HKY19*HKY24; -Kfusion(4) = HKY24*(HKY10*P(4,18) - HKY11*P(4,16) + HKY12*P(2,4) + HKY13*P(0,4) + HKY14*P(1,4) - HKY15*P(3,4) + HKY8*P(4,17) + P(4,20)); -Kfusion(5) = HKY24*(HKY10*P(5,18) - HKY11*P(5,16) + HKY12*P(2,5) + HKY13*P(0,5) + HKY14*P(1,5) - HKY15*P(3,5) + HKY8*P(5,17) + P(5,20)); -Kfusion(6) = HKY24*(HKY10*P(6,18) - HKY11*P(6,16) + HKY12*P(2,6) + HKY13*P(0,6) + HKY14*P(1,6) - HKY15*P(3,6) + HKY8*P(6,17) + P(6,20)); -Kfusion(7) = HKY24*(HKY10*P(7,18) - HKY11*P(7,16) + HKY12*P(2,7) + HKY13*P(0,7) + HKY14*P(1,7) - HKY15*P(3,7) + HKY8*P(7,17) + P(7,20)); -Kfusion(8) = HKY24*(HKY10*P(8,18) - HKY11*P(8,16) + HKY12*P(2,8) + HKY13*P(0,8) + HKY14*P(1,8) - HKY15*P(3,8) + HKY8*P(8,17) + P(8,20)); -Kfusion(9) = HKY24*(HKY10*P(9,18) - HKY11*P(9,16) + HKY12*P(2,9) + HKY13*P(0,9) + HKY14*P(1,9) - HKY15*P(3,9) + HKY8*P(9,17) + P(9,20)); -Kfusion(10) = HKY24*(HKY10*P(10,18) - HKY11*P(10,16) + HKY12*P(2,10) + HKY13*P(0,10) + HKY14*P(1,10) - HKY15*P(3,10) + HKY8*P(10,17) + P(10,20)); -Kfusion(11) = HKY24*(HKY10*P(11,18) - HKY11*P(11,16) + HKY12*P(2,11) + HKY13*P(0,11) + HKY14*P(1,11) - HKY15*P(3,11) + HKY8*P(11,17) + P(11,20)); -Kfusion(12) = HKY24*(HKY10*P(12,18) - HKY11*P(12,16) + HKY12*P(2,12) + HKY13*P(0,12) + HKY14*P(1,12) - HKY15*P(3,12) + HKY8*P(12,17) + P(12,20)); -Kfusion(13) = HKY24*(HKY10*P(13,18) - HKY11*P(13,16) + HKY12*P(2,13) + HKY13*P(0,13) + HKY14*P(1,13) - HKY15*P(3,13) + HKY8*P(13,17) + P(13,20)); -Kfusion(14) = HKY24*(HKY10*P(14,18) - HKY11*P(14,16) + HKY12*P(2,14) + HKY13*P(0,14) + HKY14*P(1,14) - HKY15*P(3,14) + HKY8*P(14,17) + P(14,20)); -Kfusion(15) = HKY24*(HKY10*P(15,18) - HKY11*P(15,16) + HKY12*P(2,15) + HKY13*P(0,15) + HKY14*P(1,15) - HKY15*P(3,15) + HKY8*P(15,17) + P(15,20)); -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)); -Kfusion(22) = HKY24*(HKY10*P(18,22) - HKY11*P(16,22) + HKY12*P(2,22) + HKY13*P(0,22) + HKY14*P(1,22) - HKY15*P(3,22) + HKY8*P(17,22) + P(20,22)); -Kfusion(23) = HKY24*(HKY10*P(18,23) - HKY11*P(16,23) + HKY12*P(2,23) + HKY13*P(0,23) + HKY14*P(1,23) - HKY15*P(3,23) + HKY8*P(17,23) + P(20,23)); +Kfusion(0) = HKY19*HKY27; +Kfusion(1) = HKY25*HKY27; +Kfusion(2) = HKY23*HKY27; +Kfusion(3) = HKY22*HKY27; +Kfusion(4) = HKY27*(HKY12*P(4,18) + HKY13*P(2,4) + HKY14*P(0,4) - HKY15*P(4,16) + HKY16*P(1,4) - HKY17*P(4,17) - HKY18*P(3,4) + P(4,20)); +Kfusion(5) = HKY27*(HKY12*P(5,18) + HKY13*P(2,5) + HKY14*P(0,5) - HKY15*P(5,16) + HKY16*P(1,5) - HKY17*P(5,17) - HKY18*P(3,5) + P(5,20)); +Kfusion(6) = HKY27*(HKY12*P(6,18) + HKY13*P(2,6) + HKY14*P(0,6) - HKY15*P(6,16) + HKY16*P(1,6) - HKY17*P(6,17) - HKY18*P(3,6) + P(6,20)); +Kfusion(7) = HKY27*(HKY12*P(7,18) + HKY13*P(2,7) + HKY14*P(0,7) - HKY15*P(7,16) + HKY16*P(1,7) - HKY17*P(7,17) - HKY18*P(3,7) + P(7,20)); +Kfusion(8) = HKY27*(HKY12*P(8,18) + HKY13*P(2,8) + HKY14*P(0,8) - HKY15*P(8,16) + HKY16*P(1,8) - HKY17*P(8,17) - HKY18*P(3,8) + P(8,20)); +Kfusion(9) = HKY27*(HKY12*P(9,18) + HKY13*P(2,9) + HKY14*P(0,9) - HKY15*P(9,16) + HKY16*P(1,9) - HKY17*P(9,17) - HKY18*P(3,9) + P(9,20)); +Kfusion(10) = HKY27*(HKY12*P(10,18) + HKY13*P(2,10) + HKY14*P(0,10) - HKY15*P(10,16) + HKY16*P(1,10) - HKY17*P(10,17) - HKY18*P(3,10) + P(10,20)); +Kfusion(11) = HKY27*(HKY12*P(11,18) + HKY13*P(2,11) + HKY14*P(0,11) - HKY15*P(11,16) + HKY16*P(1,11) - HKY17*P(11,17) - HKY18*P(3,11) + P(11,20)); +Kfusion(12) = HKY27*(HKY12*P(12,18) + HKY13*P(2,12) + HKY14*P(0,12) - HKY15*P(12,16) + HKY16*P(1,12) - HKY17*P(12,17) - HKY18*P(3,12) + P(12,20)); +Kfusion(13) = HKY27*(HKY12*P(13,18) + HKY13*P(2,13) + HKY14*P(0,13) - HKY15*P(13,16) + HKY16*P(1,13) - HKY17*P(13,17) - HKY18*P(3,13) + P(13,20)); +Kfusion(14) = HKY27*(HKY12*P(14,18) + HKY13*P(2,14) + HKY14*P(0,14) - HKY15*P(14,16) + HKY16*P(1,14) - HKY17*P(14,17) - HKY18*P(3,14) + P(14,20)); +Kfusion(15) = HKY27*(HKY12*P(15,18) + HKY13*P(2,15) + HKY14*P(0,15) - HKY15*P(15,16) + HKY16*P(1,15) - HKY17*P(15,17) - HKY18*P(3,15) + P(15,20)); +Kfusion(16) = HKY21*HKY27; +Kfusion(17) = HKY20*HKY27; +Kfusion(18) = HKY24*HKY27; +Kfusion(19) = HKY27*(HKY12*P(18,19) + HKY13*P(2,19) + HKY14*P(0,19) - HKY15*P(16,19) + HKY16*P(1,19) - HKY17*P(17,19) - HKY18*P(3,19) + P(19,20)); +Kfusion(20) = HKY26*HKY27; +Kfusion(21) = HKY27*(HKY12*P(18,21) + HKY13*P(2,21) + HKY14*P(0,21) - HKY15*P(16,21) + HKY16*P(1,21) - HKY17*P(17,21) - HKY18*P(3,21) + P(20,21)); +Kfusion(22) = HKY27*(HKY12*P(18,22) + HKY13*P(2,22) + HKY14*P(0,22) - HKY15*P(16,22) + HKY16*P(1,22) - HKY17*P(17,22) - HKY18*P(3,22) + P(20,22)); +Kfusion(23) = HKY27*(HKY12*P(18,23) + HKY13*P(2,23) + HKY14*P(0,23) - HKY15*P(16,23) + HKY16*P(1,23) - HKY17*P(17,23) - HKY18*P(3,23) + P(20,23)); // Axis 2 equations // Sub Expressions -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); +const float HKZ0 = magN*q2; +const float HKZ1 = magE*q1; +const float HKZ2 = magN*q3; +const float HKZ3 = 2*magD; +const float HKZ4 = HKZ3*q1; +const float HKZ5 = magE*q0; +const float HKZ6 = -HKZ3*q2 + magE*q3 + magN*q0; +const float HKZ7 = magE*q2 + magN*q1; +const float HKZ8 = q0*q2 + q1*q3; +const float HKZ9 = q2*q3; +const float HKZ10 = q0*q1; +const float HKZ11 = 2*powf(q1, 2); +const float HKZ12 = 2*powf(q2, 2); +const float HKZ13 = 2*HKZ8; +const float HKZ14 = HKZ13*P(0,16); +const float HKZ15 = 2*HKZ7; +const float HKZ16 = HKZ15*P(0,3); +const float HKZ17 = -2*HKZ0 + 2*HKZ1; +const float HKZ18 = HKZ17*P(0,0); +const float HKZ19 = 2*HKZ10 - 2*HKZ9; +const float HKZ20 = HKZ19*P(0,17); +const float HKZ21 = HKZ11 + HKZ12 - 1; +const float HKZ22 = HKZ21*P(0,18); +const float HKZ23 = 2*HKZ6; +const float HKZ24 = HKZ23*P(0,2); +const float HKZ25 = -2*HKZ2 + 2*HKZ4 + 2*HKZ5; +const float HKZ26 = HKZ25*P(0,1); +const float HKZ27 = HKZ13*P(16,18); +const float HKZ28 = HKZ15*P(3,18); +const float HKZ29 = HKZ17*P(0,18); +const float HKZ30 = HKZ19*P(17,18); +const float HKZ31 = HKZ23*P(2,18); +const float HKZ32 = HKZ25*P(1,18); +const float HKZ33 = HKZ21*P(18,18); +const float HKZ34 = HKZ13*P(3,16); +const float HKZ35 = HKZ15*P(3,3); +const float HKZ36 = HKZ17*P(0,3); +const float HKZ37 = HKZ19*P(3,17); +const float HKZ38 = HKZ23*P(2,3); +const float HKZ39 = HKZ25*P(1,3); +const float HKZ40 = HKZ21*P(3,18); +const float HKZ41 = HKZ13*P(16,16); +const float HKZ42 = HKZ15*P(3,16); +const float HKZ43 = HKZ17*P(0,16); +const float HKZ44 = HKZ19*P(16,17); +const float HKZ45 = HKZ23*P(2,16); +const float HKZ46 = HKZ25*P(1,16); +const float HKZ47 = HKZ21*P(16,18); +const float HKZ48 = HKZ13*P(2,16); +const float HKZ49 = HKZ15*P(2,3); +const float HKZ50 = HKZ17*P(0,2); +const float HKZ51 = HKZ19*P(2,17); +const float HKZ52 = HKZ23*P(2,2); +const float HKZ53 = HKZ25*P(1,2); +const float HKZ54 = HKZ21*P(2,18); +const float HKZ55 = HKZ13*P(16,17); +const float HKZ56 = HKZ15*P(3,17); +const float HKZ57 = HKZ17*P(0,17); +const float HKZ58 = HKZ19*P(17,17); +const float HKZ59 = HKZ23*P(2,17); +const float HKZ60 = HKZ25*P(1,17); +const float HKZ61 = HKZ21*P(17,18); +const float HKZ62 = HKZ13*P(1,16); +const float HKZ63 = HKZ15*P(1,3); +const float HKZ64 = HKZ17*P(0,1); +const float HKZ65 = HKZ19*P(1,17); +const float HKZ66 = HKZ23*P(1,2); +const float HKZ67 = HKZ25*P(1,1); +const float HKZ68 = HKZ21*P(1,18); +const float HKZ69 = -HKZ13*P(16,21) - HKZ15*P(3,21) + HKZ17*P(0,21) + HKZ19*P(17,21) + HKZ21*P(18,21) - HKZ23*P(2,21) + HKZ25*P(1,21) - P(21,21); +const float HKZ70 = 1.0F/(-HKZ13*(HKZ41 + HKZ42 - HKZ43 - HKZ44 + HKZ45 - HKZ46 - HKZ47 + P(16,21)) - HKZ15*(HKZ34 + HKZ35 - HKZ36 - HKZ37 + HKZ38 - HKZ39 - HKZ40 + P(3,21)) + HKZ17*(HKZ14 + HKZ16 - HKZ18 - HKZ20 - HKZ22 + HKZ24 - HKZ26 + P(0,21)) + HKZ19*(HKZ55 + HKZ56 - HKZ57 - HKZ58 + HKZ59 - HKZ60 - HKZ61 + P(17,21)) + HKZ21*(HKZ27 + HKZ28 - HKZ29 - HKZ30 + HKZ31 - HKZ32 - HKZ33 + P(18,21)) - HKZ23*(HKZ48 + HKZ49 - HKZ50 - HKZ51 + HKZ52 - HKZ53 - HKZ54 + P(2,21)) + HKZ25*(HKZ62 + HKZ63 - HKZ64 - HKZ65 + HKZ66 - HKZ67 - HKZ68 + P(1,21)) + HKZ69 - R_MAG); // Observation Jacobians -Hfusion.at<0>() = 2*HKZ0; -Hfusion.at<1>() = 2*HKZ1 - 2*HKZ2 - 2*HKZ3; -Hfusion.at<2>() = 2*HKZ4; -Hfusion.at<3>() = 2*HKZ5; +Hfusion.at<0>() = 2*HKZ0 - 2*HKZ1; +Hfusion.at<1>() = 2*HKZ2 - 2*HKZ4 - 2*HKZ5; +Hfusion.at<2>() = 2*HKZ6; +Hfusion.at<3>() = 2*HKZ7; Hfusion.at<4>() = 0; Hfusion.at<5>() = 0; Hfusion.at<6>() = 0; @@ -210,9 +305,9 @@ Hfusion.at<12>() = 0; Hfusion.at<13>() = 0; Hfusion.at<14>() = 0; Hfusion.at<15>() = 0; -Hfusion.at<16>() = 2*HKZ6; -Hfusion.at<17>() = 2*HKZ7 - 2*HKZ8; -Hfusion.at<18>() = HKZ9; +Hfusion.at<16>() = 2*HKZ8; +Hfusion.at<17>() = -2*HKZ10 + 2*HKZ9; +Hfusion.at<18>() = -HKZ11 - HKZ12 + 1; Hfusion.at<19>() = 0; Hfusion.at<20>() = 0; Hfusion.at<21>() = 1; @@ -221,29 +316,29 @@ Hfusion.at<23>() = 0; // Kalman gains -Kfusion(0) = HKZ16*HKZ24; -Kfusion(1) = HKZ19*HKZ24; -Kfusion(2) = HKZ22*HKZ24; -Kfusion(3) = HKZ21*HKZ24; -Kfusion(4) = HKZ24*(HKZ10*P(4,16) - HKZ11*P(4,17) + HKZ12*P(3,4) + HKZ13*P(0,4) - HKZ14*P(1,4) + HKZ15*P(2,4) + HKZ9*P(4,18) + P(4,21)); -Kfusion(5) = HKZ24*(HKZ10*P(5,16) - HKZ11*P(5,17) + HKZ12*P(3,5) + HKZ13*P(0,5) - HKZ14*P(1,5) + HKZ15*P(2,5) + HKZ9*P(5,18) + P(5,21)); -Kfusion(6) = HKZ24*(HKZ10*P(6,16) - HKZ11*P(6,17) + HKZ12*P(3,6) + HKZ13*P(0,6) - HKZ14*P(1,6) + HKZ15*P(2,6) + HKZ9*P(6,18) + P(6,21)); -Kfusion(7) = HKZ24*(HKZ10*P(7,16) - HKZ11*P(7,17) + HKZ12*P(3,7) + HKZ13*P(0,7) - HKZ14*P(1,7) + HKZ15*P(2,7) + HKZ9*P(7,18) + P(7,21)); -Kfusion(8) = HKZ24*(HKZ10*P(8,16) - HKZ11*P(8,17) + HKZ12*P(3,8) + HKZ13*P(0,8) - HKZ14*P(1,8) + HKZ15*P(2,8) + HKZ9*P(8,18) + P(8,21)); -Kfusion(9) = HKZ24*(HKZ10*P(9,16) - HKZ11*P(9,17) + HKZ12*P(3,9) + HKZ13*P(0,9) - HKZ14*P(1,9) + HKZ15*P(2,9) + HKZ9*P(9,18) + P(9,21)); -Kfusion(10) = HKZ24*(HKZ10*P(10,16) - HKZ11*P(10,17) + HKZ12*P(3,10) + HKZ13*P(0,10) - HKZ14*P(1,10) + HKZ15*P(2,10) + HKZ9*P(10,18) + P(10,21)); -Kfusion(11) = HKZ24*(HKZ10*P(11,16) - HKZ11*P(11,17) + HKZ12*P(3,11) + HKZ13*P(0,11) - HKZ14*P(1,11) + HKZ15*P(2,11) + HKZ9*P(11,18) + P(11,21)); -Kfusion(12) = HKZ24*(HKZ10*P(12,16) - HKZ11*P(12,17) + HKZ12*P(3,12) + HKZ13*P(0,12) - HKZ14*P(1,12) + HKZ15*P(2,12) + HKZ9*P(12,18) + P(12,21)); -Kfusion(13) = HKZ24*(HKZ10*P(13,16) - HKZ11*P(13,17) + HKZ12*P(3,13) + HKZ13*P(0,13) - HKZ14*P(1,13) + HKZ15*P(2,13) + HKZ9*P(13,18) + P(13,21)); -Kfusion(14) = HKZ24*(HKZ10*P(14,16) - HKZ11*P(14,17) + HKZ12*P(3,14) + HKZ13*P(0,14) - HKZ14*P(1,14) + HKZ15*P(2,14) + HKZ9*P(14,18) + P(14,21)); -Kfusion(15) = HKZ24*(HKZ10*P(15,16) - HKZ11*P(15,17) + HKZ12*P(3,15) + HKZ13*P(0,15) - HKZ14*P(1,15) + HKZ15*P(2,15) + HKZ9*P(15,18) + P(15,21)); -Kfusion(16) = HKZ20*HKZ24; -Kfusion(17) = HKZ18*HKZ24; -Kfusion(18) = HKZ17*HKZ24; -Kfusion(19) = HKZ24*(HKZ10*P(16,19) - HKZ11*P(17,19) + HKZ12*P(3,19) + HKZ13*P(0,19) - HKZ14*P(1,19) + HKZ15*P(2,19) + HKZ9*P(18,19) + P(19,21)); -Kfusion(20) = HKZ24*(HKZ10*P(16,20) - HKZ11*P(17,20) + HKZ12*P(3,20) + HKZ13*P(0,20) - HKZ14*P(1,20) + HKZ15*P(2,20) + HKZ9*P(18,20) + P(20,21)); -Kfusion(21) = HKZ23*HKZ24; -Kfusion(22) = HKZ24*(HKZ10*P(16,22) - HKZ11*P(17,22) + HKZ12*P(3,22) + HKZ13*P(0,22) - HKZ14*P(1,22) + HKZ15*P(2,22) + HKZ9*P(18,22) + P(21,22)); -Kfusion(23) = HKZ24*(HKZ10*P(16,23) - HKZ11*P(17,23) + HKZ12*P(3,23) + HKZ13*P(0,23) - HKZ14*P(1,23) + HKZ15*P(2,23) + HKZ9*P(18,23) + P(21,23)); +Kfusion(0) = HKZ70*(-HKZ14 - HKZ16 + HKZ18 + HKZ20 + HKZ22 - HKZ24 + HKZ26 - P(0,21)); +Kfusion(1) = HKZ70*(-HKZ62 - HKZ63 + HKZ64 + HKZ65 - HKZ66 + HKZ67 + HKZ68 - P(1,21)); +Kfusion(2) = HKZ70*(-HKZ48 - HKZ49 + HKZ50 + HKZ51 - HKZ52 + HKZ53 + HKZ54 - P(2,21)); +Kfusion(3) = HKZ70*(-HKZ34 - HKZ35 + HKZ36 + HKZ37 - HKZ38 + HKZ39 + HKZ40 - P(3,21)); +Kfusion(4) = HKZ70*(-HKZ13*P(4,16) - HKZ15*P(3,4) + HKZ17*P(0,4) + HKZ19*P(4,17) + HKZ21*P(4,18) - HKZ23*P(2,4) + HKZ25*P(1,4) - P(4,21)); +Kfusion(5) = HKZ70*(-HKZ13*P(5,16) - HKZ15*P(3,5) + HKZ17*P(0,5) + HKZ19*P(5,17) + HKZ21*P(5,18) - HKZ23*P(2,5) + HKZ25*P(1,5) - P(5,21)); +Kfusion(6) = HKZ70*(-HKZ13*P(6,16) - HKZ15*P(3,6) + HKZ17*P(0,6) + HKZ19*P(6,17) + HKZ21*P(6,18) - HKZ23*P(2,6) + HKZ25*P(1,6) - P(6,21)); +Kfusion(7) = HKZ70*(-HKZ13*P(7,16) - HKZ15*P(3,7) + HKZ17*P(0,7) + HKZ19*P(7,17) + HKZ21*P(7,18) - HKZ23*P(2,7) + HKZ25*P(1,7) - P(7,21)); +Kfusion(8) = HKZ70*(-HKZ13*P(8,16) - HKZ15*P(3,8) + HKZ17*P(0,8) + HKZ19*P(8,17) + HKZ21*P(8,18) - HKZ23*P(2,8) + HKZ25*P(1,8) - P(8,21)); +Kfusion(9) = HKZ70*(-HKZ13*P(9,16) - HKZ15*P(3,9) + HKZ17*P(0,9) + HKZ19*P(9,17) + HKZ21*P(9,18) - HKZ23*P(2,9) + HKZ25*P(1,9) - P(9,21)); +Kfusion(10) = HKZ70*(-HKZ13*P(10,16) - HKZ15*P(3,10) + HKZ17*P(0,10) + HKZ19*P(10,17) + HKZ21*P(10,18) - HKZ23*P(2,10) + HKZ25*P(1,10) - P(10,21)); +Kfusion(11) = HKZ70*(-HKZ13*P(11,16) - HKZ15*P(3,11) + HKZ17*P(0,11) + HKZ19*P(11,17) + HKZ21*P(11,18) - HKZ23*P(2,11) + HKZ25*P(1,11) - P(11,21)); +Kfusion(12) = HKZ70*(-HKZ13*P(12,16) - HKZ15*P(3,12) + HKZ17*P(0,12) + HKZ19*P(12,17) + HKZ21*P(12,18) - HKZ23*P(2,12) + HKZ25*P(1,12) - P(12,21)); +Kfusion(13) = HKZ70*(-HKZ13*P(13,16) - HKZ15*P(3,13) + HKZ17*P(0,13) + HKZ19*P(13,17) + HKZ21*P(13,18) - HKZ23*P(2,13) + HKZ25*P(1,13) - P(13,21)); +Kfusion(14) = HKZ70*(-HKZ13*P(14,16) - HKZ15*P(3,14) + HKZ17*P(0,14) + HKZ19*P(14,17) + HKZ21*P(14,18) - HKZ23*P(2,14) + HKZ25*P(1,14) - P(14,21)); +Kfusion(15) = HKZ70*(-HKZ13*P(15,16) - HKZ15*P(3,15) + HKZ17*P(0,15) + HKZ19*P(15,17) + HKZ21*P(15,18) - HKZ23*P(2,15) + HKZ25*P(1,15) - P(15,21)); +Kfusion(16) = HKZ70*(-HKZ41 - HKZ42 + HKZ43 + HKZ44 - HKZ45 + HKZ46 + HKZ47 - P(16,21)); +Kfusion(17) = HKZ70*(-HKZ55 - HKZ56 + HKZ57 + HKZ58 - HKZ59 + HKZ60 + HKZ61 - P(17,21)); +Kfusion(18) = HKZ70*(-HKZ27 - HKZ28 + HKZ29 + HKZ30 - HKZ31 + HKZ32 + HKZ33 - P(18,21)); +Kfusion(19) = HKZ70*(-HKZ13*P(16,19) - HKZ15*P(3,19) + HKZ17*P(0,19) + HKZ19*P(17,19) + HKZ21*P(18,19) - HKZ23*P(2,19) + HKZ25*P(1,19) - P(19,21)); +Kfusion(20) = HKZ70*(-HKZ13*P(16,20) - HKZ15*P(3,20) + HKZ17*P(0,20) + HKZ19*P(17,20) + HKZ21*P(18,20) - HKZ23*P(2,20) + HKZ25*P(1,20) - P(20,21)); +Kfusion(21) = HKZ69*HKZ70; +Kfusion(22) = HKZ70*(-HKZ13*P(16,22) - HKZ15*P(3,22) + HKZ17*P(0,22) + HKZ19*P(17,22) + HKZ21*P(18,22) - HKZ23*P(2,22) + HKZ25*P(1,22) - P(21,22)); +Kfusion(23) = HKZ70*(-HKZ13*P(16,23) - HKZ15*P(3,23) + HKZ17*P(0,23) + HKZ19*P(17,23) + HKZ21*P(18,23) - HKZ23*P(2,23) + HKZ25*P(1,23) - P(21,23)); diff --git a/EKF/python/ekf_derivation/generated/3Dmag_generated_alt.cpp b/EKF/python/ekf_derivation/generated/3Dmag_generated_alt.cpp index a7d8690055..667895c435 100644 --- a/EKF/python/ekf_derivation/generated/3Dmag_generated_alt.cpp +++ b/EKF/python/ekf_derivation/generated/3Dmag_generated_alt.cpp @@ -1,96 +1,186 @@ // Sub Expressions const float HK0 = magE*q3; -const float HK1 = magN*q0; -const float HK2 = magD*q2; -const float HK3 = HK0 + HK1 - HK2; -const float HK4 = 2*HK3; -const float HK5 = magD*q3 + magE*q2 + magN*q1; -const float HK6 = 2*HK5; -const float HK7 = magE*q1; -const float HK8 = magD*q0; -const float HK9 = magN*q2; +const float HK1 = magD*q2; +const float HK2 = -HK1; +const float HK3 = magD*q3; +const float HK4 = magE*q2; +const float HK5 = HK3 + HK4; +const float HK6 = magE*q1; +const float HK7 = magD*q0; +const float HK8 = magN*q2; +const float HK9 = 2*HK8; const float HK10 = magD*q1; const float HK11 = magE*q0; const float HK12 = magN*q3; -const float HK13 = HK10 + HK11 - HK12; -const float HK14 = 2*HK13; -const float HK15 = powf(q1, 2); -const float HK16 = powf(q2, 2); -const float HK17 = -HK16; -const float HK18 = powf(q0, 2); -const float HK19 = powf(q3, 2); -const float HK20 = HK18 - HK19; -const float HK21 = HK15 + HK17 + HK20; -const float HK22 = q0*q3; -const float HK23 = q1*q2; -const float HK24 = HK22 + HK23; -const float HK25 = q1*q3; -const float HK26 = q0*q2; -const float HK27 = 2*HK24; -const float HK28 = -2*HK25 + 2*HK26; -const float HK29 = 2*HK5; -const float HK30 = 2*HK3; -const float HK31 = -HK7 + HK8 + HK9; -const float HK32 = 2*HK31; -const float HK33 = HK32*P(0,2); +const float HK13 = HK10 + HK11 - 2*HK12; +const float HK14 = 2*powf(q2, 2); +const float HK15 = -HK14; +const float HK16 = 2*powf(q3, 2); +const float HK17 = 1 - HK16; +const float HK18 = q0*q3; +const float HK19 = q1*q2; +const float HK20 = HK18 + HK19; +const float HK21 = q1*q3; +const float HK22 = q0*q2; +const float HK23 = 2*HK5; +const float HK24 = HK23*P(0,1); +const float HK25 = 2*HK20; +const float HK26 = HK25*P(0,17); +const float HK27 = -2*HK0 + 2*HK1; +const float HK28 = HK27*P(0,0); +const float HK29 = -2*HK21 + 2*HK22; +const float HK30 = HK29*P(0,18); +const float HK31 = HK16 - 1; +const float HK32 = HK14 + HK31; +const float HK33 = HK32*P(0,16); const float HK34 = 2*HK13; const float HK35 = HK34*P(0,3); -const float HK36 = HK21*P(0,16) + HK27*P(0,17) - HK28*P(0,18) + HK29*P(0,1) + HK30*P(0,0) - HK33 + HK35 + P(0,19); -const float HK37 = HK21*P(16,16) + HK27*P(16,17) - HK28*P(16,18) + HK29*P(1,16) + HK30*P(0,16) - HK32*P(2,16) + HK34*P(3,16) + P(16,19); -const float HK38 = HK21*P(16,18) + HK27*P(17,18) - HK28*P(18,18) + HK29*P(1,18) + HK30*P(0,18) - HK32*P(2,18) + HK34*P(3,18) + P(18,19); -const float HK39 = HK29*P(1,2); -const float HK40 = HK30*P(0,2); -const float HK41 = HK21*P(2,16) + HK27*P(2,17) - HK28*P(2,18) - HK32*P(2,2) + HK34*P(2,3) + HK39 + HK40 + P(2,19); -const float HK42 = HK21*P(16,17) + HK27*P(17,17) - HK28*P(17,18) + HK29*P(1,17) + HK30*P(0,17) - HK32*P(2,17) + HK34*P(3,17) + P(17,19); -const float HK43 = HK29*P(1,3); -const float HK44 = HK30*P(0,3); -const float HK45 = HK21*P(3,16) + HK27*P(3,17) - HK28*P(3,18) - HK32*P(2,3) + HK34*P(3,3) + HK43 + HK44 + P(3,19); -const float HK46 = HK32*P(1,2); -const float HK47 = HK34*P(1,3); -const float HK48 = HK21*P(1,16) + HK27*P(1,17) - HK28*P(1,18) + HK29*P(1,1) + HK30*P(0,1) - HK46 + HK47 + P(1,19); -const float HK49 = HK21*P(16,19) + HK27*P(17,19) - HK28*P(18,19) + HK29*P(1,19) + HK30*P(0,19) - HK32*P(2,19) + HK34*P(3,19) + P(19,19); -const float HK50 = 1.0F/(HK21*HK37 + HK27*HK42 - HK28*HK38 + HK29*HK48 + HK30*HK36 - HK32*HK41 + HK34*HK45 + HK49 + R_MAG); -const float HK51 = 2*HK31; -const float HK52 = -HK15; -const float HK53 = HK16 + HK20 + HK52; -const float HK54 = q0*q1; -const float HK55 = q2*q3; -const float HK56 = HK54 + HK55; -const float HK57 = 2*HK56; -const float HK58 = 2*HK22 - 2*HK23; -const float HK59 = HK32*P(0,1); -const float HK60 = HK29*P(0,2) + HK34*P(0,0) - HK44 + HK53*P(0,17) + HK57*P(0,18) - HK58*P(0,16) + HK59 + P(0,20); -const float HK61 = HK29*P(2,17) - HK30*P(3,17) + HK32*P(1,17) + HK34*P(0,17) + HK53*P(17,17) + HK57*P(17,18) - HK58*P(16,17) + P(17,20); -const float HK62 = HK29*P(2,16) - HK30*P(3,16) + HK32*P(1,16) + HK34*P(0,16) + HK53*P(16,17) + HK57*P(16,18) - HK58*P(16,16) + P(16,20); -const float HK63 = HK29*P(2,3); -const float HK64 = -HK30*P(3,3) + HK32*P(1,3) + HK35 + HK53*P(3,17) + HK57*P(3,18) - HK58*P(3,16) + HK63 + P(3,20); -const float HK65 = HK29*P(2,18) - HK30*P(3,18) + HK32*P(1,18) + HK34*P(0,18) + HK53*P(17,18) + HK57*P(18,18) - HK58*P(16,18) + P(18,20); -const float HK66 = HK34*P(0,1); -const float HK67 = -HK30*P(1,3) + HK32*P(1,1) + HK39 + HK53*P(1,17) + HK57*P(1,18) - HK58*P(1,16) + HK66 + P(1,20); -const float HK68 = HK30*P(2,3); -const float HK69 = HK29*P(2,2) + HK34*P(0,2) + HK46 + HK53*P(2,17) + HK57*P(2,18) - HK58*P(2,16) - HK68 + P(2,20); -const float HK70 = HK29*P(2,20) - HK30*P(3,20) + HK32*P(1,20) + HK34*P(0,20) + HK53*P(17,20) + HK57*P(18,20) - HK58*P(16,20) + P(20,20); -const float HK71 = 1.0F/(HK29*HK69 - HK30*HK64 + HK32*HK67 + HK34*HK60 + HK53*HK61 + HK57*HK65 - HK58*HK62 + HK70 + R_MAG); -const float HK72 = HK25 + HK26; -const float HK73 = HK17 + HK18 + HK19 + HK52; -const float HK74 = 2*HK72; -const float HK75 = 2*HK54 - 2*HK55; -const float HK76 = HK29*P(0,3) + HK32*P(0,0) + HK40 - HK66 + HK73*P(0,18) + HK74*P(0,16) - HK75*P(0,17) + P(0,21); -const float HK77 = HK29*P(3,18) + HK30*P(2,18) + HK32*P(0,18) - HK34*P(1,18) + HK73*P(18,18) + HK74*P(16,18) - HK75*P(17,18) + P(18,21); -const float HK78 = HK29*P(3,17) + HK30*P(2,17) + HK32*P(0,17) - HK34*P(1,17) + HK73*P(17,18) + HK74*P(16,17) - HK75*P(17,17) + P(17,21); -const float HK79 = HK30*P(1,2) - HK34*P(1,1) + HK43 + HK59 + HK73*P(1,18) + HK74*P(1,16) - HK75*P(1,17) + P(1,21); -const float HK80 = HK29*P(3,16) + HK30*P(2,16) + HK32*P(0,16) - HK34*P(1,16) + HK73*P(16,18) + HK74*P(16,16) - HK75*P(16,17) + P(16,21); -const float HK81 = HK29*P(3,3) + HK32*P(0,3) - HK47 + HK68 + HK73*P(3,18) + HK74*P(3,16) - HK75*P(3,17) + P(3,21); -const float HK82 = HK30*P(2,2) + HK33 - HK34*P(1,2) + HK63 + HK73*P(2,18) + HK74*P(2,16) - HK75*P(2,17) + P(2,21); -const float HK83 = HK29*P(3,21) + HK30*P(2,21) + HK32*P(0,21) - HK34*P(1,21) + HK73*P(18,21) + HK74*P(16,21) - HK75*P(17,21) + P(21,21); -const float HK84 = 1.0F/(HK29*HK81 + HK30*HK82 + HK32*HK76 - HK34*HK79 + HK73*HK77 + HK74*HK80 - HK75*HK78 + HK83 + R_MAG); +const float HK36 = -HK6; +const float HK37 = 2*HK36 + 2*HK7 + 2*HK9; +const float HK38 = HK37*P(0,2); +const float HK39 = -R_MAG; +const float HK40 = HK23*P(1,16); +const float HK41 = HK25*P(16,17); +const float HK42 = HK27*P(0,16); +const float HK43 = HK29*P(16,18); +const float HK44 = HK34*P(3,16); +const float HK45 = HK32*P(16,16); +const float HK46 = HK37*P(2,16); +const float HK47 = HK23*P(1,1); +const float HK48 = HK25*P(1,17); +const float HK49 = HK27*P(0,1); +const float HK50 = HK29*P(1,18); +const float HK51 = HK34*P(1,3); +const float HK52 = HK32*P(1,16); +const float HK53 = HK37*P(1,2); +const float HK54 = HK23*P(1,17); +const float HK55 = HK25*P(17,17); +const float HK56 = HK27*P(0,17); +const float HK57 = HK29*P(17,18); +const float HK58 = HK34*P(3,17); +const float HK59 = HK32*P(16,17); +const float HK60 = HK37*P(2,17); +const float HK61 = HK23*P(1,3); +const float HK62 = HK25*P(3,17); +const float HK63 = HK27*P(0,3); +const float HK64 = HK29*P(3,18); +const float HK65 = HK34*P(3,3); +const float HK66 = HK37*P(2,3); +const float HK67 = HK32*P(3,16); +const float HK68 = HK23*P(1,18); +const float HK69 = HK25*P(17,18); +const float HK70 = HK27*P(0,18); +const float HK71 = HK29*P(18,18); +const float HK72 = HK34*P(3,18); +const float HK73 = HK32*P(16,18); +const float HK74 = HK37*P(2,18); +const float HK75 = HK23*P(1,2); +const float HK76 = HK25*P(2,17); +const float HK77 = HK27*P(0,2); +const float HK78 = HK29*P(2,18); +const float HK79 = HK34*P(2,3); +const float HK80 = HK32*P(2,16); +const float HK81 = HK37*P(2,2); +const float HK82 = -HK23*P(1,19) - HK25*P(17,19) + HK27*P(0,19) + HK29*P(18,19) + HK32*P(16,19) - HK34*P(3,19) + HK37*P(2,19) - P(19,19); +const float HK83 = 1.0F/(-HK23*(HK47 + HK48 - HK49 - HK50 + HK51 - HK52 - HK53 + P(1,19)) - HK25*(HK54 + HK55 - HK56 - HK57 + HK58 - HK59 - HK60 + P(17,19)) + HK27*(HK24 + HK26 - HK28 - HK30 - HK33 + HK35 - HK38 + P(0,19)) + HK29*(HK68 + HK69 - HK70 - HK71 + HK72 - HK73 - HK74 + P(18,19)) + HK32*(HK40 + HK41 - HK42 - HK43 + HK44 - HK45 - HK46 + P(16,19)) - HK34*(HK61 + HK62 - HK63 - HK64 + HK65 - HK66 - HK67 + P(3,19)) + HK37*(HK75 + HK76 - HK77 - HK78 + HK79 - HK80 - HK81 + P(2,19)) + HK39 + HK82); +const float HK84 = -P(19,21); +const float HK85 = -HK12; +const float HK86 = HK10 + HK85; +const float HK87 = -2*HK6 + HK7 + HK8; +const float HK88 = magN*q1; +const float HK89 = HK3 + HK88; +const float HK90 = 2*HK0; +const float HK91 = magN*q0; +const float HK92 = 2*powf(q1, 2); +const float HK93 = -HK92; +const float HK94 = q0*q1; +const float HK95 = q2*q3; +const float HK96 = HK94 + HK95; +const float HK97 = 2*HK96; +const float HK98 = 2*HK89; +const float HK99 = 2*HK86; +const float HK100 = 2*HK18 - 2*HK19; +const float HK101 = 2*HK87; +const float HK102 = HK31 + HK92; +const float HK103 = 2*HK2 + 2*HK90 + 2*HK91; +const float HK104 = -HK100*P(0,16) + HK101*P(0,1) - HK102*P(0,17) - HK103*P(0,3) + HK97*P(0,18) + HK98*P(0,2) + HK99*P(0,0) + P(0,20); +const float HK105 = -HK100*P(16,17) + HK101*P(1,17) - HK102*P(17,17) - HK103*P(3,17) + HK97*P(17,18) + HK98*P(2,17) + HK99*P(0,17) + P(17,20); +const float HK106 = -HK100*P(16,16) + HK101*P(1,16) - HK102*P(16,17) - HK103*P(3,16) + HK97*P(16,18) + HK98*P(2,16) + HK99*P(0,16) + P(16,20); +const float HK107 = -HK100*P(3,16) + HK101*P(1,3) - HK102*P(3,17) - HK103*P(3,3) + HK97*P(3,18) + HK98*P(2,3) + HK99*P(0,3) + P(3,20); +const float HK108 = -HK100*P(2,16) + HK101*P(1,2) - HK102*P(2,17) - HK103*P(2,3) + HK97*P(2,18) + HK98*P(2,2) + HK99*P(0,2) + P(2,20); +const float HK109 = -HK100*P(16,18) + HK101*P(1,18) - HK102*P(17,18) - HK103*P(3,18) + HK97*P(18,18) + HK98*P(2,18) + HK99*P(0,18) + P(18,20); +const float HK110 = -HK100*P(1,16) + HK101*P(1,1) - HK102*P(1,17) - HK103*P(1,3) + HK97*P(1,18) + HK98*P(1,2) + HK99*P(0,1) + P(1,20); +const float HK111 = -HK100*P(16,20) + HK101*P(1,20) - HK102*P(17,20) - HK103*P(3,20) + HK97*P(18,20) + HK98*P(2,20) + HK99*P(0,20) + P(20,20); +const float HK112 = 1.0F/(-HK100*HK106 + HK101*HK110 - HK102*HK105 - HK103*HK107 + HK104*HK99 + HK108*HK98 + HK109*HK97 + HK111 + R_MAG); +const float HK113 = 2*HK10; +const float HK114 = HK0 - 2*HK1 + HK91; +const float HK115 = HK4 + HK88; +const float HK116 = HK21 + HK22; +const float HK117 = 2*HK116; +const float HK118 = HK117*P(0,16); +const float HK119 = 2*HK115; +const float HK120 = HK119*P(0,3); +const float HK121 = 2*HK6 - 2*HK8; +const float HK122 = HK121*P(0,0); +const float HK123 = 2*HK94 - 2*HK95; +const float HK124 = HK123*P(0,17); +const float HK125 = HK14 + HK92 - 1; +const float HK126 = HK125*P(0,18); +const float HK127 = 2*HK114; +const float HK128 = HK127*P(0,2); +const float HK129 = 2*HK11 + 2*HK113 + 2*HK85; +const float HK130 = HK129*P(0,1); +const float HK131 = HK117*P(16,18); +const float HK132 = HK119*P(3,18); +const float HK133 = HK121*P(0,18); +const float HK134 = HK123*P(17,18); +const float HK135 = HK127*P(2,18); +const float HK136 = HK129*P(1,18); +const float HK137 = HK125*P(18,18); +const float HK138 = HK117*P(3,16); +const float HK139 = HK119*P(3,3); +const float HK140 = HK121*P(0,3); +const float HK141 = HK123*P(3,17); +const float HK142 = HK127*P(2,3); +const float HK143 = HK129*P(1,3); +const float HK144 = HK125*P(3,18); +const float HK145 = HK117*P(16,16); +const float HK146 = HK119*P(3,16); +const float HK147 = HK121*P(0,16); +const float HK148 = HK123*P(16,17); +const float HK149 = HK127*P(2,16); +const float HK150 = HK129*P(1,16); +const float HK151 = HK125*P(16,18); +const float HK152 = HK117*P(2,16); +const float HK153 = HK119*P(2,3); +const float HK154 = HK121*P(0,2); +const float HK155 = HK123*P(2,17); +const float HK156 = HK127*P(2,2); +const float HK157 = HK129*P(1,2); +const float HK158 = HK125*P(2,18); +const float HK159 = HK117*P(16,17); +const float HK160 = HK119*P(3,17); +const float HK161 = HK121*P(0,17); +const float HK162 = HK123*P(17,17); +const float HK163 = HK127*P(2,17); +const float HK164 = HK129*P(1,17); +const float HK165 = HK125*P(17,18); +const float HK166 = HK117*P(1,16); +const float HK167 = HK119*P(1,3); +const float HK168 = HK121*P(0,1); +const float HK169 = HK123*P(1,17); +const float HK170 = HK127*P(1,2); +const float HK171 = HK129*P(1,1); +const float HK172 = HK125*P(1,18); +const float HK173 = -HK117*P(16,21) - HK119*P(3,21) + HK121*P(0,21) + HK123*P(17,21) + HK125*P(18,21) - HK127*P(2,21) + HK129*P(1,21) - P(21,21); +const float HK174 = 1.0F/(-HK117*(HK145 + HK146 - HK147 - HK148 + HK149 - HK150 - HK151 + P(16,21)) - HK119*(HK138 + HK139 - HK140 - HK141 + HK142 - HK143 - HK144 + P(3,21)) + HK121*(HK118 + HK120 - HK122 - HK124 - HK126 + HK128 - HK130 + P(0,21)) + HK123*(HK159 + HK160 - HK161 - HK162 + HK163 - HK164 - HK165 + P(17,21)) + HK125*(HK131 + HK132 - HK133 - HK134 + HK135 - HK136 - HK137 + P(18,21)) - HK127*(HK152 + HK153 - HK154 - HK155 + HK156 - HK157 - HK158 + P(2,21)) + HK129*(HK166 + HK167 - HK168 - HK169 + HK170 - HK171 - HK172 + P(1,21)) + HK173 + HK39); // Observation Jacobians - axis 0 -Hfusion.at<0>() = HK4; -Hfusion.at<1>() = HK6; -Hfusion.at<2>() = 2*HK7 - 2*HK8 - 2*HK9; -Hfusion.at<3>() = HK14; +Hfusion.at<0>() = 2*HK0 + 2*HK2; +Hfusion.at<1>() = 2*HK5; +Hfusion.at<2>() = 2*HK6 - 2*HK7 - 2*HK9; +Hfusion.at<3>() = 2*HK13; Hfusion.at<4>() = 0; Hfusion.at<5>() = 0; Hfusion.at<6>() = 0; @@ -103,9 +193,9 @@ Hfusion.at<12>() = 0; Hfusion.at<13>() = 0; Hfusion.at<14>() = 0; Hfusion.at<15>() = 0; -Hfusion.at<16>() = HK21; -Hfusion.at<17>() = 2*HK24; -Hfusion.at<18>() = 2*HK25 - 2*HK26; +Hfusion.at<16>() = HK15 + HK17; +Hfusion.at<17>() = 2*HK20; +Hfusion.at<18>() = 2*HK21 - 2*HK22; Hfusion.at<19>() = 1; Hfusion.at<20>() = 0; Hfusion.at<21>() = 0; @@ -114,37 +204,37 @@ Hfusion.at<23>() = 0; // Kalman gains - axis 0 -Kfusion(0) = HK36*HK50; -Kfusion(1) = HK48*HK50; -Kfusion(2) = HK41*HK50; -Kfusion(3) = HK45*HK50; -Kfusion(4) = HK50*(HK21*P(4,16) + HK27*P(4,17) - HK28*P(4,18) + HK29*P(1,4) + HK30*P(0,4) - HK32*P(2,4) + HK34*P(3,4) + P(4,19)); -Kfusion(5) = HK50*(HK21*P(5,16) + HK27*P(5,17) - HK28*P(5,18) + HK29*P(1,5) + HK30*P(0,5) - HK32*P(2,5) + HK34*P(3,5) + P(5,19)); -Kfusion(6) = HK50*(HK21*P(6,16) + HK27*P(6,17) - HK28*P(6,18) + HK29*P(1,6) + HK30*P(0,6) - HK32*P(2,6) + HK34*P(3,6) + P(6,19)); -Kfusion(7) = HK50*(HK21*P(7,16) + HK27*P(7,17) - HK28*P(7,18) + HK29*P(1,7) + HK30*P(0,7) - HK32*P(2,7) + HK34*P(3,7) + P(7,19)); -Kfusion(8) = HK50*(HK21*P(8,16) + HK27*P(8,17) - HK28*P(8,18) + HK29*P(1,8) + HK30*P(0,8) - HK32*P(2,8) + HK34*P(3,8) + P(8,19)); -Kfusion(9) = HK50*(HK21*P(9,16) + HK27*P(9,17) - HK28*P(9,18) + HK29*P(1,9) + HK30*P(0,9) - HK32*P(2,9) + HK34*P(3,9) + P(9,19)); -Kfusion(10) = HK50*(HK21*P(10,16) + HK27*P(10,17) - HK28*P(10,18) + HK29*P(1,10) + HK30*P(0,10) - HK32*P(2,10) + HK34*P(3,10) + P(10,19)); -Kfusion(11) = HK50*(HK21*P(11,16) + HK27*P(11,17) - HK28*P(11,18) + HK29*P(1,11) + HK30*P(0,11) - HK32*P(2,11) + HK34*P(3,11) + P(11,19)); -Kfusion(12) = HK50*(HK21*P(12,16) + HK27*P(12,17) - HK28*P(12,18) + HK29*P(1,12) + HK30*P(0,12) - HK32*P(2,12) + HK34*P(3,12) + P(12,19)); -Kfusion(13) = HK50*(HK21*P(13,16) + HK27*P(13,17) - HK28*P(13,18) + HK29*P(1,13) + HK30*P(0,13) - HK32*P(2,13) + HK34*P(3,13) + P(13,19)); -Kfusion(14) = HK50*(HK21*P(14,16) + HK27*P(14,17) - HK28*P(14,18) + HK29*P(1,14) + HK30*P(0,14) - HK32*P(2,14) + HK34*P(3,14) + P(14,19)); -Kfusion(15) = HK50*(HK21*P(15,16) + HK27*P(15,17) - HK28*P(15,18) + HK29*P(1,15) + HK30*P(0,15) - HK32*P(2,15) + HK34*P(3,15) + P(15,19)); -Kfusion(16) = HK37*HK50; -Kfusion(17) = HK42*HK50; -Kfusion(18) = HK38*HK50; -Kfusion(19) = HK49*HK50; -Kfusion(20) = HK50*(HK21*P(16,20) + HK27*P(17,20) - HK28*P(18,20) + HK29*P(1,20) + HK30*P(0,20) - HK32*P(2,20) + HK34*P(3,20) + P(19,20)); -Kfusion(21) = HK50*(HK21*P(16,21) + HK27*P(17,21) - HK28*P(18,21) + HK29*P(1,21) + HK30*P(0,21) - HK32*P(2,21) + HK34*P(3,21) + P(19,21)); -Kfusion(22) = HK50*(HK21*P(16,22) + HK27*P(17,22) - HK28*P(18,22) + HK29*P(1,22) + HK30*P(0,22) - HK32*P(2,22) + HK34*P(3,22) + P(19,22)); -Kfusion(23) = HK50*(HK21*P(16,23) + HK27*P(17,23) - HK28*P(18,23) + HK29*P(1,23) + HK30*P(0,23) - HK32*P(2,23) + HK34*P(3,23) + P(19,23)); +Kfusion(0) = HK83*(-HK24 - HK26 + HK28 + HK30 + HK33 - HK35 + HK38 - P(0,19)); +Kfusion(1) = HK83*(-HK47 - HK48 + HK49 + HK50 - HK51 + HK52 + HK53 - P(1,19)); +Kfusion(2) = HK83*(-HK75 - HK76 + HK77 + HK78 - HK79 + HK80 + HK81 - P(2,19)); +Kfusion(3) = HK83*(-HK61 - HK62 + HK63 + HK64 - HK65 + HK66 + HK67 - P(3,19)); +Kfusion(4) = HK83*(-HK23*P(1,4) - HK25*P(4,17) + HK27*P(0,4) + HK29*P(4,18) + HK32*P(4,16) - HK34*P(3,4) + HK37*P(2,4) - P(4,19)); +Kfusion(5) = HK83*(-HK23*P(1,5) - HK25*P(5,17) + HK27*P(0,5) + HK29*P(5,18) + HK32*P(5,16) - HK34*P(3,5) + HK37*P(2,5) - P(5,19)); +Kfusion(6) = HK83*(-HK23*P(1,6) - HK25*P(6,17) + HK27*P(0,6) + HK29*P(6,18) + HK32*P(6,16) - HK34*P(3,6) + HK37*P(2,6) - P(6,19)); +Kfusion(7) = HK83*(-HK23*P(1,7) - HK25*P(7,17) + HK27*P(0,7) + HK29*P(7,18) + HK32*P(7,16) - HK34*P(3,7) + HK37*P(2,7) - P(7,19)); +Kfusion(8) = HK83*(-HK23*P(1,8) - HK25*P(8,17) + HK27*P(0,8) + HK29*P(8,18) + HK32*P(8,16) - HK34*P(3,8) + HK37*P(2,8) - P(8,19)); +Kfusion(9) = HK83*(-HK23*P(1,9) - HK25*P(9,17) + HK27*P(0,9) + HK29*P(9,18) + HK32*P(9,16) - HK34*P(3,9) + HK37*P(2,9) - P(9,19)); +Kfusion(10) = HK83*(-HK23*P(1,10) - HK25*P(10,17) + HK27*P(0,10) + HK29*P(10,18) + HK32*P(10,16) - HK34*P(3,10) + HK37*P(2,10) - P(10,19)); +Kfusion(11) = HK83*(-HK23*P(1,11) - HK25*P(11,17) + HK27*P(0,11) + HK29*P(11,18) + HK32*P(11,16) - HK34*P(3,11) + HK37*P(2,11) - P(11,19)); +Kfusion(12) = HK83*(-HK23*P(1,12) - HK25*P(12,17) + HK27*P(0,12) + HK29*P(12,18) + HK32*P(12,16) - HK34*P(3,12) + HK37*P(2,12) - P(12,19)); +Kfusion(13) = HK83*(-HK23*P(1,13) - HK25*P(13,17) + HK27*P(0,13) + HK29*P(13,18) + HK32*P(13,16) - HK34*P(3,13) + HK37*P(2,13) - P(13,19)); +Kfusion(14) = HK83*(-HK23*P(1,14) - HK25*P(14,17) + HK27*P(0,14) + HK29*P(14,18) + HK32*P(14,16) - HK34*P(3,14) + HK37*P(2,14) - P(14,19)); +Kfusion(15) = HK83*(-HK23*P(1,15) - HK25*P(15,17) + HK27*P(0,15) + HK29*P(15,18) + HK32*P(15,16) - HK34*P(3,15) + HK37*P(2,15) - P(15,19)); +Kfusion(16) = HK83*(-HK40 - HK41 + HK42 + HK43 - HK44 + HK45 + HK46 - P(16,19)); +Kfusion(17) = HK83*(-HK54 - HK55 + HK56 + HK57 - HK58 + HK59 + HK60 - P(17,19)); +Kfusion(18) = HK83*(-HK68 - HK69 + HK70 + HK71 - HK72 + HK73 + HK74 - P(18,19)); +Kfusion(19) = HK82*HK83; +Kfusion(20) = HK83*(-HK23*P(1,20) - HK25*P(17,20) + HK27*P(0,20) + HK29*P(18,20) + HK32*P(16,20) - HK34*P(3,20) + HK37*P(2,20) - P(19,20)); +Kfusion(21) = HK83*(-HK23*P(1,21) - HK25*P(17,21) + HK27*P(0,21) + HK29*P(18,21) + HK32*P(16,21) - HK34*P(3,21) + HK37*P(2,21) + HK84); +Kfusion(22) = HK83*(-HK23*P(1,22) - HK25*P(17,22) + HK27*P(0,22) + HK29*P(18,22) + HK32*P(16,22) - HK34*P(3,22) + HK37*P(2,22) - P(19,22)); +Kfusion(23) = HK83*(-HK23*P(1,23) - HK25*P(17,23) + HK27*P(0,23) + HK29*P(18,23) + HK32*P(16,23) - HK34*P(3,23) + HK37*P(2,23) - P(19,23)); // Observation Jacobians - axis 1 -Hfusion.at<0>() = HK14; -Hfusion.at<1>() = HK51; -Hfusion.at<2>() = HK6; -Hfusion.at<3>() = -2*HK0 - 2*HK1 + 2*HK2; +Hfusion.at<0>() = 2*HK86; +Hfusion.at<1>() = 2*HK87; +Hfusion.at<2>() = 2*HK89; +Hfusion.at<3>() = 2*HK1 - 2*HK90 - 2*HK91; Hfusion.at<4>() = 0; Hfusion.at<5>() = 0; Hfusion.at<6>() = 0; @@ -157,9 +247,9 @@ Hfusion.at<12>() = 0; Hfusion.at<13>() = 0; Hfusion.at<14>() = 0; Hfusion.at<15>() = 0; -Hfusion.at<16>() = -2*HK22 + 2*HK23; -Hfusion.at<17>() = HK53; -Hfusion.at<18>() = 2*HK56; +Hfusion.at<16>() = -2*HK18 + 2*HK19; +Hfusion.at<17>() = HK17 + HK93; +Hfusion.at<18>() = 2*HK96; Hfusion.at<19>() = 0; Hfusion.at<20>() = 1; Hfusion.at<21>() = 0; @@ -168,37 +258,37 @@ Hfusion.at<23>() = 0; // Kalman gains - axis 1 -Kfusion(0) = HK60*HK71; -Kfusion(1) = HK67*HK71; -Kfusion(2) = HK69*HK71; -Kfusion(3) = HK64*HK71; -Kfusion(4) = HK71*(HK29*P(2,4) - HK30*P(3,4) + HK32*P(1,4) + HK34*P(0,4) + HK53*P(4,17) + HK57*P(4,18) - HK58*P(4,16) + P(4,20)); -Kfusion(5) = HK71*(HK29*P(2,5) - HK30*P(3,5) + HK32*P(1,5) + HK34*P(0,5) + HK53*P(5,17) + HK57*P(5,18) - HK58*P(5,16) + P(5,20)); -Kfusion(6) = HK71*(HK29*P(2,6) - HK30*P(3,6) + HK32*P(1,6) + HK34*P(0,6) + HK53*P(6,17) + HK57*P(6,18) - HK58*P(6,16) + P(6,20)); -Kfusion(7) = HK71*(HK29*P(2,7) - HK30*P(3,7) + HK32*P(1,7) + HK34*P(0,7) + HK53*P(7,17) + HK57*P(7,18) - HK58*P(7,16) + P(7,20)); -Kfusion(8) = HK71*(HK29*P(2,8) - HK30*P(3,8) + HK32*P(1,8) + HK34*P(0,8) + HK53*P(8,17) + HK57*P(8,18) - HK58*P(8,16) + P(8,20)); -Kfusion(9) = HK71*(HK29*P(2,9) - HK30*P(3,9) + HK32*P(1,9) + HK34*P(0,9) + HK53*P(9,17) + HK57*P(9,18) - HK58*P(9,16) + P(9,20)); -Kfusion(10) = HK71*(HK29*P(2,10) - HK30*P(3,10) + HK32*P(1,10) + HK34*P(0,10) + HK53*P(10,17) + HK57*P(10,18) - HK58*P(10,16) + P(10,20)); -Kfusion(11) = HK71*(HK29*P(2,11) - HK30*P(3,11) + HK32*P(1,11) + HK34*P(0,11) + HK53*P(11,17) + HK57*P(11,18) - HK58*P(11,16) + P(11,20)); -Kfusion(12) = HK71*(HK29*P(2,12) - HK30*P(3,12) + HK32*P(1,12) + HK34*P(0,12) + HK53*P(12,17) + HK57*P(12,18) - HK58*P(12,16) + P(12,20)); -Kfusion(13) = HK71*(HK29*P(2,13) - HK30*P(3,13) + HK32*P(1,13) + HK34*P(0,13) + HK53*P(13,17) + HK57*P(13,18) - HK58*P(13,16) + P(13,20)); -Kfusion(14) = HK71*(HK29*P(2,14) - HK30*P(3,14) + HK32*P(1,14) + HK34*P(0,14) + HK53*P(14,17) + HK57*P(14,18) - HK58*P(14,16) + P(14,20)); -Kfusion(15) = HK71*(HK29*P(2,15) - HK30*P(3,15) + HK32*P(1,15) + HK34*P(0,15) + HK53*P(15,17) + HK57*P(15,18) - HK58*P(15,16) + P(15,20)); -Kfusion(16) = HK62*HK71; -Kfusion(17) = HK61*HK71; -Kfusion(18) = HK65*HK71; -Kfusion(19) = HK71*(HK29*P(2,19) - HK30*P(3,19) + HK32*P(1,19) + HK34*P(0,19) + HK53*P(17,19) + HK57*P(18,19) - HK58*P(16,19) + P(19,20)); -Kfusion(20) = HK70*HK71; -Kfusion(21) = HK71*(HK29*P(2,21) - HK30*P(3,21) + HK32*P(1,21) + HK34*P(0,21) + HK53*P(17,21) + HK57*P(18,21) - HK58*P(16,21) + P(20,21)); -Kfusion(22) = HK71*(HK29*P(2,22) - HK30*P(3,22) + HK32*P(1,22) + HK34*P(0,22) + HK53*P(17,22) + HK57*P(18,22) - HK58*P(16,22) + P(20,22)); -Kfusion(23) = HK71*(HK29*P(2,23) - HK30*P(3,23) + HK32*P(1,23) + HK34*P(0,23) + HK53*P(17,23) + HK57*P(18,23) - HK58*P(16,23) + P(20,23)); +Kfusion(0) = HK104*HK112; +Kfusion(1) = HK110*HK112; +Kfusion(2) = HK108*HK112; +Kfusion(3) = HK107*HK112; +Kfusion(4) = HK112*(-HK100*P(4,16) + HK101*P(1,4) - HK102*P(4,17) - HK103*P(3,4) + HK97*P(4,18) + HK98*P(2,4) + HK99*P(0,4) + P(4,20)); +Kfusion(5) = HK112*(-HK100*P(5,16) + HK101*P(1,5) - HK102*P(5,17) - HK103*P(3,5) + HK97*P(5,18) + HK98*P(2,5) + HK99*P(0,5) + P(5,20)); +Kfusion(6) = HK112*(-HK100*P(6,16) + HK101*P(1,6) - HK102*P(6,17) - HK103*P(3,6) + HK97*P(6,18) + HK98*P(2,6) + HK99*P(0,6) + P(6,20)); +Kfusion(7) = HK112*(-HK100*P(7,16) + HK101*P(1,7) - HK102*P(7,17) - HK103*P(3,7) + HK97*P(7,18) + HK98*P(2,7) + HK99*P(0,7) + P(7,20)); +Kfusion(8) = HK112*(-HK100*P(8,16) + HK101*P(1,8) - HK102*P(8,17) - HK103*P(3,8) + HK97*P(8,18) + HK98*P(2,8) + HK99*P(0,8) + P(8,20)); +Kfusion(9) = HK112*(-HK100*P(9,16) + HK101*P(1,9) - HK102*P(9,17) - HK103*P(3,9) + HK97*P(9,18) + HK98*P(2,9) + HK99*P(0,9) + P(9,20)); +Kfusion(10) = HK112*(-HK100*P(10,16) + HK101*P(1,10) - HK102*P(10,17) - HK103*P(3,10) + HK97*P(10,18) + HK98*P(2,10) + HK99*P(0,10) + P(10,20)); +Kfusion(11) = HK112*(-HK100*P(11,16) + HK101*P(1,11) - HK102*P(11,17) - HK103*P(3,11) + HK97*P(11,18) + HK98*P(2,11) + HK99*P(0,11) + P(11,20)); +Kfusion(12) = HK112*(-HK100*P(12,16) + HK101*P(1,12) - HK102*P(12,17) - HK103*P(3,12) + HK97*P(12,18) + HK98*P(2,12) + HK99*P(0,12) + P(12,20)); +Kfusion(13) = HK112*(-HK100*P(13,16) + HK101*P(1,13) - HK102*P(13,17) - HK103*P(3,13) + HK97*P(13,18) + HK98*P(2,13) + HK99*P(0,13) + P(13,20)); +Kfusion(14) = HK112*(-HK100*P(14,16) + HK101*P(1,14) - HK102*P(14,17) - HK103*P(3,14) + HK97*P(14,18) + HK98*P(2,14) + HK99*P(0,14) + P(14,20)); +Kfusion(15) = HK112*(-HK100*P(15,16) + HK101*P(1,15) - HK102*P(15,17) - HK103*P(3,15) + HK97*P(15,18) + HK98*P(2,15) + HK99*P(0,15) + P(15,20)); +Kfusion(16) = HK106*HK112; +Kfusion(17) = HK105*HK112; +Kfusion(18) = HK109*HK112; +Kfusion(19) = HK112*(-HK100*P(16,19) + HK101*P(1,19) - HK102*P(17,19) - HK103*P(3,19) + HK97*P(18,19) + HK98*P(2,19) + HK99*P(0,19) + P(19,20)); +Kfusion(20) = HK111*HK112; +Kfusion(21) = HK112*(-HK100*P(16,21) + HK101*P(1,21) - HK102*P(17,21) - HK103*P(3,21) + HK97*P(18,21) + HK98*P(2,21) + HK99*P(0,21) + P(20,21)); +Kfusion(22) = HK112*(-HK100*P(16,22) + HK101*P(1,22) - HK102*P(17,22) - HK103*P(3,22) + HK97*P(18,22) + HK98*P(2,22) + HK99*P(0,22) + P(20,22)); +Kfusion(23) = HK112*(-HK100*P(16,23) + HK101*P(1,23) - HK102*P(17,23) - HK103*P(3,23) + HK97*P(18,23) + HK98*P(2,23) + HK99*P(0,23) + P(20,23)); // Observation Jacobians - axis 2 -Hfusion.at<0>() = HK51; -Hfusion.at<1>() = -2*HK10 - 2*HK11 + 2*HK12; -Hfusion.at<2>() = HK4; -Hfusion.at<3>() = HK6; +Hfusion.at<0>() = 2*HK36 + 2*HK8; +Hfusion.at<1>() = -2*HK11 - 2*HK113 + 2*HK12; +Hfusion.at<2>() = 2*HK114; +Hfusion.at<3>() = 2*HK115; Hfusion.at<4>() = 0; Hfusion.at<5>() = 0; Hfusion.at<6>() = 0; @@ -211,9 +301,9 @@ Hfusion.at<12>() = 0; Hfusion.at<13>() = 0; Hfusion.at<14>() = 0; Hfusion.at<15>() = 0; -Hfusion.at<16>() = 2*HK72; -Hfusion.at<17>() = -2*HK54 + 2*HK55; -Hfusion.at<18>() = HK73; +Hfusion.at<16>() = 2*HK116; +Hfusion.at<17>() = -2*HK94 + 2*HK95; +Hfusion.at<18>() = HK15 + HK93 + 1; Hfusion.at<19>() = 0; Hfusion.at<20>() = 0; Hfusion.at<21>() = 1; @@ -222,29 +312,29 @@ Hfusion.at<23>() = 0; // Kalman gains - axis 2 -Kfusion(0) = HK76*HK84; -Kfusion(1) = HK79*HK84; -Kfusion(2) = HK82*HK84; -Kfusion(3) = HK81*HK84; -Kfusion(4) = HK84*(HK29*P(3,4) + HK30*P(2,4) + HK32*P(0,4) - HK34*P(1,4) + HK73*P(4,18) + HK74*P(4,16) - HK75*P(4,17) + P(4,21)); -Kfusion(5) = HK84*(HK29*P(3,5) + HK30*P(2,5) + HK32*P(0,5) - HK34*P(1,5) + HK73*P(5,18) + HK74*P(5,16) - HK75*P(5,17) + P(5,21)); -Kfusion(6) = HK84*(HK29*P(3,6) + HK30*P(2,6) + HK32*P(0,6) - HK34*P(1,6) + HK73*P(6,18) + HK74*P(6,16) - HK75*P(6,17) + P(6,21)); -Kfusion(7) = HK84*(HK29*P(3,7) + HK30*P(2,7) + HK32*P(0,7) - HK34*P(1,7) + HK73*P(7,18) + HK74*P(7,16) - HK75*P(7,17) + P(7,21)); -Kfusion(8) = HK84*(HK29*P(3,8) + HK30*P(2,8) + HK32*P(0,8) - HK34*P(1,8) + HK73*P(8,18) + HK74*P(8,16) - HK75*P(8,17) + P(8,21)); -Kfusion(9) = HK84*(HK29*P(3,9) + HK30*P(2,9) + HK32*P(0,9) - HK34*P(1,9) + HK73*P(9,18) + HK74*P(9,16) - HK75*P(9,17) + P(9,21)); -Kfusion(10) = HK84*(HK29*P(3,10) + HK30*P(2,10) + HK32*P(0,10) - HK34*P(1,10) + HK73*P(10,18) + HK74*P(10,16) - HK75*P(10,17) + P(10,21)); -Kfusion(11) = HK84*(HK29*P(3,11) + HK30*P(2,11) + HK32*P(0,11) - HK34*P(1,11) + HK73*P(11,18) + HK74*P(11,16) - HK75*P(11,17) + P(11,21)); -Kfusion(12) = HK84*(HK29*P(3,12) + HK30*P(2,12) + HK32*P(0,12) - HK34*P(1,12) + HK73*P(12,18) + HK74*P(12,16) - HK75*P(12,17) + P(12,21)); -Kfusion(13) = HK84*(HK29*P(3,13) + HK30*P(2,13) + HK32*P(0,13) - HK34*P(1,13) + HK73*P(13,18) + HK74*P(13,16) - HK75*P(13,17) + P(13,21)); -Kfusion(14) = HK84*(HK29*P(3,14) + HK30*P(2,14) + HK32*P(0,14) - HK34*P(1,14) + HK73*P(14,18) + HK74*P(14,16) - HK75*P(14,17) + P(14,21)); -Kfusion(15) = HK84*(HK29*P(3,15) + HK30*P(2,15) + HK32*P(0,15) - HK34*P(1,15) + HK73*P(15,18) + HK74*P(15,16) - HK75*P(15,17) + P(15,21)); -Kfusion(16) = HK80*HK84; -Kfusion(17) = HK78*HK84; -Kfusion(18) = HK77*HK84; -Kfusion(19) = HK84*(HK29*P(3,19) + HK30*P(2,19) + HK32*P(0,19) - HK34*P(1,19) + HK73*P(18,19) + HK74*P(16,19) - HK75*P(17,19) + P(19,21)); -Kfusion(20) = HK84*(HK29*P(3,20) + HK30*P(2,20) + HK32*P(0,20) - HK34*P(1,20) + HK73*P(18,20) + HK74*P(16,20) - HK75*P(17,20) + P(20,21)); -Kfusion(21) = HK83*HK84; -Kfusion(22) = HK84*(HK29*P(3,22) + HK30*P(2,22) + HK32*P(0,22) - HK34*P(1,22) + HK73*P(18,22) + HK74*P(16,22) - HK75*P(17,22) + P(21,22)); -Kfusion(23) = HK84*(HK29*P(3,23) + HK30*P(2,23) + HK32*P(0,23) - HK34*P(1,23) + HK73*P(18,23) + HK74*P(16,23) - HK75*P(17,23) + P(21,23)); +Kfusion(0) = HK174*(-HK118 - HK120 + HK122 + HK124 + HK126 - HK128 + HK130 - P(0,21)); +Kfusion(1) = HK174*(-HK166 - HK167 + HK168 + HK169 - HK170 + HK171 + HK172 - P(1,21)); +Kfusion(2) = HK174*(-HK152 - HK153 + HK154 + HK155 - HK156 + HK157 + HK158 - P(2,21)); +Kfusion(3) = HK174*(-HK138 - HK139 + HK140 + HK141 - HK142 + HK143 + HK144 - P(3,21)); +Kfusion(4) = HK174*(-HK117*P(4,16) - HK119*P(3,4) + HK121*P(0,4) + HK123*P(4,17) + HK125*P(4,18) - HK127*P(2,4) + HK129*P(1,4) - P(4,21)); +Kfusion(5) = HK174*(-HK117*P(5,16) - HK119*P(3,5) + HK121*P(0,5) + HK123*P(5,17) + HK125*P(5,18) - HK127*P(2,5) + HK129*P(1,5) - P(5,21)); +Kfusion(6) = HK174*(-HK117*P(6,16) - HK119*P(3,6) + HK121*P(0,6) + HK123*P(6,17) + HK125*P(6,18) - HK127*P(2,6) + HK129*P(1,6) - P(6,21)); +Kfusion(7) = HK174*(-HK117*P(7,16) - HK119*P(3,7) + HK121*P(0,7) + HK123*P(7,17) + HK125*P(7,18) - HK127*P(2,7) + HK129*P(1,7) - P(7,21)); +Kfusion(8) = HK174*(-HK117*P(8,16) - HK119*P(3,8) + HK121*P(0,8) + HK123*P(8,17) + HK125*P(8,18) - HK127*P(2,8) + HK129*P(1,8) - P(8,21)); +Kfusion(9) = HK174*(-HK117*P(9,16) - HK119*P(3,9) + HK121*P(0,9) + HK123*P(9,17) + HK125*P(9,18) - HK127*P(2,9) + HK129*P(1,9) - P(9,21)); +Kfusion(10) = HK174*(-HK117*P(10,16) - HK119*P(3,10) + HK121*P(0,10) + HK123*P(10,17) + HK125*P(10,18) - HK127*P(2,10) + HK129*P(1,10) - P(10,21)); +Kfusion(11) = HK174*(-HK117*P(11,16) - HK119*P(3,11) + HK121*P(0,11) + HK123*P(11,17) + HK125*P(11,18) - HK127*P(2,11) + HK129*P(1,11) - P(11,21)); +Kfusion(12) = HK174*(-HK117*P(12,16) - HK119*P(3,12) + HK121*P(0,12) + HK123*P(12,17) + HK125*P(12,18) - HK127*P(2,12) + HK129*P(1,12) - P(12,21)); +Kfusion(13) = HK174*(-HK117*P(13,16) - HK119*P(3,13) + HK121*P(0,13) + HK123*P(13,17) + HK125*P(13,18) - HK127*P(2,13) + HK129*P(1,13) - P(13,21)); +Kfusion(14) = HK174*(-HK117*P(14,16) - HK119*P(3,14) + HK121*P(0,14) + HK123*P(14,17) + HK125*P(14,18) - HK127*P(2,14) + HK129*P(1,14) - P(14,21)); +Kfusion(15) = HK174*(-HK117*P(15,16) - HK119*P(3,15) + HK121*P(0,15) + HK123*P(15,17) + HK125*P(15,18) - HK127*P(2,15) + HK129*P(1,15) - P(15,21)); +Kfusion(16) = HK174*(-HK145 - HK146 + HK147 + HK148 - HK149 + HK150 + HK151 - P(16,21)); +Kfusion(17) = HK174*(-HK159 - HK160 + HK161 + HK162 - HK163 + HK164 + HK165 - P(17,21)); +Kfusion(18) = HK174*(-HK131 - HK132 + HK133 + HK134 - HK135 + HK136 + HK137 - P(18,21)); +Kfusion(19) = HK174*(-HK117*P(16,19) - HK119*P(3,19) + HK121*P(0,19) + HK123*P(17,19) + HK125*P(18,19) - HK127*P(2,19) + HK129*P(1,19) + HK84); +Kfusion(20) = HK174*(-HK117*P(16,20) - HK119*P(3,20) + HK121*P(0,20) + HK123*P(17,20) + HK125*P(18,20) - HK127*P(2,20) + HK129*P(1,20) - P(20,21)); +Kfusion(21) = HK173*HK174; +Kfusion(22) = HK174*(-HK117*P(16,22) - HK119*P(3,22) + HK121*P(0,22) + HK123*P(17,22) + HK125*P(18,22) - HK127*P(2,22) + HK129*P(1,22) - P(21,22)); +Kfusion(23) = HK174*(-HK117*P(16,23) - HK119*P(3,23) + HK121*P(0,23) + HK123*P(17,23) + HK125*P(18,23) - HK127*P(2,23) + HK129*P(1,23) - P(21,23)); diff --git a/EKF/python/ekf_derivation/generated/3Dmag_innov_var_generated.cpp b/EKF/python/ekf_derivation/generated/3Dmag_innov_var_generated.cpp index 3ee6caa10f..17befb802f 100644 --- a/EKF/python/ekf_derivation/generated/3Dmag_innov_var_generated.cpp +++ b/EKF/python/ekf_derivation/generated/3Dmag_innov_var_generated.cpp @@ -2,28 +2,34 @@ 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 = powf(q2, 2); -const float IV9 = powf(q3, 2); -const float IV10 = powf(q0, 2) - powf(q1, 2); -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; +const float IV3 = magN*q1; +const float IV4 = 2*IV3 + 2*magD*q3; +const float IV5 = magD*q1; +const float IV6 = -magN*q3; +const float IV7 = 2*IV5 + 2*IV6; +const float IV8 = 2*q0*q3 - 2*q1*q2; +const float IV9 = magN*q2; +const float IV10 = magE*q1; +const float IV11 = -4*IV10 + 2*IV9 + 2*magD*q0; +const float IV12 = 2*powf(q1, 2) - 1; +const float IV13 = IV12 + 2*powf(q3, 2); +const float IV14 = magN*q0; +const float IV15 = magD*q2; +const float IV16 = magE*q3; +const float IV17 = 2*IV14 - 2*IV15 + 4*IV16; +const float IV18 = 2*q0*q2 + 2*q1*q3; +const float IV19 = 2*IV3 + 2*magE*q2; +const float IV20 = 2*IV10 - 2*IV9; +const float IV21 = 2*IV0 - 2*IV1; +const float IV22 = 2*IV14 - 4*IV15 + 2*IV16; +const float IV23 = 4*IV5 + 2*IV6 + 2*magE*q0; +const float IV24 = IV12 + 2*powf(q2, 2); // 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; +Hfusion.at<1>() = IV11*P(1,20) + IV11*(IV11*P(1,1) - IV13*P(1,17) - IV17*P(1,3) + IV2*P(1,18) + IV4*P(1,2) + IV7*P(0,1) - IV8*P(1,16) + P(1,20)) - IV13*P(17,20) - IV13*(IV11*P(1,17) - IV13*P(17,17) - IV17*P(3,17) + IV2*P(17,18) + IV4*P(2,17) + IV7*P(0,17) - IV8*P(16,17) + P(17,20)) - IV17*P(3,20) - IV17*(IV11*P(1,3) - IV13*P(3,17) - IV17*P(3,3) + IV2*P(3,18) + IV4*P(2,3) + IV7*P(0,3) - IV8*P(3,16) + P(3,20)) + IV2*P(18,20) + IV2*(IV11*P(1,18) - IV13*P(17,18) - IV17*P(3,18) + IV2*P(18,18) + IV4*P(2,18) + IV7*P(0,18) - IV8*P(16,18) + P(18,20)) + IV4*P(2,20) + IV4*(IV11*P(1,2) - IV13*P(2,17) - IV17*P(2,3) + IV2*P(2,18) + IV4*P(2,2) + IV7*P(0,2) - IV8*P(2,16) + P(2,20)) + IV7*P(0,20) + IV7*(IV11*P(0,1) - IV13*P(0,17) - IV17*P(0,3) + IV2*P(0,18) + IV4*P(0,2) + IV7*P(0,0) - IV8*P(0,16) + P(0,20)) - IV8*P(16,20) - IV8*(IV11*P(1,16) - IV13*P(16,17) - IV17*P(3,16) + IV2*P(16,18) + IV4*P(2,16) + IV7*P(0,16) - IV8*P(16,16) + P(16,20)) + P(20,20) + R_MAG; +Hfusion.at<2>() = IV18*P(16,21) + IV18*(IV18*P(16,16) + IV19*P(3,16) - IV20*P(0,16) - IV21*P(16,17) + IV22*P(2,16) - IV23*P(1,16) - IV24*P(16,18) + P(16,21)) + IV19*P(3,21) + IV19*(IV18*P(3,16) + IV19*P(3,3) - IV20*P(0,3) - IV21*P(3,17) + IV22*P(2,3) - IV23*P(1,3) - IV24*P(3,18) + P(3,21)) - IV20*P(0,21) - IV20*(IV18*P(0,16) + IV19*P(0,3) - IV20*P(0,0) - IV21*P(0,17) + IV22*P(0,2) - IV23*P(0,1) - IV24*P(0,18) + P(0,21)) - IV21*P(17,21) - IV21*(IV18*P(16,17) + IV19*P(3,17) - IV20*P(0,17) - IV21*P(17,17) + IV22*P(2,17) - IV23*P(1,17) - IV24*P(17,18) + P(17,21)) + IV22*P(2,21) + IV22*(IV18*P(2,16) + IV19*P(2,3) - IV20*P(0,2) - IV21*P(2,17) + IV22*P(2,2) - IV23*P(1,2) - IV24*P(2,18) + P(2,21)) - IV23*P(1,21) - IV23*(IV18*P(1,16) + IV19*P(1,3) - IV20*P(0,1) - IV21*P(1,17) + IV22*P(1,2) - IV23*P(1,1) - IV24*P(1,18) + P(1,21)) - IV24*P(18,21) - IV24*(IV18*P(16,18) + IV19*P(3,18) - IV20*P(0,18) - IV21*P(17,18) + IV22*P(2,18) - IV23*P(1,18) - IV24*P(18,18) + P(18,21)) + P(21,21) + R_MAG; // Kalman gains - axis 0 diff --git a/EKF/python/ekf_derivation/generated/acc_bf_generated.cpp b/EKF/python/ekf_derivation/generated/acc_bf_generated.cpp index 9ba6db192d..0e6d60826a 100644 --- a/EKF/python/ekf_derivation/generated/acc_bf_generated.cpp +++ b/EKF/python/ekf_derivation/generated/acc_bf_generated.cpp @@ -1,48 +1,61 @@ // Axis 0 equations // Sub Expressions -const float HK0 = vn - vwn; +const float HK0 = q2*vd; const float HK1 = ve - vwe; -const float HK2 = HK0*q0 + HK1*q3 - q2*vd; -const float HK3 = 2*Kaccx; -const float HK4 = HK0*q1 + HK1*q2 + q3*vd; -const float HK5 = HK0*q2 - HK1*q1 + q0*vd; -const float HK6 = -HK0*q3 + HK1*q0 + q1*vd; -const float HK7 = powf(q0, 2) + powf(q1, 2) - powf(q2, 2) - powf(q3, 2); -const float HK8 = HK7*Kaccx; -const float HK9 = q0*q3 + q1*q2; -const float HK10 = HK3*HK9; -const float HK11 = q0*q2 - q1*q3; -const float HK12 = 2*HK9; -const float HK13 = 2*HK11; -const float HK14 = 2*HK4; -const float HK15 = 2*HK2; -const float HK16 = 2*HK5; -const float HK17 = 2*HK6; -const float HK18 = -HK12*P(0,23) + HK12*P(0,5) - HK13*P(0,6) + HK14*P(0,1) + HK15*P(0,0) - HK16*P(0,2) + HK17*P(0,3) - HK7*P(0,22) + HK7*P(0,4); -const float HK19 = HK12*P(5,23); -const float HK20 = -HK12*P(23,23) - HK13*P(6,23) + HK14*P(1,23) + HK15*P(0,23) - HK16*P(2,23) + HK17*P(3,23) + HK19 - HK7*P(22,23) + HK7*P(4,23); -const float HK21 = powf(Kaccx, 2); -const float HK22 = HK12*HK21; -const float HK23 = HK12*P(5,5) - HK13*P(5,6) + HK14*P(1,5) + HK15*P(0,5) - HK16*P(2,5) + HK17*P(3,5) - HK19 + HK7*P(4,5) - HK7*P(5,22); -const float HK24 = HK12*P(5,6) - HK12*P(6,23) - HK13*P(6,6) + HK14*P(1,6) + HK15*P(0,6) - HK16*P(2,6) + HK17*P(3,6) + HK7*P(4,6) - HK7*P(6,22); -const float HK25 = HK7*P(4,22); -const float HK26 = -HK12*P(4,23) + HK12*P(4,5) - HK13*P(4,6) + HK14*P(1,4) + HK15*P(0,4) - HK16*P(2,4) + HK17*P(3,4) - HK25 + HK7*P(4,4); -const float HK27 = HK21*HK7; -const float HK28 = -HK12*P(22,23) + HK12*P(5,22) - HK13*P(6,22) + HK14*P(1,22) + HK15*P(0,22) - HK16*P(2,22) + HK17*P(3,22) + HK25 - HK7*P(22,22); -const float HK29 = -HK12*P(1,23) + HK12*P(1,5) - HK13*P(1,6) + HK14*P(1,1) + HK15*P(0,1) - HK16*P(1,2) + HK17*P(1,3) - HK7*P(1,22) + HK7*P(1,4); -const float HK30 = -HK12*P(2,23) + HK12*P(2,5) - HK13*P(2,6) + HK14*P(1,2) + HK15*P(0,2) - HK16*P(2,2) + HK17*P(2,3) - HK7*P(2,22) + HK7*P(2,4); -const float HK31 = -HK12*P(3,23) + HK12*P(3,5) - HK13*P(3,6) + HK14*P(1,3) + HK15*P(0,3) - HK16*P(2,3) + HK17*P(3,3) - HK7*P(3,22) + HK7*P(3,4); -const float HK32 = Kaccx/(-HK13*HK21*HK24 + HK14*HK21*HK29 + HK15*HK18*HK21 - HK16*HK21*HK30 + HK17*HK21*HK31 - HK20*HK22 + HK22*HK23 + HK26*HK27 - HK27*HK28 + R_ACC); +const float HK2 = HK1*q3; +const float HK3 = HK0 - HK2; +const float HK4 = 2*Kaccx; +const float HK5 = HK1*q2 + q3*vd; +const float HK6 = q0*vd; +const float HK7 = HK1*q1; +const float HK8 = 2*vn - 2*vwn; +const float HK9 = HK8*q2; +const float HK10 = HK6 - HK7 + HK9; +const float HK11 = HK1*q0 - HK8*q3 + q1*vd; +const float HK12 = 2*powf(q2, 2); +const float HK13 = 2*powf(q3, 2); +const float HK14 = HK12 + HK13 - 1; +const float HK15 = HK14*Kaccx; +const float HK16 = q0*q3 + q1*q2; +const float HK17 = HK16*HK4; +const float HK18 = q0*q2; +const float HK19 = q1*q3; +const float HK20 = HK18 - HK19; +const float HK21 = 2*HK3; +const float HK22 = 2*HK10; +const float HK23 = 2*HK20; +const float HK24 = 2*HK16; +const float HK25 = 2*HK5; +const float HK26 = 2*HK11; +const float HK27 = HK14*P(0,22) - HK24*P(0,23) + HK24*P(0,5) + HK25*P(0,1) + HK26*P(0,3); +const float HK28 = -HK12 - HK13 + 1; +const float HK29 = -2*HK0 + 2*HK2; +const float HK30 = -2*HK6 + 2*HK7 - 2*HK9; +const float HK31 = -2*HK18 + 2*HK19; +const float HK32 = HK24*P(5,23); +const float HK33 = HK14*P(22,23) - HK24*P(23,23) + HK25*P(1,23) + HK26*P(3,23) + HK32; +const float HK34 = powf(Kaccx, 2); +const float HK35 = HK24*HK34; +const float HK36 = HK14*P(5,22) + HK24*P(5,5) + HK25*P(1,5) + HK26*P(3,5) - HK32; +const float HK37 = HK14*P(6,22) + HK24*P(5,6) - HK24*P(6,23) + HK25*P(1,6) + HK26*P(3,6); +const float HK38 = HK14*P(1,22) - HK24*P(1,23) + HK24*P(1,5) + HK25*P(1,1) + HK26*P(1,3); +const float HK39 = HK14*P(4,22); +const float HK40 = -HK24*P(4,23) + HK24*P(4,5) + HK25*P(1,4) + HK26*P(3,4) + HK39; +const float HK41 = HK14*HK34; +const float HK42 = HK14*P(22,22) - HK24*P(22,23) + HK24*P(5,22) + HK25*P(1,22) + HK26*P(3,22); +const float HK43 = HK14*P(3,22) - HK24*P(3,23) + HK24*P(3,5) + HK25*P(1,3) + HK26*P(3,3); +const float HK44 = HK14*P(2,22) - HK24*P(2,23) + HK24*P(2,5) + HK25*P(1,2) + HK26*P(2,3); +const float HK45 = Kaccx/(HK21*HK34*(HK27 + HK28*P(0,4) + HK29*P(0,0) + HK30*P(0,2) + HK31*P(0,6)) + HK22*HK34*(HK28*P(2,4) + HK29*P(0,2) + HK30*P(2,2) + HK31*P(2,6) + HK44) + HK23*HK34*(HK28*P(4,6) + HK29*P(0,6) + HK30*P(2,6) + HK31*P(6,6) + HK37) - HK25*HK34*(HK28*P(1,4) + HK29*P(0,1) + HK30*P(1,2) + HK31*P(1,6) + HK38) - HK26*HK34*(HK28*P(3,4) + HK29*P(0,3) + HK30*P(2,3) + HK31*P(3,6) + HK43) + HK35*(HK28*P(4,23) + HK29*P(0,23) + HK30*P(2,23) + HK31*P(6,23) + HK33) - HK35*(HK28*P(4,5) + HK29*P(0,5) + HK30*P(2,5) + HK31*P(5,6) + HK36) - HK41*(HK28*P(4,22) + HK29*P(0,22) + HK30*P(2,22) + HK31*P(6,22) + HK42) + HK41*(HK28*P(4,4) + HK29*P(0,4) + HK30*P(2,4) + HK31*P(4,6) + HK40) - R_ACC); // Observation Jacobians -Hfusion.at<0>() = -HK2*HK3; -Hfusion.at<1>() = -HK3*HK4; -Hfusion.at<2>() = HK3*HK5; -Hfusion.at<3>() = -HK3*HK6; -Hfusion.at<4>() = -HK8; -Hfusion.at<5>() = -HK10; -Hfusion.at<6>() = HK11*HK3; +Hfusion.at<0>() = HK3*HK4; +Hfusion.at<1>() = -HK4*HK5; +Hfusion.at<2>() = HK10*HK4; +Hfusion.at<3>() = -HK11*HK4; +Hfusion.at<4>() = HK15; +Hfusion.at<5>() = -HK17; +Hfusion.at<6>() = HK20*HK4; Hfusion.at<7>() = 0; Hfusion.at<8>() = 0; Hfusion.at<9>() = 0; @@ -58,82 +71,82 @@ Hfusion.at<18>() = 0; Hfusion.at<19>() = 0; Hfusion.at<20>() = 0; Hfusion.at<21>() = 0; -Hfusion.at<22>() = HK8; -Hfusion.at<23>() = HK10; +Hfusion.at<22>() = -HK15; +Hfusion.at<23>() = HK17; // Kalman gains -Kfusion(0) = -HK18*HK32; -Kfusion(1) = -HK29*HK32; -Kfusion(2) = -HK30*HK32; -Kfusion(3) = -HK31*HK32; -Kfusion(4) = -HK26*HK32; -Kfusion(5) = -HK23*HK32; -Kfusion(6) = -HK24*HK32; -Kfusion(7) = -HK32*(HK12*P(5,7) - HK12*P(7,23) - HK13*P(6,7) + HK14*P(1,7) + HK15*P(0,7) - HK16*P(2,7) + HK17*P(3,7) + HK7*P(4,7) - HK7*P(7,22)); -Kfusion(8) = -HK32*(HK12*P(5,8) - HK12*P(8,23) - HK13*P(6,8) + HK14*P(1,8) + HK15*P(0,8) - HK16*P(2,8) + HK17*P(3,8) + HK7*P(4,8) - HK7*P(8,22)); -Kfusion(9) = -HK32*(HK12*P(5,9) - HK12*P(9,23) - HK13*P(6,9) + HK14*P(1,9) + HK15*P(0,9) - HK16*P(2,9) + HK17*P(3,9) + HK7*P(4,9) - HK7*P(9,22)); -Kfusion(10) = -HK32*(-HK12*P(10,23) + HK12*P(5,10) - HK13*P(6,10) + HK14*P(1,10) + HK15*P(0,10) - HK16*P(2,10) + HK17*P(3,10) - HK7*P(10,22) + HK7*P(4,10)); -Kfusion(11) = -HK32*(-HK12*P(11,23) + HK12*P(5,11) - HK13*P(6,11) + HK14*P(1,11) + HK15*P(0,11) - HK16*P(2,11) + HK17*P(3,11) - HK7*P(11,22) + HK7*P(4,11)); -Kfusion(12) = -HK32*(-HK12*P(12,23) + HK12*P(5,12) - HK13*P(6,12) + HK14*P(1,12) + HK15*P(0,12) - HK16*P(2,12) + HK17*P(3,12) - HK7*P(12,22) + HK7*P(4,12)); -Kfusion(13) = -HK32*(-HK12*P(13,23) + HK12*P(5,13) - HK13*P(6,13) + HK14*P(1,13) + HK15*P(0,13) - HK16*P(2,13) + HK17*P(3,13) - HK7*P(13,22) + HK7*P(4,13)); -Kfusion(14) = -HK32*(-HK12*P(14,23) + HK12*P(5,14) - HK13*P(6,14) + HK14*P(1,14) + HK15*P(0,14) - HK16*P(2,14) + HK17*P(3,14) - HK7*P(14,22) + HK7*P(4,14)); -Kfusion(15) = -HK32*(-HK12*P(15,23) + HK12*P(5,15) - HK13*P(6,15) + HK14*P(1,15) + HK15*P(0,15) - HK16*P(2,15) + HK17*P(3,15) - HK7*P(15,22) + HK7*P(4,15)); -Kfusion(16) = -HK32*(-HK12*P(16,23) + HK12*P(5,16) - HK13*P(6,16) + HK14*P(1,16) + HK15*P(0,16) - HK16*P(2,16) + HK17*P(3,16) - HK7*P(16,22) + HK7*P(4,16)); -Kfusion(17) = -HK32*(-HK12*P(17,23) + HK12*P(5,17) - HK13*P(6,17) + HK14*P(1,17) + HK15*P(0,17) - HK16*P(2,17) + HK17*P(3,17) - HK7*P(17,22) + HK7*P(4,17)); -Kfusion(18) = -HK32*(-HK12*P(18,23) + HK12*P(5,18) - HK13*P(6,18) + HK14*P(1,18) + HK15*P(0,18) - HK16*P(2,18) + HK17*P(3,18) - HK7*P(18,22) + HK7*P(4,18)); -Kfusion(19) = -HK32*(-HK12*P(19,23) + HK12*P(5,19) - HK13*P(6,19) + HK14*P(1,19) + HK15*P(0,19) - HK16*P(2,19) + HK17*P(3,19) - HK7*P(19,22) + HK7*P(4,19)); -Kfusion(20) = -HK32*(-HK12*P(20,23) + HK12*P(5,20) - HK13*P(6,20) + HK14*P(1,20) + HK15*P(0,20) - HK16*P(2,20) + HK17*P(3,20) - HK7*P(20,22) + HK7*P(4,20)); -Kfusion(21) = -HK32*(-HK12*P(21,23) + HK12*P(5,21) - HK13*P(6,21) + HK14*P(1,21) + HK15*P(0,21) - HK16*P(2,21) + HK17*P(3,21) - HK7*P(21,22) + HK7*P(4,21)); -Kfusion(22) = -HK28*HK32; -Kfusion(23) = -HK20*HK32; +Kfusion(0) = HK45*(-HK14*P(0,4) - HK21*P(0,0) - HK22*P(0,2) - HK23*P(0,6) + HK27); +Kfusion(1) = HK45*(-HK14*P(1,4) - HK21*P(0,1) - HK22*P(1,2) - HK23*P(1,6) + HK38); +Kfusion(2) = HK45*(-HK14*P(2,4) - HK21*P(0,2) - HK22*P(2,2) - HK23*P(2,6) + HK44); +Kfusion(3) = HK45*(-HK14*P(3,4) - HK21*P(0,3) - HK22*P(2,3) - HK23*P(3,6) + HK43); +Kfusion(4) = HK45*(-HK14*P(4,4) - HK21*P(0,4) - HK22*P(2,4) - HK23*P(4,6) + HK40); +Kfusion(5) = HK45*(-HK14*P(4,5) - HK21*P(0,5) - HK22*P(2,5) - HK23*P(5,6) + HK36); +Kfusion(6) = HK45*(-HK14*P(4,6) - HK21*P(0,6) - HK22*P(2,6) - HK23*P(6,6) + HK37); +Kfusion(7) = HK45*(-HK14*P(4,7) + HK14*P(7,22) - HK21*P(0,7) - HK22*P(2,7) - HK23*P(6,7) + HK24*P(5,7) - HK24*P(7,23) + HK25*P(1,7) + HK26*P(3,7)); +Kfusion(8) = HK45*(-HK14*P(4,8) + HK14*P(8,22) - HK21*P(0,8) - HK22*P(2,8) - HK23*P(6,8) + HK24*P(5,8) - HK24*P(8,23) + HK25*P(1,8) + HK26*P(3,8)); +Kfusion(9) = HK45*(-HK14*P(4,9) + HK14*P(9,22) - HK21*P(0,9) - HK22*P(2,9) - HK23*P(6,9) + HK24*P(5,9) - HK24*P(9,23) + HK25*P(1,9) + HK26*P(3,9)); +Kfusion(10) = HK45*(HK14*P(10,22) - HK14*P(4,10) - HK21*P(0,10) - HK22*P(2,10) - HK23*P(6,10) - HK24*P(10,23) + HK24*P(5,10) + HK25*P(1,10) + HK26*P(3,10)); +Kfusion(11) = HK45*(HK14*P(11,22) - HK14*P(4,11) - HK21*P(0,11) - HK22*P(2,11) - HK23*P(6,11) - HK24*P(11,23) + HK24*P(5,11) + HK25*P(1,11) + HK26*P(3,11)); +Kfusion(12) = HK45*(HK14*P(12,22) - HK14*P(4,12) - HK21*P(0,12) - HK22*P(2,12) - HK23*P(6,12) - HK24*P(12,23) + HK24*P(5,12) + HK25*P(1,12) + HK26*P(3,12)); +Kfusion(13) = HK45*(HK14*P(13,22) - HK14*P(4,13) - HK21*P(0,13) - HK22*P(2,13) - HK23*P(6,13) - HK24*P(13,23) + HK24*P(5,13) + HK25*P(1,13) + HK26*P(3,13)); +Kfusion(14) = HK45*(HK14*P(14,22) - HK14*P(4,14) - HK21*P(0,14) - HK22*P(2,14) - HK23*P(6,14) - HK24*P(14,23) + HK24*P(5,14) + HK25*P(1,14) + HK26*P(3,14)); +Kfusion(15) = HK45*(HK14*P(15,22) - HK14*P(4,15) - HK21*P(0,15) - HK22*P(2,15) - HK23*P(6,15) - HK24*P(15,23) + HK24*P(5,15) + HK25*P(1,15) + HK26*P(3,15)); +Kfusion(16) = HK45*(HK14*P(16,22) - HK14*P(4,16) - HK21*P(0,16) - HK22*P(2,16) - HK23*P(6,16) - HK24*P(16,23) + HK24*P(5,16) + HK25*P(1,16) + HK26*P(3,16)); +Kfusion(17) = HK45*(HK14*P(17,22) - HK14*P(4,17) - HK21*P(0,17) - HK22*P(2,17) - HK23*P(6,17) - HK24*P(17,23) + HK24*P(5,17) + HK25*P(1,17) + HK26*P(3,17)); +Kfusion(18) = HK45*(HK14*P(18,22) - HK14*P(4,18) - HK21*P(0,18) - HK22*P(2,18) - HK23*P(6,18) - HK24*P(18,23) + HK24*P(5,18) + HK25*P(1,18) + HK26*P(3,18)); +Kfusion(19) = HK45*(HK14*P(19,22) - HK14*P(4,19) - HK21*P(0,19) - HK22*P(2,19) - HK23*P(6,19) - HK24*P(19,23) + HK24*P(5,19) + HK25*P(1,19) + HK26*P(3,19)); +Kfusion(20) = HK45*(HK14*P(20,22) - HK14*P(4,20) - HK21*P(0,20) - HK22*P(2,20) - HK23*P(6,20) - HK24*P(20,23) + HK24*P(5,20) + HK25*P(1,20) + HK26*P(3,20)); +Kfusion(21) = HK45*(HK14*P(21,22) - HK14*P(4,21) - HK21*P(0,21) - HK22*P(2,21) - HK23*P(6,21) - HK24*P(21,23) + HK24*P(5,21) + HK25*P(1,21) + HK26*P(3,21)); +Kfusion(22) = HK45*(-HK21*P(0,22) - HK22*P(2,22) - HK23*P(6,22) - HK39 + HK42); +Kfusion(23) = HK45*(-HK14*P(4,23) - HK21*P(0,23) - HK22*P(2,23) - HK23*P(6,23) + HK33); // Axis 1 equations // Sub Expressions -const float HK0 = ve - vwe; -const float HK1 = vn - vwn; -const float HK2 = HK0*q0 - HK1*q3 + q1*vd; -const float HK3 = 2*Kaccy; -const float HK4 = -HK0*q1 + HK1*q2 + q0*vd; -const float HK5 = HK0*q2 + HK1*q1 + q3*vd; -const float HK6 = HK0*q3 + HK1*q0 - q2*vd; +const float HK0 = vn - vwn; +const float HK1 = -HK0*q3 + q1*vd; +const float HK2 = 2*Kaccy; +const float HK3 = 2*ve - 2*vwe; +const float HK4 = HK0*q2 - HK3*q1 + q0*vd; +const float HK5 = HK0*q1 + q3*vd; +const float HK6 = HK0*q0 + HK3*q3 - q2*vd; const float HK7 = q0*q3 - q1*q2; -const float HK8 = HK3*HK7; -const float HK9 = powf(q0, 2) - powf(q1, 2) + powf(q2, 2) - powf(q3, 2); +const float HK8 = HK2*HK7; +const float HK9 = 2*powf(q1, 2) + 2*powf(q3, 2) - 1; const float HK10 = HK9*Kaccy; const float HK11 = q0*q1 + q2*q3; const float HK12 = 2*HK11; const float HK13 = 2*HK7; const float HK14 = 2*HK5; -const float HK15 = 2*HK2; +const float HK15 = 2*HK1; const float HK16 = 2*HK4; const float HK17 = 2*HK6; -const float HK18 = HK12*P(0,6) + HK13*P(0,22) - HK13*P(0,4) + HK14*P(0,2) + HK15*P(0,0) + HK16*P(0,1) - HK17*P(0,3) - HK9*P(0,23) + HK9*P(0,5); +const float HK18 = HK12*P(0,6) + HK13*P(0,22) - HK13*P(0,4) + HK14*P(0,2) + HK15*P(0,0) + HK16*P(0,1) - HK17*P(0,3) + HK9*P(0,23) - HK9*P(0,5); const float HK19 = powf(Kaccy, 2); -const float HK20 = HK12*P(6,6) - HK13*P(4,6) + HK13*P(6,22) + HK14*P(2,6) + HK15*P(0,6) + HK16*P(1,6) - HK17*P(3,6) + HK9*P(5,6) - HK9*P(6,23); +const float HK20 = HK12*P(6,6) - HK13*P(4,6) + HK13*P(6,22) + HK14*P(2,6) + HK15*P(0,6) + HK16*P(1,6) - HK17*P(3,6) - HK9*P(5,6) + HK9*P(6,23); const float HK21 = HK13*P(4,22); -const float HK22 = HK12*P(6,22) + HK13*P(22,22) + HK14*P(2,22) + HK15*P(0,22) + HK16*P(1,22) - HK17*P(3,22) - HK21 - HK9*P(22,23) + HK9*P(5,22); +const float HK22 = HK12*P(6,22) + HK13*P(22,22) + HK14*P(2,22) + HK15*P(0,22) + HK16*P(1,22) - HK17*P(3,22) - HK21 + HK9*P(22,23) - HK9*P(5,22); const float HK23 = HK13*HK19; -const float HK24 = HK12*P(4,6) - HK13*P(4,4) + HK14*P(2,4) + HK15*P(0,4) + HK16*P(1,4) - HK17*P(3,4) + HK21 - HK9*P(4,23) + HK9*P(4,5); -const float HK25 = HK9*P(5,23); -const float HK26 = HK12*P(5,6) - HK13*P(4,5) + HK13*P(5,22) + HK14*P(2,5) + HK15*P(0,5) + HK16*P(1,5) - HK17*P(3,5) - HK25 + HK9*P(5,5); -const float HK27 = HK19*HK9; -const float HK28 = HK12*P(6,23) + HK13*P(22,23) - HK13*P(4,23) + HK14*P(2,23) + HK15*P(0,23) + HK16*P(1,23) - HK17*P(3,23) + HK25 - HK9*P(23,23); -const float HK29 = HK12*P(2,6) + HK13*P(2,22) - HK13*P(2,4) + HK14*P(2,2) + HK15*P(0,2) + HK16*P(1,2) - HK17*P(2,3) - HK9*P(2,23) + HK9*P(2,5); -const float HK30 = HK12*P(1,6) + HK13*P(1,22) - HK13*P(1,4) + HK14*P(1,2) + HK15*P(0,1) + HK16*P(1,1) - HK17*P(1,3) - HK9*P(1,23) + HK9*P(1,5); -const float HK31 = HK12*P(3,6) + HK13*P(3,22) - HK13*P(3,4) + HK14*P(2,3) + HK15*P(0,3) + HK16*P(1,3) - HK17*P(3,3) - HK9*P(3,23) + HK9*P(3,5); -const float HK32 = Kaccy/(HK12*HK19*HK20 + HK14*HK19*HK29 + HK15*HK18*HK19 + HK16*HK19*HK30 - HK17*HK19*HK31 + HK22*HK23 - HK23*HK24 + HK26*HK27 - HK27*HK28 + R_ACC); +const float HK24 = HK12*P(4,6) - HK13*P(4,4) + HK14*P(2,4) + HK15*P(0,4) + HK16*P(1,4) - HK17*P(3,4) + HK21 + HK9*P(4,23) - HK9*P(4,5); +const float HK25 = HK12*P(2,6) + HK13*P(2,22) - HK13*P(2,4) + HK14*P(2,2) + HK15*P(0,2) + HK16*P(1,2) - HK17*P(2,3) + HK9*P(2,23) - HK9*P(2,5); +const float HK26 = HK9*P(5,23); +const float HK27 = HK12*P(6,23) + HK13*P(22,23) - HK13*P(4,23) + HK14*P(2,23) + HK15*P(0,23) + HK16*P(1,23) - HK17*P(3,23) - HK26 + HK9*P(23,23); +const float HK28 = HK19*HK9; +const float HK29 = HK12*P(5,6) - HK13*P(4,5) + HK13*P(5,22) + HK14*P(2,5) + HK15*P(0,5) + HK16*P(1,5) - HK17*P(3,5) + HK26 - HK9*P(5,5); +const float HK30 = HK12*P(1,6) + HK13*P(1,22) - HK13*P(1,4) + HK14*P(1,2) + HK15*P(0,1) + HK16*P(1,1) - HK17*P(1,3) + HK9*P(1,23) - HK9*P(1,5); +const float HK31 = HK12*P(3,6) + HK13*P(3,22) - HK13*P(3,4) + HK14*P(2,3) + HK15*P(0,3) + HK16*P(1,3) - HK17*P(3,3) + HK9*P(3,23) - HK9*P(3,5); +const float HK32 = Kaccy/(HK12*HK19*HK20 + HK14*HK19*HK25 + HK15*HK18*HK19 + HK16*HK19*HK30 - HK17*HK19*HK31 + HK22*HK23 - HK23*HK24 + HK27*HK28 - HK28*HK29 + R_ACC); // Observation Jacobians -Hfusion.at<0>() = -HK2*HK3; -Hfusion.at<1>() = -HK3*HK4; -Hfusion.at<2>() = -HK3*HK5; -Hfusion.at<3>() = HK3*HK6; +Hfusion.at<0>() = -HK1*HK2; +Hfusion.at<1>() = -HK2*HK4; +Hfusion.at<2>() = -HK2*HK5; +Hfusion.at<3>() = HK2*HK6; Hfusion.at<4>() = HK8; -Hfusion.at<5>() = -HK10; -Hfusion.at<6>() = -HK11*HK3; +Hfusion.at<5>() = HK10; +Hfusion.at<6>() = -HK11*HK2; Hfusion.at<7>() = 0; Hfusion.at<8>() = 0; Hfusion.at<9>() = 0; @@ -150,33 +163,33 @@ Hfusion.at<19>() = 0; Hfusion.at<20>() = 0; Hfusion.at<21>() = 0; Hfusion.at<22>() = -HK8; -Hfusion.at<23>() = HK10; +Hfusion.at<23>() = -HK10; // Kalman gains Kfusion(0) = -HK18*HK32; Kfusion(1) = -HK30*HK32; -Kfusion(2) = -HK29*HK32; +Kfusion(2) = -HK25*HK32; Kfusion(3) = -HK31*HK32; Kfusion(4) = -HK24*HK32; -Kfusion(5) = -HK26*HK32; +Kfusion(5) = -HK29*HK32; Kfusion(6) = -HK20*HK32; -Kfusion(7) = -HK32*(HK12*P(6,7) - HK13*P(4,7) + HK13*P(7,22) + HK14*P(2,7) + HK15*P(0,7) + HK16*P(1,7) - HK17*P(3,7) + HK9*P(5,7) - HK9*P(7,23)); -Kfusion(8) = -HK32*(HK12*P(6,8) - HK13*P(4,8) + HK13*P(8,22) + HK14*P(2,8) + HK15*P(0,8) + HK16*P(1,8) - HK17*P(3,8) + HK9*P(5,8) - HK9*P(8,23)); -Kfusion(9) = -HK32*(HK12*P(6,9) - HK13*P(4,9) + HK13*P(9,22) + HK14*P(2,9) + HK15*P(0,9) + HK16*P(1,9) - HK17*P(3,9) + HK9*P(5,9) - HK9*P(9,23)); -Kfusion(10) = -HK32*(HK12*P(6,10) + HK13*P(10,22) - HK13*P(4,10) + HK14*P(2,10) + HK15*P(0,10) + HK16*P(1,10) - HK17*P(3,10) - HK9*P(10,23) + HK9*P(5,10)); -Kfusion(11) = -HK32*(HK12*P(6,11) + HK13*P(11,22) - HK13*P(4,11) + HK14*P(2,11) + HK15*P(0,11) + HK16*P(1,11) - HK17*P(3,11) - HK9*P(11,23) + HK9*P(5,11)); -Kfusion(12) = -HK32*(HK12*P(6,12) + HK13*P(12,22) - HK13*P(4,12) + HK14*P(2,12) + HK15*P(0,12) + HK16*P(1,12) - HK17*P(3,12) - HK9*P(12,23) + HK9*P(5,12)); -Kfusion(13) = -HK32*(HK12*P(6,13) + HK13*P(13,22) - HK13*P(4,13) + HK14*P(2,13) + HK15*P(0,13) + HK16*P(1,13) - HK17*P(3,13) - HK9*P(13,23) + HK9*P(5,13)); -Kfusion(14) = -HK32*(HK12*P(6,14) + HK13*P(14,22) - HK13*P(4,14) + HK14*P(2,14) + HK15*P(0,14) + HK16*P(1,14) - HK17*P(3,14) - HK9*P(14,23) + HK9*P(5,14)); -Kfusion(15) = -HK32*(HK12*P(6,15) + HK13*P(15,22) - HK13*P(4,15) + HK14*P(2,15) + HK15*P(0,15) + HK16*P(1,15) - HK17*P(3,15) - HK9*P(15,23) + HK9*P(5,15)); -Kfusion(16) = -HK32*(HK12*P(6,16) + HK13*P(16,22) - HK13*P(4,16) + HK14*P(2,16) + HK15*P(0,16) + HK16*P(1,16) - HK17*P(3,16) - HK9*P(16,23) + HK9*P(5,16)); -Kfusion(17) = -HK32*(HK12*P(6,17) + HK13*P(17,22) - HK13*P(4,17) + HK14*P(2,17) + HK15*P(0,17) + HK16*P(1,17) - HK17*P(3,17) - HK9*P(17,23) + HK9*P(5,17)); -Kfusion(18) = -HK32*(HK12*P(6,18) + HK13*P(18,22) - HK13*P(4,18) + HK14*P(2,18) + HK15*P(0,18) + HK16*P(1,18) - HK17*P(3,18) - HK9*P(18,23) + HK9*P(5,18)); -Kfusion(19) = -HK32*(HK12*P(6,19) + HK13*P(19,22) - HK13*P(4,19) + HK14*P(2,19) + HK15*P(0,19) + HK16*P(1,19) - HK17*P(3,19) - HK9*P(19,23) + HK9*P(5,19)); -Kfusion(20) = -HK32*(HK12*P(6,20) + HK13*P(20,22) - HK13*P(4,20) + HK14*P(2,20) + HK15*P(0,20) + HK16*P(1,20) - HK17*P(3,20) - HK9*P(20,23) + HK9*P(5,20)); -Kfusion(21) = -HK32*(HK12*P(6,21) + HK13*P(21,22) - HK13*P(4,21) + HK14*P(2,21) + HK15*P(0,21) + HK16*P(1,21) - HK17*P(3,21) - HK9*P(21,23) + HK9*P(5,21)); +Kfusion(7) = -HK32*(HK12*P(6,7) - HK13*P(4,7) + HK13*P(7,22) + HK14*P(2,7) + HK15*P(0,7) + HK16*P(1,7) - HK17*P(3,7) - HK9*P(5,7) + HK9*P(7,23)); +Kfusion(8) = -HK32*(HK12*P(6,8) - HK13*P(4,8) + HK13*P(8,22) + HK14*P(2,8) + HK15*P(0,8) + HK16*P(1,8) - HK17*P(3,8) - HK9*P(5,8) + HK9*P(8,23)); +Kfusion(9) = -HK32*(HK12*P(6,9) - HK13*P(4,9) + HK13*P(9,22) + HK14*P(2,9) + HK15*P(0,9) + HK16*P(1,9) - HK17*P(3,9) - HK9*P(5,9) + HK9*P(9,23)); +Kfusion(10) = -HK32*(HK12*P(6,10) + HK13*P(10,22) - HK13*P(4,10) + HK14*P(2,10) + HK15*P(0,10) + HK16*P(1,10) - HK17*P(3,10) + HK9*P(10,23) - HK9*P(5,10)); +Kfusion(11) = -HK32*(HK12*P(6,11) + HK13*P(11,22) - HK13*P(4,11) + HK14*P(2,11) + HK15*P(0,11) + HK16*P(1,11) - HK17*P(3,11) + HK9*P(11,23) - HK9*P(5,11)); +Kfusion(12) = -HK32*(HK12*P(6,12) + HK13*P(12,22) - HK13*P(4,12) + HK14*P(2,12) + HK15*P(0,12) + HK16*P(1,12) - HK17*P(3,12) + HK9*P(12,23) - HK9*P(5,12)); +Kfusion(13) = -HK32*(HK12*P(6,13) + HK13*P(13,22) - HK13*P(4,13) + HK14*P(2,13) + HK15*P(0,13) + HK16*P(1,13) - HK17*P(3,13) + HK9*P(13,23) - HK9*P(5,13)); +Kfusion(14) = -HK32*(HK12*P(6,14) + HK13*P(14,22) - HK13*P(4,14) + HK14*P(2,14) + HK15*P(0,14) + HK16*P(1,14) - HK17*P(3,14) + HK9*P(14,23) - HK9*P(5,14)); +Kfusion(15) = -HK32*(HK12*P(6,15) + HK13*P(15,22) - HK13*P(4,15) + HK14*P(2,15) + HK15*P(0,15) + HK16*P(1,15) - HK17*P(3,15) + HK9*P(15,23) - HK9*P(5,15)); +Kfusion(16) = -HK32*(HK12*P(6,16) + HK13*P(16,22) - HK13*P(4,16) + HK14*P(2,16) + HK15*P(0,16) + HK16*P(1,16) - HK17*P(3,16) + HK9*P(16,23) - HK9*P(5,16)); +Kfusion(17) = -HK32*(HK12*P(6,17) + HK13*P(17,22) - HK13*P(4,17) + HK14*P(2,17) + HK15*P(0,17) + HK16*P(1,17) - HK17*P(3,17) + HK9*P(17,23) - HK9*P(5,17)); +Kfusion(18) = -HK32*(HK12*P(6,18) + HK13*P(18,22) - HK13*P(4,18) + HK14*P(2,18) + HK15*P(0,18) + HK16*P(1,18) - HK17*P(3,18) + HK9*P(18,23) - HK9*P(5,18)); +Kfusion(19) = -HK32*(HK12*P(6,19) + HK13*P(19,22) - HK13*P(4,19) + HK14*P(2,19) + HK15*P(0,19) + HK16*P(1,19) - HK17*P(3,19) + HK9*P(19,23) - HK9*P(5,19)); +Kfusion(20) = -HK32*(HK12*P(6,20) + HK13*P(20,22) - HK13*P(4,20) + HK14*P(2,20) + HK15*P(0,20) + HK16*P(1,20) - HK17*P(3,20) + HK9*P(20,23) - HK9*P(5,20)); +Kfusion(21) = -HK32*(HK12*P(6,21) + HK13*P(21,22) - HK13*P(4,21) + HK14*P(2,21) + HK15*P(0,21) + HK16*P(1,21) - HK17*P(3,21) + HK9*P(21,23) - HK9*P(5,21)); Kfusion(22) = -HK22*HK32; -Kfusion(23) = -HK28*HK32; +Kfusion(23) = -HK27*HK32; diff --git a/EKF/python/ekf_derivation/generated/acc_bf_generated_alt.cpp b/EKF/python/ekf_derivation/generated/acc_bf_generated_alt.cpp index 1985759b3b..734b2d3d1b 100644 --- a/EKF/python/ekf_derivation/generated/acc_bf_generated_alt.cpp +++ b/EKF/python/ekf_derivation/generated/acc_bf_generated_alt.cpp @@ -1,79 +1,99 @@ // Sub Expressions -const float HK0 = vn - vwn; +const float HK0 = q2*vd; const float HK1 = ve - vwe; -const float HK2 = HK0*q0 + HK1*q3 - q2*vd; -const float HK3 = 2*Kaccx; -const float HK4 = HK0*q1 + HK1*q2 + q3*vd; -const float HK5 = HK0*q2 - HK1*q1 + q0*vd; -const float HK6 = -HK0*q3 + HK1*q0 + q1*vd; -const float HK7 = powf(q1, 2); -const float HK8 = powf(q2, 2); -const float HK9 = powf(q0, 2) - powf(q3, 2); -const float HK10 = HK7 - HK8 + HK9; -const float HK11 = HK10*Kaccx; -const float HK12 = q0*q3; -const float HK13 = q1*q2; -const float HK14 = HK12 + HK13; -const float HK15 = HK14*HK3; -const float HK16 = q0*q2 - q1*q3; -const float HK17 = 2*HK14; -const float HK18 = 2*HK16; -const float HK19 = 2*HK4; -const float HK20 = 2*HK2; -const float HK21 = 2*HK5; -const float HK22 = 2*HK6; -const float HK23 = HK22*P(0,3); -const float HK24 = -HK10*P(0,22) + HK10*P(0,4) - HK17*P(0,23) + HK17*P(0,5) - HK18*P(0,6) + HK19*P(0,1) + HK20*P(0,0) - HK21*P(0,2) + HK23; -const float HK25 = HK17*P(5,23); -const float HK26 = -HK10*P(22,23) + HK10*P(4,23) - HK17*P(23,23) - HK18*P(6,23) + HK19*P(1,23) + HK20*P(0,23) - HK21*P(2,23) + HK22*P(3,23) + HK25; -const float HK27 = powf(Kaccx, 2); -const float HK28 = HK17*HK27; -const float HK29 = HK10*P(4,5) - HK10*P(5,22) + HK17*P(5,5) - HK18*P(5,6) + HK19*P(1,5) + HK20*P(0,5) - HK21*P(2,5) + HK22*P(3,5) - HK25; -const float HK30 = HK10*P(4,6) - HK10*P(6,22) + HK17*P(5,6) - HK17*P(6,23) - HK18*P(6,6) + HK19*P(1,6) + HK20*P(0,6) - HK21*P(2,6) + HK22*P(3,6); -const float HK31 = HK10*P(4,22); -const float HK32 = HK10*P(4,4) - HK17*P(4,23) + HK17*P(4,5) - HK18*P(4,6) + HK19*P(1,4) + HK20*P(0,4) - HK21*P(2,4) + HK22*P(3,4) - HK31; -const float HK33 = HK10*HK27; -const float HK34 = -HK10*P(22,22) - HK17*P(22,23) + HK17*P(5,22) - HK18*P(6,22) + HK19*P(1,22) + HK20*P(0,22) - HK21*P(2,22) + HK22*P(3,22) + HK31; -const float HK35 = HK21*P(1,2); -const float HK36 = -HK10*P(1,22) + HK10*P(1,4) - HK17*P(1,23) + HK17*P(1,5) - HK18*P(1,6) + HK19*P(1,1) + HK20*P(0,1) + HK22*P(1,3) - HK35; -const float HK37 = HK19*P(1,2); -const float HK38 = -HK10*P(2,22) + HK10*P(2,4) - HK17*P(2,23) + HK17*P(2,5) - HK18*P(2,6) + HK20*P(0,2) - HK21*P(2,2) + HK22*P(2,3) + HK37; -const float HK39 = HK20*P(0,3); -const float HK40 = -HK10*P(3,22) + HK10*P(3,4) - HK17*P(3,23) + HK17*P(3,5) - HK18*P(3,6) + HK19*P(1,3) - HK21*P(2,3) + HK22*P(3,3) + HK39; -const float HK41 = Kaccx/(-HK18*HK27*HK30 + HK19*HK27*HK36 + HK20*HK24*HK27 - HK21*HK27*HK38 + HK22*HK27*HK40 - HK26*HK28 + HK28*HK29 + HK32*HK33 - HK33*HK34 + R_ACC); -const float HK42 = HK12 - HK13; -const float HK43 = 2*Kaccy; -const float HK44 = HK42*HK43; -const float HK45 = -HK7 + HK8 + HK9; -const float HK46 = HK45*Kaccy; -const float HK47 = q0*q1 + q2*q3; -const float HK48 = 2*HK47; -const float HK49 = 2*HK42; -const float HK50 = HK19*P(0,2) + HK21*P(0,1) + HK22*P(0,0) - HK39 - HK45*P(0,23) + HK45*P(0,5) + HK48*P(0,6) + HK49*P(0,22) - HK49*P(0,4); -const float HK51 = powf(Kaccy, 2); -const float HK52 = HK19*P(2,6) - HK20*P(3,6) + HK21*P(1,6) + HK22*P(0,6) + HK45*P(5,6) - HK45*P(6,23) + HK48*P(6,6) - HK49*P(4,6) + HK49*P(6,22); -const float HK53 = HK49*P(4,22); -const float HK54 = HK19*P(2,22) - HK20*P(3,22) + HK21*P(1,22) + HK22*P(0,22) - HK45*P(22,23) + HK45*P(5,22) + HK48*P(6,22) + HK49*P(22,22) - HK53; -const float HK55 = HK49*HK51; -const float HK56 = HK19*P(2,4) - HK20*P(3,4) + HK21*P(1,4) + HK22*P(0,4) - HK45*P(4,23) + HK45*P(4,5) + HK48*P(4,6) - HK49*P(4,4) + HK53; -const float HK57 = HK45*P(5,23); -const float HK58 = HK19*P(2,5) - HK20*P(3,5) + HK21*P(1,5) + HK22*P(0,5) + HK45*P(5,5) + HK48*P(5,6) - HK49*P(4,5) + HK49*P(5,22) - HK57; -const float HK59 = HK45*HK51; -const float HK60 = HK19*P(2,23) - HK20*P(3,23) + HK21*P(1,23) + HK22*P(0,23) - HK45*P(23,23) + HK48*P(6,23) + HK49*P(22,23) - HK49*P(4,23) + HK57; -const float HK61 = HK19*P(2,2) - HK20*P(2,3) + HK22*P(0,2) + HK35 - HK45*P(2,23) + HK45*P(2,5) + HK48*P(2,6) + HK49*P(2,22) - HK49*P(2,4); -const float HK62 = -HK20*P(1,3) + HK21*P(1,1) + HK22*P(0,1) + HK37 - HK45*P(1,23) + HK45*P(1,5) + HK48*P(1,6) + HK49*P(1,22) - HK49*P(1,4); -const float HK63 = HK19*P(2,3) - HK20*P(3,3) + HK21*P(1,3) + HK23 - HK45*P(3,23) + HK45*P(3,5) + HK48*P(3,6) + HK49*P(3,22) - HK49*P(3,4); -const float HK64 = Kaccy/(HK19*HK51*HK61 - HK20*HK51*HK63 + HK21*HK51*HK62 + HK22*HK50*HK51 + HK48*HK51*HK52 + HK54*HK55 - HK55*HK56 + HK58*HK59 - HK59*HK60 + R_ACC); +const float HK2 = HK1*q3; +const float HK3 = HK0 - HK2; +const float HK4 = 2*Kaccx; +const float HK5 = q3*vd; +const float HK6 = HK1*q2 + HK5; +const float HK7 = q0*vd; +const float HK8 = HK1*q1; +const float HK9 = vn - vwn; +const float HK10 = HK9*q2; +const float HK11 = 2*HK10; +const float HK12 = HK11 + HK7 - HK8; +const float HK13 = q1*vd; +const float HK14 = HK9*q3; +const float HK15 = HK1*q0 + HK13 - 2*HK14; +const float HK16 = 2*powf(q2, 2); +const float HK17 = 2*powf(q3, 2); +const float HK18 = HK17 - 1; +const float HK19 = HK16 + HK18; +const float HK20 = HK19*Kaccx; +const float HK21 = q0*q3; +const float HK22 = q1*q2; +const float HK23 = HK21 + HK22; +const float HK24 = HK23*HK4; +const float HK25 = q0*q2; +const float HK26 = q1*q3; +const float HK27 = HK25 - HK26; +const float HK28 = 2*HK3; +const float HK29 = 2*HK12; +const float HK30 = 2*HK27; +const float HK31 = 2*HK23; +const float HK32 = 2*HK6; +const float HK33 = 2*HK15; +const float HK34 = HK19*P(0,22) - HK31*P(0,23) + HK31*P(0,5) + HK32*P(0,1) + HK33*P(0,3); +const float HK35 = -HK16 - HK17 + 1; +const float HK36 = -HK0; +const float HK37 = 2*HK2 + 2*HK36; +const float HK38 = -2*HK11 - 2*HK7 + 2*HK8; +const float HK39 = -2*HK25 + 2*HK26; +const float HK40 = HK31*P(5,23); +const float HK41 = HK19*P(22,23) - HK31*P(23,23) + HK32*P(1,23) + HK33*P(3,23) + HK40; +const float HK42 = powf(Kaccx, 2); +const float HK43 = HK31*HK42; +const float HK44 = HK19*P(5,22) + HK31*P(5,5) + HK32*P(1,5) + HK33*P(3,5) - HK40; +const float HK45 = HK19*P(6,22) + HK31*P(5,6) - HK31*P(6,23) + HK32*P(1,6) + HK33*P(3,6); +const float HK46 = HK19*P(1,22) - HK31*P(1,23) + HK31*P(1,5) + HK32*P(1,1) + HK33*P(1,3); +const float HK47 = HK19*P(4,22); +const float HK48 = -HK31*P(4,23) + HK31*P(4,5) + HK32*P(1,4) + HK33*P(3,4) + HK47; +const float HK49 = HK19*HK42; +const float HK50 = HK19*P(22,22) - HK31*P(22,23) + HK31*P(5,22) + HK32*P(1,22) + HK33*P(3,22); +const float HK51 = HK19*P(3,22) - HK31*P(3,23) + HK31*P(3,5) + HK32*P(1,3) + HK33*P(3,3); +const float HK52 = HK19*P(2,22) - HK31*P(2,23) + HK31*P(2,5) + HK32*P(1,2) + HK33*P(2,3); +const float HK53 = Kaccx/(HK28*HK42*(HK34 + HK35*P(0,4) + HK37*P(0,0) + HK38*P(0,2) + HK39*P(0,6)) + HK29*HK42*(HK35*P(2,4) + HK37*P(0,2) + HK38*P(2,2) + HK39*P(2,6) + HK52) + HK30*HK42*(HK35*P(4,6) + HK37*P(0,6) + HK38*P(2,6) + HK39*P(6,6) + HK45) - HK32*HK42*(HK35*P(1,4) + HK37*P(0,1) + HK38*P(1,2) + HK39*P(1,6) + HK46) - HK33*HK42*(HK35*P(3,4) + HK37*P(0,3) + HK38*P(2,3) + HK39*P(3,6) + HK51) + HK43*(HK35*P(4,23) + HK37*P(0,23) + HK38*P(2,23) + HK39*P(6,23) + HK41) - HK43*(HK35*P(4,5) + HK37*P(0,5) + HK38*P(2,5) + HK39*P(5,6) + HK44) - HK49*(HK35*P(4,22) + HK37*P(0,22) + HK38*P(2,22) + HK39*P(6,22) + HK50) + HK49*(HK35*P(4,4) + HK37*P(0,4) + HK38*P(2,4) + HK39*P(4,6) + HK48) - R_ACC); +const float HK54 = HK13 - HK14; +const float HK55 = 2*Kaccy; +const float HK56 = HK10 + HK7 - 2*HK8; +const float HK57 = HK5 + HK9*q1; +const float HK58 = 2*HK2 + HK36 + HK9*q0; +const float HK59 = HK21 - HK22; +const float HK60 = HK55*HK59; +const float HK61 = HK18 + 2*powf(q1, 2); +const float HK62 = HK61*Kaccy; +const float HK63 = q0*q1 + q2*q3; +const float HK64 = 2*HK63; +const float HK65 = 2*HK59; +const float HK66 = 2*HK57; +const float HK67 = 2*HK54; +const float HK68 = 2*HK56; +const float HK69 = 2*HK58; +const float HK70 = HK61*P(0,23) - HK61*P(0,5) + HK64*P(0,6) + HK65*P(0,22) - HK65*P(0,4) + HK66*P(0,2) + HK67*P(0,0) + HK68*P(0,1) - HK69*P(0,3); +const float HK71 = powf(Kaccy, 2); +const float HK72 = -HK61*P(5,6) + HK61*P(6,23) + HK64*P(6,6) - HK65*P(4,6) + HK65*P(6,22) + HK66*P(2,6) + HK67*P(0,6) + HK68*P(1,6) - HK69*P(3,6); +const float HK73 = HK65*P(4,22); +const float HK74 = HK61*P(22,23) - HK61*P(5,22) + HK64*P(6,22) + HK65*P(22,22) + HK66*P(2,22) + HK67*P(0,22) + HK68*P(1,22) - HK69*P(3,22) - HK73; +const float HK75 = HK65*HK71; +const float HK76 = HK61*P(4,23) - HK61*P(4,5) + HK64*P(4,6) - HK65*P(4,4) + HK66*P(2,4) + HK67*P(0,4) + HK68*P(1,4) - HK69*P(3,4) + HK73; +const float HK77 = HK61*P(2,23) - HK61*P(2,5) + HK64*P(2,6) + HK65*P(2,22) - HK65*P(2,4) + HK66*P(2,2) + HK67*P(0,2) + HK68*P(1,2) - HK69*P(2,3); +const float HK78 = HK61*P(5,23); +const float HK79 = HK61*P(23,23) + HK64*P(6,23) + HK65*P(22,23) - HK65*P(4,23) + HK66*P(2,23) + HK67*P(0,23) + HK68*P(1,23) - HK69*P(3,23) - HK78; +const float HK80 = HK61*HK71; +const float HK81 = -HK61*P(5,5) + HK64*P(5,6) - HK65*P(4,5) + HK65*P(5,22) + HK66*P(2,5) + HK67*P(0,5) + HK68*P(1,5) - HK69*P(3,5) + HK78; +const float HK82 = HK61*P(1,23) - HK61*P(1,5) + HK64*P(1,6) + HK65*P(1,22) - HK65*P(1,4) + HK66*P(1,2) + HK67*P(0,1) + HK68*P(1,1) - HK69*P(1,3); +const float HK83 = HK61*P(3,23) - HK61*P(3,5) + HK64*P(3,6) + HK65*P(3,22) - HK65*P(3,4) + HK66*P(2,3) + HK67*P(0,3) + HK68*P(1,3) - HK69*P(3,3); +const float HK84 = Kaccy/(HK64*HK71*HK72 + HK66*HK71*HK77 + HK67*HK70*HK71 + HK68*HK71*HK82 - HK69*HK71*HK83 + HK74*HK75 - HK75*HK76 + HK79*HK80 - HK80*HK81 + R_ACC); // Observation Jacobians - axis 0 -Hfusion.at<0>() = -HK2*HK3; -Hfusion.at<1>() = -HK3*HK4; -Hfusion.at<2>() = HK3*HK5; -Hfusion.at<3>() = -HK3*HK6; -Hfusion.at<4>() = -HK11; -Hfusion.at<5>() = -HK15; -Hfusion.at<6>() = HK16*HK3; +Hfusion.at<0>() = HK3*HK4; +Hfusion.at<1>() = -HK4*HK6; +Hfusion.at<2>() = HK12*HK4; +Hfusion.at<3>() = -HK15*HK4; +Hfusion.at<4>() = HK20; +Hfusion.at<5>() = -HK24; +Hfusion.at<6>() = HK27*HK4; Hfusion.at<7>() = 0; Hfusion.at<8>() = 0; Hfusion.at<9>() = 0; @@ -89,45 +109,45 @@ Hfusion.at<18>() = 0; Hfusion.at<19>() = 0; Hfusion.at<20>() = 0; Hfusion.at<21>() = 0; -Hfusion.at<22>() = HK11; -Hfusion.at<23>() = HK15; +Hfusion.at<22>() = -HK20; +Hfusion.at<23>() = HK24; // Kalman gains - axis 0 -Kfusion(0) = -HK24*HK41; -Kfusion(1) = -HK36*HK41; -Kfusion(2) = -HK38*HK41; -Kfusion(3) = -HK40*HK41; -Kfusion(4) = -HK32*HK41; -Kfusion(5) = -HK29*HK41; -Kfusion(6) = -HK30*HK41; -Kfusion(7) = -HK41*(HK10*P(4,7) - HK10*P(7,22) + HK17*P(5,7) - HK17*P(7,23) - HK18*P(6,7) + HK19*P(1,7) + HK20*P(0,7) - HK21*P(2,7) + HK22*P(3,7)); -Kfusion(8) = -HK41*(HK10*P(4,8) - HK10*P(8,22) + HK17*P(5,8) - HK17*P(8,23) - HK18*P(6,8) + HK19*P(1,8) + HK20*P(0,8) - HK21*P(2,8) + HK22*P(3,8)); -Kfusion(9) = -HK41*(HK10*P(4,9) - HK10*P(9,22) + HK17*P(5,9) - HK17*P(9,23) - HK18*P(6,9) + HK19*P(1,9) + HK20*P(0,9) - HK21*P(2,9) + HK22*P(3,9)); -Kfusion(10) = -HK41*(-HK10*P(10,22) + HK10*P(4,10) - HK17*P(10,23) + HK17*P(5,10) - HK18*P(6,10) + HK19*P(1,10) + HK20*P(0,10) - HK21*P(2,10) + HK22*P(3,10)); -Kfusion(11) = -HK41*(-HK10*P(11,22) + HK10*P(4,11) - HK17*P(11,23) + HK17*P(5,11) - HK18*P(6,11) + HK19*P(1,11) + HK20*P(0,11) - HK21*P(2,11) + HK22*P(3,11)); -Kfusion(12) = -HK41*(-HK10*P(12,22) + HK10*P(4,12) - HK17*P(12,23) + HK17*P(5,12) - HK18*P(6,12) + HK19*P(1,12) + HK20*P(0,12) - HK21*P(2,12) + HK22*P(3,12)); -Kfusion(13) = -HK41*(-HK10*P(13,22) + HK10*P(4,13) - HK17*P(13,23) + HK17*P(5,13) - HK18*P(6,13) + HK19*P(1,13) + HK20*P(0,13) - HK21*P(2,13) + HK22*P(3,13)); -Kfusion(14) = -HK41*(-HK10*P(14,22) + HK10*P(4,14) - HK17*P(14,23) + HK17*P(5,14) - HK18*P(6,14) + HK19*P(1,14) + HK20*P(0,14) - HK21*P(2,14) + HK22*P(3,14)); -Kfusion(15) = -HK41*(-HK10*P(15,22) + HK10*P(4,15) - HK17*P(15,23) + HK17*P(5,15) - HK18*P(6,15) + HK19*P(1,15) + HK20*P(0,15) - HK21*P(2,15) + HK22*P(3,15)); -Kfusion(16) = -HK41*(-HK10*P(16,22) + HK10*P(4,16) - HK17*P(16,23) + HK17*P(5,16) - HK18*P(6,16) + HK19*P(1,16) + HK20*P(0,16) - HK21*P(2,16) + HK22*P(3,16)); -Kfusion(17) = -HK41*(-HK10*P(17,22) + HK10*P(4,17) - HK17*P(17,23) + HK17*P(5,17) - HK18*P(6,17) + HK19*P(1,17) + HK20*P(0,17) - HK21*P(2,17) + HK22*P(3,17)); -Kfusion(18) = -HK41*(-HK10*P(18,22) + HK10*P(4,18) - HK17*P(18,23) + HK17*P(5,18) - HK18*P(6,18) + HK19*P(1,18) + HK20*P(0,18) - HK21*P(2,18) + HK22*P(3,18)); -Kfusion(19) = -HK41*(-HK10*P(19,22) + HK10*P(4,19) - HK17*P(19,23) + HK17*P(5,19) - HK18*P(6,19) + HK19*P(1,19) + HK20*P(0,19) - HK21*P(2,19) + HK22*P(3,19)); -Kfusion(20) = -HK41*(-HK10*P(20,22) + HK10*P(4,20) - HK17*P(20,23) + HK17*P(5,20) - HK18*P(6,20) + HK19*P(1,20) + HK20*P(0,20) - HK21*P(2,20) + HK22*P(3,20)); -Kfusion(21) = -HK41*(-HK10*P(21,22) + HK10*P(4,21) - HK17*P(21,23) + HK17*P(5,21) - HK18*P(6,21) + HK19*P(1,21) + HK20*P(0,21) - HK21*P(2,21) + HK22*P(3,21)); -Kfusion(22) = -HK34*HK41; -Kfusion(23) = -HK26*HK41; +Kfusion(0) = HK53*(-HK19*P(0,4) - HK28*P(0,0) - HK29*P(0,2) - HK30*P(0,6) + HK34); +Kfusion(1) = HK53*(-HK19*P(1,4) - HK28*P(0,1) - HK29*P(1,2) - HK30*P(1,6) + HK46); +Kfusion(2) = HK53*(-HK19*P(2,4) - HK28*P(0,2) - HK29*P(2,2) - HK30*P(2,6) + HK52); +Kfusion(3) = HK53*(-HK19*P(3,4) - HK28*P(0,3) - HK29*P(2,3) - HK30*P(3,6) + HK51); +Kfusion(4) = HK53*(-HK19*P(4,4) - HK28*P(0,4) - HK29*P(2,4) - HK30*P(4,6) + HK48); +Kfusion(5) = HK53*(-HK19*P(4,5) - HK28*P(0,5) - HK29*P(2,5) - HK30*P(5,6) + HK44); +Kfusion(6) = HK53*(-HK19*P(4,6) - HK28*P(0,6) - HK29*P(2,6) - HK30*P(6,6) + HK45); +Kfusion(7) = HK53*(-HK19*P(4,7) + HK19*P(7,22) - HK28*P(0,7) - HK29*P(2,7) - HK30*P(6,7) + HK31*P(5,7) - HK31*P(7,23) + HK32*P(1,7) + HK33*P(3,7)); +Kfusion(8) = HK53*(-HK19*P(4,8) + HK19*P(8,22) - HK28*P(0,8) - HK29*P(2,8) - HK30*P(6,8) + HK31*P(5,8) - HK31*P(8,23) + HK32*P(1,8) + HK33*P(3,8)); +Kfusion(9) = HK53*(-HK19*P(4,9) + HK19*P(9,22) - HK28*P(0,9) - HK29*P(2,9) - HK30*P(6,9) + HK31*P(5,9) - HK31*P(9,23) + HK32*P(1,9) + HK33*P(3,9)); +Kfusion(10) = HK53*(HK19*P(10,22) - HK19*P(4,10) - HK28*P(0,10) - HK29*P(2,10) - HK30*P(6,10) - HK31*P(10,23) + HK31*P(5,10) + HK32*P(1,10) + HK33*P(3,10)); +Kfusion(11) = HK53*(HK19*P(11,22) - HK19*P(4,11) - HK28*P(0,11) - HK29*P(2,11) - HK30*P(6,11) - HK31*P(11,23) + HK31*P(5,11) + HK32*P(1,11) + HK33*P(3,11)); +Kfusion(12) = HK53*(HK19*P(12,22) - HK19*P(4,12) - HK28*P(0,12) - HK29*P(2,12) - HK30*P(6,12) - HK31*P(12,23) + HK31*P(5,12) + HK32*P(1,12) + HK33*P(3,12)); +Kfusion(13) = HK53*(HK19*P(13,22) - HK19*P(4,13) - HK28*P(0,13) - HK29*P(2,13) - HK30*P(6,13) - HK31*P(13,23) + HK31*P(5,13) + HK32*P(1,13) + HK33*P(3,13)); +Kfusion(14) = HK53*(HK19*P(14,22) - HK19*P(4,14) - HK28*P(0,14) - HK29*P(2,14) - HK30*P(6,14) - HK31*P(14,23) + HK31*P(5,14) + HK32*P(1,14) + HK33*P(3,14)); +Kfusion(15) = HK53*(HK19*P(15,22) - HK19*P(4,15) - HK28*P(0,15) - HK29*P(2,15) - HK30*P(6,15) - HK31*P(15,23) + HK31*P(5,15) + HK32*P(1,15) + HK33*P(3,15)); +Kfusion(16) = HK53*(HK19*P(16,22) - HK19*P(4,16) - HK28*P(0,16) - HK29*P(2,16) - HK30*P(6,16) - HK31*P(16,23) + HK31*P(5,16) + HK32*P(1,16) + HK33*P(3,16)); +Kfusion(17) = HK53*(HK19*P(17,22) - HK19*P(4,17) - HK28*P(0,17) - HK29*P(2,17) - HK30*P(6,17) - HK31*P(17,23) + HK31*P(5,17) + HK32*P(1,17) + HK33*P(3,17)); +Kfusion(18) = HK53*(HK19*P(18,22) - HK19*P(4,18) - HK28*P(0,18) - HK29*P(2,18) - HK30*P(6,18) - HK31*P(18,23) + HK31*P(5,18) + HK32*P(1,18) + HK33*P(3,18)); +Kfusion(19) = HK53*(HK19*P(19,22) - HK19*P(4,19) - HK28*P(0,19) - HK29*P(2,19) - HK30*P(6,19) - HK31*P(19,23) + HK31*P(5,19) + HK32*P(1,19) + HK33*P(3,19)); +Kfusion(20) = HK53*(HK19*P(20,22) - HK19*P(4,20) - HK28*P(0,20) - HK29*P(2,20) - HK30*P(6,20) - HK31*P(20,23) + HK31*P(5,20) + HK32*P(1,20) + HK33*P(3,20)); +Kfusion(21) = HK53*(HK19*P(21,22) - HK19*P(4,21) - HK28*P(0,21) - HK29*P(2,21) - HK30*P(6,21) - HK31*P(21,23) + HK31*P(5,21) + HK32*P(1,21) + HK33*P(3,21)); +Kfusion(22) = HK53*(-HK28*P(0,22) - HK29*P(2,22) - HK30*P(6,22) - HK47 + HK50); +Kfusion(23) = HK53*(-HK19*P(4,23) - HK28*P(0,23) - HK29*P(2,23) - HK30*P(6,23) + HK41); // Observation Jacobians - axis 1 -Hfusion.at<0>() = -HK22*Kaccy; -Hfusion.at<1>() = -HK21*Kaccy; -Hfusion.at<2>() = -HK19*Kaccy; -Hfusion.at<3>() = HK20*Kaccy; -Hfusion.at<4>() = HK44; -Hfusion.at<5>() = -HK46; -Hfusion.at<6>() = -HK43*HK47; +Hfusion.at<0>() = -HK54*HK55; +Hfusion.at<1>() = -HK55*HK56; +Hfusion.at<2>() = -HK55*HK57; +Hfusion.at<3>() = HK55*HK58; +Hfusion.at<4>() = HK60; +Hfusion.at<5>() = HK62; +Hfusion.at<6>() = -HK55*HK63; Hfusion.at<7>() = 0; Hfusion.at<8>() = 0; Hfusion.at<9>() = 0; @@ -143,35 +163,35 @@ Hfusion.at<18>() = 0; Hfusion.at<19>() = 0; Hfusion.at<20>() = 0; Hfusion.at<21>() = 0; -Hfusion.at<22>() = -HK44; -Hfusion.at<23>() = HK46; +Hfusion.at<22>() = -HK60; +Hfusion.at<23>() = -HK62; // Kalman gains - axis 1 -Kfusion(0) = -HK50*HK64; -Kfusion(1) = -HK62*HK64; -Kfusion(2) = -HK61*HK64; -Kfusion(3) = -HK63*HK64; -Kfusion(4) = -HK56*HK64; -Kfusion(5) = -HK58*HK64; -Kfusion(6) = -HK52*HK64; -Kfusion(7) = -HK64*(HK19*P(2,7) - HK20*P(3,7) + HK21*P(1,7) + HK22*P(0,7) + HK45*P(5,7) - HK45*P(7,23) + HK48*P(6,7) - HK49*P(4,7) + HK49*P(7,22)); -Kfusion(8) = -HK64*(HK19*P(2,8) - HK20*P(3,8) + HK21*P(1,8) + HK22*P(0,8) + HK45*P(5,8) - HK45*P(8,23) + HK48*P(6,8) - HK49*P(4,8) + HK49*P(8,22)); -Kfusion(9) = -HK64*(HK19*P(2,9) - HK20*P(3,9) + HK21*P(1,9) + HK22*P(0,9) + HK45*P(5,9) - HK45*P(9,23) + HK48*P(6,9) - HK49*P(4,9) + HK49*P(9,22)); -Kfusion(10) = -HK64*(HK19*P(2,10) - HK20*P(3,10) + HK21*P(1,10) + HK22*P(0,10) - HK45*P(10,23) + HK45*P(5,10) + HK48*P(6,10) + HK49*P(10,22) - HK49*P(4,10)); -Kfusion(11) = -HK64*(HK19*P(2,11) - HK20*P(3,11) + HK21*P(1,11) + HK22*P(0,11) - HK45*P(11,23) + HK45*P(5,11) + HK48*P(6,11) + HK49*P(11,22) - HK49*P(4,11)); -Kfusion(12) = -HK64*(HK19*P(2,12) - HK20*P(3,12) + HK21*P(1,12) + HK22*P(0,12) - HK45*P(12,23) + HK45*P(5,12) + HK48*P(6,12) + HK49*P(12,22) - HK49*P(4,12)); -Kfusion(13) = -HK64*(HK19*P(2,13) - HK20*P(3,13) + HK21*P(1,13) + HK22*P(0,13) - HK45*P(13,23) + HK45*P(5,13) + HK48*P(6,13) + HK49*P(13,22) - HK49*P(4,13)); -Kfusion(14) = -HK64*(HK19*P(2,14) - HK20*P(3,14) + HK21*P(1,14) + HK22*P(0,14) - HK45*P(14,23) + HK45*P(5,14) + HK48*P(6,14) + HK49*P(14,22) - HK49*P(4,14)); -Kfusion(15) = -HK64*(HK19*P(2,15) - HK20*P(3,15) + HK21*P(1,15) + HK22*P(0,15) - HK45*P(15,23) + HK45*P(5,15) + HK48*P(6,15) + HK49*P(15,22) - HK49*P(4,15)); -Kfusion(16) = -HK64*(HK19*P(2,16) - HK20*P(3,16) + HK21*P(1,16) + HK22*P(0,16) - HK45*P(16,23) + HK45*P(5,16) + HK48*P(6,16) + HK49*P(16,22) - HK49*P(4,16)); -Kfusion(17) = -HK64*(HK19*P(2,17) - HK20*P(3,17) + HK21*P(1,17) + HK22*P(0,17) - HK45*P(17,23) + HK45*P(5,17) + HK48*P(6,17) + HK49*P(17,22) - HK49*P(4,17)); -Kfusion(18) = -HK64*(HK19*P(2,18) - HK20*P(3,18) + HK21*P(1,18) + HK22*P(0,18) - HK45*P(18,23) + HK45*P(5,18) + HK48*P(6,18) + HK49*P(18,22) - HK49*P(4,18)); -Kfusion(19) = -HK64*(HK19*P(2,19) - HK20*P(3,19) + HK21*P(1,19) + HK22*P(0,19) - HK45*P(19,23) + HK45*P(5,19) + HK48*P(6,19) + HK49*P(19,22) - HK49*P(4,19)); -Kfusion(20) = -HK64*(HK19*P(2,20) - HK20*P(3,20) + HK21*P(1,20) + HK22*P(0,20) - HK45*P(20,23) + HK45*P(5,20) + HK48*P(6,20) + HK49*P(20,22) - HK49*P(4,20)); -Kfusion(21) = -HK64*(HK19*P(2,21) - HK20*P(3,21) + HK21*P(1,21) + HK22*P(0,21) - HK45*P(21,23) + HK45*P(5,21) + HK48*P(6,21) + HK49*P(21,22) - HK49*P(4,21)); -Kfusion(22) = -HK54*HK64; -Kfusion(23) = -HK60*HK64; +Kfusion(0) = -HK70*HK84; +Kfusion(1) = -HK82*HK84; +Kfusion(2) = -HK77*HK84; +Kfusion(3) = -HK83*HK84; +Kfusion(4) = -HK76*HK84; +Kfusion(5) = -HK81*HK84; +Kfusion(6) = -HK72*HK84; +Kfusion(7) = -HK84*(-HK61*P(5,7) + HK61*P(7,23) + HK64*P(6,7) - HK65*P(4,7) + HK65*P(7,22) + HK66*P(2,7) + HK67*P(0,7) + HK68*P(1,7) - HK69*P(3,7)); +Kfusion(8) = -HK84*(-HK61*P(5,8) + HK61*P(8,23) + HK64*P(6,8) - HK65*P(4,8) + HK65*P(8,22) + HK66*P(2,8) + HK67*P(0,8) + HK68*P(1,8) - HK69*P(3,8)); +Kfusion(9) = -HK84*(-HK61*P(5,9) + HK61*P(9,23) + HK64*P(6,9) - HK65*P(4,9) + HK65*P(9,22) + HK66*P(2,9) + HK67*P(0,9) + HK68*P(1,9) - HK69*P(3,9)); +Kfusion(10) = -HK84*(HK61*P(10,23) - HK61*P(5,10) + HK64*P(6,10) + HK65*P(10,22) - HK65*P(4,10) + HK66*P(2,10) + HK67*P(0,10) + HK68*P(1,10) - HK69*P(3,10)); +Kfusion(11) = -HK84*(HK61*P(11,23) - HK61*P(5,11) + HK64*P(6,11) + HK65*P(11,22) - HK65*P(4,11) + HK66*P(2,11) + HK67*P(0,11) + HK68*P(1,11) - HK69*P(3,11)); +Kfusion(12) = -HK84*(HK61*P(12,23) - HK61*P(5,12) + HK64*P(6,12) + HK65*P(12,22) - HK65*P(4,12) + HK66*P(2,12) + HK67*P(0,12) + HK68*P(1,12) - HK69*P(3,12)); +Kfusion(13) = -HK84*(HK61*P(13,23) - HK61*P(5,13) + HK64*P(6,13) + HK65*P(13,22) - HK65*P(4,13) + HK66*P(2,13) + HK67*P(0,13) + HK68*P(1,13) - HK69*P(3,13)); +Kfusion(14) = -HK84*(HK61*P(14,23) - HK61*P(5,14) + HK64*P(6,14) + HK65*P(14,22) - HK65*P(4,14) + HK66*P(2,14) + HK67*P(0,14) + HK68*P(1,14) - HK69*P(3,14)); +Kfusion(15) = -HK84*(HK61*P(15,23) - HK61*P(5,15) + HK64*P(6,15) + HK65*P(15,22) - HK65*P(4,15) + HK66*P(2,15) + HK67*P(0,15) + HK68*P(1,15) - HK69*P(3,15)); +Kfusion(16) = -HK84*(HK61*P(16,23) - HK61*P(5,16) + HK64*P(6,16) + HK65*P(16,22) - HK65*P(4,16) + HK66*P(2,16) + HK67*P(0,16) + HK68*P(1,16) - HK69*P(3,16)); +Kfusion(17) = -HK84*(HK61*P(17,23) - HK61*P(5,17) + HK64*P(6,17) + HK65*P(17,22) - HK65*P(4,17) + HK66*P(2,17) + HK67*P(0,17) + HK68*P(1,17) - HK69*P(3,17)); +Kfusion(18) = -HK84*(HK61*P(18,23) - HK61*P(5,18) + HK64*P(6,18) + HK65*P(18,22) - HK65*P(4,18) + HK66*P(2,18) + HK67*P(0,18) + HK68*P(1,18) - HK69*P(3,18)); +Kfusion(19) = -HK84*(HK61*P(19,23) - HK61*P(5,19) + HK64*P(6,19) + HK65*P(19,22) - HK65*P(4,19) + HK66*P(2,19) + HK67*P(0,19) + HK68*P(1,19) - HK69*P(3,19)); +Kfusion(20) = -HK84*(HK61*P(20,23) - HK61*P(5,20) + HK64*P(6,20) + HK65*P(20,22) - HK65*P(4,20) + HK66*P(2,20) + HK67*P(0,20) + HK68*P(1,20) - HK69*P(3,20)); +Kfusion(21) = -HK84*(HK61*P(21,23) - HK61*P(5,21) + HK64*P(6,21) + HK65*P(21,22) - HK65*P(4,21) + HK66*P(2,21) + HK67*P(0,21) + HK68*P(1,21) - HK69*P(3,21)); +Kfusion(22) = -HK74*HK84; +Kfusion(23) = -HK79*HK84; // Observation Jacobians - axis 2 diff --git a/EKF/python/ekf_derivation/generated/beta_generated.cpp b/EKF/python/ekf_derivation/generated/beta_generated.cpp index 50bf4da0e2..da34396676 100644 --- a/EKF/python/ekf_derivation/generated/beta_generated.cpp +++ b/EKF/python/ekf_derivation/generated/beta_generated.cpp @@ -1,67 +1,70 @@ // Sub Expressions -const float HK0 = vn - vwn; -const float HK1 = ve - vwe; -const float HK2 = HK0*q0 + HK1*q3 - q2*vd; -const float HK3 = q0*q2 - q1*q3; -const float HK4 = 2*vd; -const float HK5 = q0*q3; -const float HK6 = q1*q2; -const float HK7 = 2*HK5 + 2*HK6; -const float HK8 = powf(q0, 2); -const float HK9 = powf(q3, 2); -const float HK10 = HK8 - HK9; -const float HK11 = powf(q1, 2); -const float HK12 = powf(q2, 2); -const float HK13 = HK11 - HK12; -const float HK14 = HK10 + HK13; -const float HK15 = HK0*HK14 + HK1*HK7 - HK3*HK4; -const float HK16 = 1.0F/HK15; +const float HK0 = q1*vd; +const float HK1 = vn - vwn; +const float HK2 = HK1*q3; +const float HK3 = q2*vd; +const float HK4 = ve - vwe; +const float HK5 = HK4*q3; +const float HK6 = q0*q2 - q1*q3; +const float HK7 = 2*vd; +const float HK8 = HK6*HK7; +const float HK9 = q0*q3; +const float HK10 = q1*q2; +const float HK11 = 2*HK10 + 2*HK9; +const float HK12 = HK11*HK4; +const float HK13 = 2*powf(q3, 2) - 1; +const float HK14 = HK13 + 2*powf(q2, 2); +const float HK15 = HK1*HK14; +const float HK16 = 1.0F/(-HK12 + HK15 + HK8); const float HK17 = q0*q1 + q2*q3; -const float HK18 = HK10 - HK11 + HK12; -const float HK19 = HK16*(-2*HK0*(HK5 - HK6) + HK1*HK18 + HK17*HK4); -const float HK20 = -HK0*q3 + HK1*q0 + q1*vd; -const float HK21 = -HK19*HK2 + HK20; -const float HK22 = 2*HK16; -const float HK23 = HK0*q1 + HK1*q2 + q3*vd; -const float HK24 = HK0*q2 - HK1*q1 + q0*vd; -const float HK25 = -HK19*HK23 + HK24; -const float HK26 = HK19*HK24 + HK23; -const float HK27 = HK19*HK20 + HK2; -const float HK28 = HK14*HK19 + 2*HK5 - 2*HK6; -const float HK29 = HK16*HK28; -const float HK30 = HK19*HK7; -const float HK31 = HK17 + HK19*HK3; -const float HK32 = HK13 + HK30 - HK8 + HK9; -const float HK33 = 2*HK31; -const float HK34 = 2*HK26; -const float HK35 = 2*HK25; -const float HK36 = 2*HK27; -const float HK37 = 2*HK21; -const float HK38 = HK28*P(0,22) - HK28*P(0,4) + HK32*P(0,23) - HK32*P(0,5) + HK33*P(0,6) + HK34*P(0,2) + HK35*P(0,1) - HK36*P(0,3) + HK37*P(0,0); -const float HK39 = powf(HK15, -2); -const float HK40 = -HK28*P(4,6) + HK28*P(6,22) - HK32*P(5,6) + HK32*P(6,23) + HK33*P(6,6) + HK34*P(2,6) + HK35*P(1,6) - HK36*P(3,6) + HK37*P(0,6); -const float HK41 = HK32*P(5,23); -const float HK42 = HK28*P(22,23) - HK28*P(4,23) + HK32*P(23,23) + HK33*P(6,23) + HK34*P(2,23) + HK35*P(1,23) - HK36*P(3,23) + HK37*P(0,23) - HK41; -const float HK43 = HK32*HK39; -const float HK44 = HK28*P(4,22); -const float HK45 = HK28*P(22,22) + HK32*P(22,23) - HK32*P(5,22) + HK33*P(6,22) + HK34*P(2,22) + HK35*P(1,22) - HK36*P(3,22) + HK37*P(0,22) - HK44; -const float HK46 = HK28*HK39; -const float HK47 = -HK28*P(4,5) + HK28*P(5,22) - HK32*P(5,5) + HK33*P(5,6) + HK34*P(2,5) + HK35*P(1,5) - HK36*P(3,5) + HK37*P(0,5) + HK41; -const float HK48 = -HK28*P(4,4) + HK32*P(4,23) - HK32*P(4,5) + HK33*P(4,6) + HK34*P(2,4) + HK35*P(1,4) - HK36*P(3,4) + HK37*P(0,4) + HK44; -const float HK49 = HK28*P(2,22) - HK28*P(2,4) + HK32*P(2,23) - HK32*P(2,5) + HK33*P(2,6) + HK34*P(2,2) + HK35*P(1,2) - HK36*P(2,3) + HK37*P(0,2); -const float HK50 = HK28*P(1,22) - HK28*P(1,4) + HK32*P(1,23) - HK32*P(1,5) + HK33*P(1,6) + HK34*P(1,2) + HK35*P(1,1) - HK36*P(1,3) + HK37*P(0,1); -const float HK51 = HK28*P(3,22) - HK28*P(3,4) + HK32*P(3,23) - HK32*P(3,5) + HK33*P(3,6) + HK34*P(2,3) + HK35*P(1,3) - HK36*P(3,3) + HK37*P(0,3); -const float HK52 = HK16/(HK33*HK39*HK40 + HK34*HK39*HK49 + HK35*HK39*HK50 - HK36*HK39*HK51 + HK37*HK38*HK39 + HK42*HK43 - HK43*HK47 + HK45*HK46 - HK46*HK48 + R_BETA); +const float HK18 = HK17*HK7; +const float HK19 = 2*HK1*(-HK10 + HK9); +const float HK20 = HK13 + 2*powf(q1, 2); +const float HK21 = HK20*HK4; +const float HK22 = HK16*(-HK18 + HK19 + HK21); +const float HK23 = 2*HK16; +const float HK24 = HK23*(HK0 - HK2 + HK22*(HK3 - HK5)); +const float HK25 = q0*vd; +const float HK26 = HK1*q2; +const float HK27 = HK4*q1; +const float HK28 = 2*HK27; +const float HK29 = q3*vd; +const float HK30 = 1.0F/(HK12 - HK15 - HK8); +const float HK31 = HK30*(HK18 - HK19 - HK21); +const float HK32 = HK31*(HK29 + HK4*q2); +const float HK33 = 2*HK30; +const float HK34 = HK23*(HK1*q1 + HK22*(HK25 + 2*HK26 - HK27) + HK29); +const float HK35 = HK33*(HK1*q0 - HK3 + HK31*(HK0 - 2*HK2 + HK4*q0) + 2*HK5); +const float HK36 = HK14*HK31; +const float HK37 = 2*HK9; +const float HK38 = 2*HK10; +const float HK39 = -HK37 + HK38; +const float HK40 = HK30*(HK11*HK31 + HK20); +const float HK41 = HK23*(HK17 + HK22*HK6); +const float HK42 = HK16*(HK14*HK22 + HK39); +const float HK43 = HK16*(HK11*HK22 + HK20); +const float HK44 = HK30*(-HK36 + HK37 - HK38); +const float HK45 = HK33*(-HK25 - HK26 + HK28 + HK32); +const float HK46 = -HK24*P(0,0) - HK34*P(0,2) - HK35*P(0,3) - HK40*P(0,5) - HK41*P(0,6) + HK42*P(0,22) - HK43*P(0,23) - HK44*P(0,4) - HK45*P(0,1); +const float HK47 = -HK24*P(0,6) - HK34*P(2,6) - HK35*P(3,6) - HK40*P(5,6) - HK41*P(6,6) + HK42*P(6,22) - HK43*P(6,23) - HK44*P(4,6) - HK45*P(1,6); +const float HK48 = -HK24*P(0,22) - HK34*P(2,22) - HK35*P(3,22) - HK40*P(5,22) - HK41*P(6,22) + HK42*P(22,22) - HK43*P(22,23) - HK44*P(4,22) - HK45*P(1,22); +const float HK49 = -HK24*P(0,23) - HK34*P(2,23) - HK35*P(3,23) - HK40*P(5,23) - HK41*P(6,23) + HK42*P(22,23) - HK43*P(23,23) - HK44*P(4,23) - HK45*P(1,23); +const float HK50 = -HK24*P(0,5) - HK34*P(2,5) - HK35*P(3,5) - HK40*P(5,5) - HK41*P(5,6) + HK42*P(5,22) - HK43*P(5,23) - HK44*P(4,5) - HK45*P(1,5); +const float HK51 = -HK24*P(0,4) - HK34*P(2,4) - HK35*P(3,4) - HK40*P(4,5) - HK41*P(4,6) + HK42*P(4,22) - HK43*P(4,23) - HK44*P(4,4) - HK45*P(1,4); +const float HK52 = -HK24*P(0,2) - HK34*P(2,2) - HK35*P(2,3) - HK40*P(2,5) - HK41*P(2,6) + HK42*P(2,22) - HK43*P(2,23) - HK44*P(2,4) - HK45*P(1,2); +const float HK53 = -HK24*P(0,1) - HK34*P(1,2) - HK35*P(1,3) - HK40*P(1,5) - HK41*P(1,6) + HK42*P(1,22) - HK43*P(1,23) - HK44*P(1,4) - HK45*P(1,1); +const float HK54 = -HK24*P(0,3) - HK34*P(2,3) - HK35*P(3,3) - HK40*P(3,5) - HK41*P(3,6) + HK42*P(3,22) - HK43*P(3,23) - HK44*P(3,4) - HK45*P(1,3); +const float HK55 = 1.0F/(-HK24*HK46 - HK34*HK52 - HK35*HK54 - HK40*HK50 - HK41*HK47 + HK42*HK48 - HK43*HK49 - HK44*HK51 - HK45*HK53 + R_BETA); // Observation Jacobians -Hfusion.at<0>() = HK21*HK22; -Hfusion.at<1>() = HK22*HK25; -Hfusion.at<2>() = HK22*HK26; -Hfusion.at<3>() = -HK22*HK27; -Hfusion.at<4>() = -HK29; -Hfusion.at<5>() = HK16*(HK18 - HK30); -Hfusion.at<6>() = HK22*HK31; +Hfusion.at<0>() = -HK24; +Hfusion.at<1>() = HK33*(HK25 + HK26 - HK28 - HK32); +Hfusion.at<2>() = -HK34; +Hfusion.at<3>() = -HK35; +Hfusion.at<4>() = HK30*(HK36 + HK39); +Hfusion.at<5>() = -HK40; +Hfusion.at<6>() = -HK41; Hfusion.at<7>() = 0; Hfusion.at<8>() = 0; Hfusion.at<9>() = 0; @@ -77,34 +80,34 @@ Hfusion.at<18>() = 0; Hfusion.at<19>() = 0; Hfusion.at<20>() = 0; Hfusion.at<21>() = 0; -Hfusion.at<22>() = HK29; -Hfusion.at<23>() = HK16*HK32; +Hfusion.at<22>() = HK42; +Hfusion.at<23>() = -HK43; // Kalman gains -Kfusion(0) = HK38*HK52; -Kfusion(1) = HK50*HK52; -Kfusion(2) = HK49*HK52; -Kfusion(3) = HK51*HK52; -Kfusion(4) = HK48*HK52; -Kfusion(5) = HK47*HK52; -Kfusion(6) = HK40*HK52; -Kfusion(7) = HK52*(-HK28*P(4,7) + HK28*P(7,22) - HK32*P(5,7) + HK32*P(7,23) + HK33*P(6,7) + HK34*P(2,7) + HK35*P(1,7) - HK36*P(3,7) + HK37*P(0,7)); -Kfusion(8) = HK52*(-HK28*P(4,8) + HK28*P(8,22) - HK32*P(5,8) + HK32*P(8,23) + HK33*P(6,8) + HK34*P(2,8) + HK35*P(1,8) - HK36*P(3,8) + HK37*P(0,8)); -Kfusion(9) = HK52*(-HK28*P(4,9) + HK28*P(9,22) - HK32*P(5,9) + HK32*P(9,23) + HK33*P(6,9) + HK34*P(2,9) + HK35*P(1,9) - HK36*P(3,9) + HK37*P(0,9)); -Kfusion(10) = HK52*(HK28*P(10,22) - HK28*P(4,10) + HK32*P(10,23) - HK32*P(5,10) + HK33*P(6,10) + HK34*P(2,10) + HK35*P(1,10) - HK36*P(3,10) + HK37*P(0,10)); -Kfusion(11) = HK52*(HK28*P(11,22) - HK28*P(4,11) + HK32*P(11,23) - HK32*P(5,11) + HK33*P(6,11) + HK34*P(2,11) + HK35*P(1,11) - HK36*P(3,11) + HK37*P(0,11)); -Kfusion(12) = HK52*(HK28*P(12,22) - HK28*P(4,12) + HK32*P(12,23) - HK32*P(5,12) + HK33*P(6,12) + HK34*P(2,12) + HK35*P(1,12) - HK36*P(3,12) + HK37*P(0,12)); -Kfusion(13) = HK52*(HK28*P(13,22) - HK28*P(4,13) + HK32*P(13,23) - HK32*P(5,13) + HK33*P(6,13) + HK34*P(2,13) + HK35*P(1,13) - HK36*P(3,13) + HK37*P(0,13)); -Kfusion(14) = HK52*(HK28*P(14,22) - HK28*P(4,14) + HK32*P(14,23) - HK32*P(5,14) + HK33*P(6,14) + HK34*P(2,14) + HK35*P(1,14) - HK36*P(3,14) + HK37*P(0,14)); -Kfusion(15) = HK52*(HK28*P(15,22) - HK28*P(4,15) + HK32*P(15,23) - HK32*P(5,15) + HK33*P(6,15) + HK34*P(2,15) + HK35*P(1,15) - HK36*P(3,15) + HK37*P(0,15)); -Kfusion(16) = HK52*(HK28*P(16,22) - HK28*P(4,16) + HK32*P(16,23) - HK32*P(5,16) + HK33*P(6,16) + HK34*P(2,16) + HK35*P(1,16) - HK36*P(3,16) + HK37*P(0,16)); -Kfusion(17) = HK52*(HK28*P(17,22) - HK28*P(4,17) + HK32*P(17,23) - HK32*P(5,17) + HK33*P(6,17) + HK34*P(2,17) + HK35*P(1,17) - HK36*P(3,17) + HK37*P(0,17)); -Kfusion(18) = HK52*(HK28*P(18,22) - HK28*P(4,18) + HK32*P(18,23) - HK32*P(5,18) + HK33*P(6,18) + HK34*P(2,18) + HK35*P(1,18) - HK36*P(3,18) + HK37*P(0,18)); -Kfusion(19) = HK52*(HK28*P(19,22) - HK28*P(4,19) + HK32*P(19,23) - HK32*P(5,19) + HK33*P(6,19) + HK34*P(2,19) + HK35*P(1,19) - HK36*P(3,19) + HK37*P(0,19)); -Kfusion(20) = HK52*(HK28*P(20,22) - HK28*P(4,20) + HK32*P(20,23) - HK32*P(5,20) + HK33*P(6,20) + HK34*P(2,20) + HK35*P(1,20) - HK36*P(3,20) + HK37*P(0,20)); -Kfusion(21) = HK52*(HK28*P(21,22) - HK28*P(4,21) + HK32*P(21,23) - HK32*P(5,21) + HK33*P(6,21) + HK34*P(2,21) + HK35*P(1,21) - HK36*P(3,21) + HK37*P(0,21)); -Kfusion(22) = HK45*HK52; -Kfusion(23) = HK42*HK52; +Kfusion(0) = HK46*HK55; +Kfusion(1) = HK53*HK55; +Kfusion(2) = HK52*HK55; +Kfusion(3) = HK54*HK55; +Kfusion(4) = HK51*HK55; +Kfusion(5) = HK50*HK55; +Kfusion(6) = HK47*HK55; +Kfusion(7) = HK55*(-HK24*P(0,7) - HK34*P(2,7) - HK35*P(3,7) - HK40*P(5,7) - HK41*P(6,7) + HK42*P(7,22) - HK43*P(7,23) - HK44*P(4,7) - HK45*P(1,7)); +Kfusion(8) = HK55*(-HK24*P(0,8) - HK34*P(2,8) - HK35*P(3,8) - HK40*P(5,8) - HK41*P(6,8) + HK42*P(8,22) - HK43*P(8,23) - HK44*P(4,8) - HK45*P(1,8)); +Kfusion(9) = HK55*(-HK24*P(0,9) - HK34*P(2,9) - HK35*P(3,9) - HK40*P(5,9) - HK41*P(6,9) + HK42*P(9,22) - HK43*P(9,23) - HK44*P(4,9) - HK45*P(1,9)); +Kfusion(10) = HK55*(-HK24*P(0,10) - HK34*P(2,10) - HK35*P(3,10) - HK40*P(5,10) - HK41*P(6,10) + HK42*P(10,22) - HK43*P(10,23) - HK44*P(4,10) - HK45*P(1,10)); +Kfusion(11) = HK55*(-HK24*P(0,11) - HK34*P(2,11) - HK35*P(3,11) - HK40*P(5,11) - HK41*P(6,11) + HK42*P(11,22) - HK43*P(11,23) - HK44*P(4,11) - HK45*P(1,11)); +Kfusion(12) = HK55*(-HK24*P(0,12) - HK34*P(2,12) - HK35*P(3,12) - HK40*P(5,12) - HK41*P(6,12) + HK42*P(12,22) - HK43*P(12,23) - HK44*P(4,12) - HK45*P(1,12)); +Kfusion(13) = HK55*(-HK24*P(0,13) - HK34*P(2,13) - HK35*P(3,13) - HK40*P(5,13) - HK41*P(6,13) + HK42*P(13,22) - HK43*P(13,23) - HK44*P(4,13) - HK45*P(1,13)); +Kfusion(14) = HK55*(-HK24*P(0,14) - HK34*P(2,14) - HK35*P(3,14) - HK40*P(5,14) - HK41*P(6,14) + HK42*P(14,22) - HK43*P(14,23) - HK44*P(4,14) - HK45*P(1,14)); +Kfusion(15) = HK55*(-HK24*P(0,15) - HK34*P(2,15) - HK35*P(3,15) - HK40*P(5,15) - HK41*P(6,15) + HK42*P(15,22) - HK43*P(15,23) - HK44*P(4,15) - HK45*P(1,15)); +Kfusion(16) = HK55*(-HK24*P(0,16) - HK34*P(2,16) - HK35*P(3,16) - HK40*P(5,16) - HK41*P(6,16) + HK42*P(16,22) - HK43*P(16,23) - HK44*P(4,16) - HK45*P(1,16)); +Kfusion(17) = HK55*(-HK24*P(0,17) - HK34*P(2,17) - HK35*P(3,17) - HK40*P(5,17) - HK41*P(6,17) + HK42*P(17,22) - HK43*P(17,23) - HK44*P(4,17) - HK45*P(1,17)); +Kfusion(18) = HK55*(-HK24*P(0,18) - HK34*P(2,18) - HK35*P(3,18) - HK40*P(5,18) - HK41*P(6,18) + HK42*P(18,22) - HK43*P(18,23) - HK44*P(4,18) - HK45*P(1,18)); +Kfusion(19) = HK55*(-HK24*P(0,19) - HK34*P(2,19) - HK35*P(3,19) - HK40*P(5,19) - HK41*P(6,19) + HK42*P(19,22) - HK43*P(19,23) - HK44*P(4,19) - HK45*P(1,19)); +Kfusion(20) = HK55*(-HK24*P(0,20) - HK34*P(2,20) - HK35*P(3,20) - HK40*P(5,20) - HK41*P(6,20) + HK42*P(20,22) - HK43*P(20,23) - HK44*P(4,20) - HK45*P(1,20)); +Kfusion(21) = HK55*(-HK24*P(0,21) - HK34*P(2,21) - HK35*P(3,21) - HK40*P(5,21) - HK41*P(6,21) + HK42*P(21,22) - HK43*P(21,23) - HK44*P(4,21) - HK45*P(1,21)); +Kfusion(22) = HK48*HK55; +Kfusion(23) = HK49*HK55; diff --git a/EKF/python/ekf_derivation/generated/covariance_generated.cpp b/EKF/python/ekf_derivation/generated/covariance_generated.cpp index 7d13639140..18b563ddd4 100644 --- a/EKF/python/ekf_derivation/generated/covariance_generated.cpp +++ b/EKF/python/ekf_derivation/generated/covariance_generated.cpp @@ -40,252 +40,288 @@ const float PS37 = q1*q3; const float PS38 = q0*q2; const float PS39 = q1*q2; const float PS40 = q0*q3; -const float PS41 = -PS2; -const float PS42 = powf(q0, 2); -const float PS43 = -PS4 + PS42; -const float PS44 = PS0 + PS41 + PS43; -const float PS45 = P(0,13) - P(1,13)*PS11 + P(10,13)*PS6 + P(11,13)*PS7 + P(12,13)*PS9 - P(2,13)*PS12 - P(3,13)*PS13; -const float PS46 = PS37 + PS38; -const float PS47 = P(0,15) - P(1,15)*PS11 + P(10,15)*PS6 + P(11,15)*PS7 + P(12,15)*PS9 - P(2,15)*PS12 - P(3,15)*PS13; -const float PS48 = 2*PS47; -const float PS49 = dvy - dvy_b; -const float PS50 = dvx - dvx_b; -const float PS51 = dvz - dvz_b; -const float PS52 = PS49*q0 + PS50*q3 - PS51*q1; -const float PS53 = 2*PS29; -const float PS54 = -PS39 + PS40; -const float PS55 = P(0,14) - P(1,14)*PS11 + P(10,14)*PS6 + P(11,14)*PS7 + P(12,14)*PS9 - P(2,14)*PS12 - P(3,14)*PS13; -const float PS56 = 2*PS55; -const float PS57 = -PS49*q3 + PS50*q0 + PS51*q2; -const float PS58 = 2*PS33; -const float PS59 = PS49*q1 - PS50*q2 + PS51*q0; -const float PS60 = 2*PS59; -const float PS61 = PS49*q2 + PS50*q1 + PS51*q3; -const float PS62 = 2*PS61; -const float PS63 = P(0,4) - P(1,4)*PS11 - P(2,4)*PS12 - P(3,4)*PS13 + P(4,10)*PS6 + P(4,11)*PS7 + P(4,12)*PS9; -const float PS64 = -PS0; -const float PS65 = PS2 + PS43 + PS64; -const float PS66 = PS39 + PS40; -const float PS67 = 2*PS45; -const float PS68 = -PS35 + PS36; -const float PS69 = P(0,5) - P(1,5)*PS11 - P(2,5)*PS12 - P(3,5)*PS13 + P(5,10)*PS6 + P(5,11)*PS7 + P(5,12)*PS9; -const float PS70 = PS4 + PS41 + PS42 + PS64; -const float PS71 = PS35 + PS36; -const float PS72 = 2*PS57; -const float PS73 = -PS37 + PS38; -const float PS74 = 2*PS52; -const float PS75 = P(0,6) - P(1,6)*PS11 - P(2,6)*PS12 - P(3,6)*PS13 + P(6,10)*PS6 + P(6,11)*PS7 + P(6,12)*PS9; -const float PS76 = -P(10,11)*PS34; -const float PS77 = P(0,11)*PS11 + P(1,11) + P(11,11)*PS9 + P(2,11)*PS13 - P(3,11)*PS12 - PS19 + PS76; -const float PS78 = P(0,2)*PS13; -const float PS79 = P(0,3)*PS12; -const float PS80 = P(0,0)*PS11 + P(0,1) - P(0,10)*PS34 + P(0,11)*PS9 - P(0,12)*PS7 + PS78 - PS79; -const float PS81 = P(0,2)*PS11; -const float PS82 = P(1,2) - P(2,10)*PS34 + P(2,11)*PS9 - P(2,12)*PS7 + P(2,2)*PS13 + PS28 + PS81; -const float PS83 = P(10,11)*PS9; -const float PS84 = P(10,12)*PS7; -const float PS85 = P(0,10)*PS11 + P(1,10) - P(10,10)*PS34 + P(2,10)*PS13 - P(3,10)*PS12 + PS83 - PS84; -const float PS86 = -P(10,12)*PS34; -const float PS87 = P(0,12)*PS11 + P(1,12) - P(12,12)*PS7 + P(2,12)*PS13 - P(3,12)*PS12 + PS16 + PS86; -const float PS88 = P(0,3)*PS11; -const float PS89 = P(1,3) - P(3,10)*PS34 + P(3,11)*PS9 - P(3,12)*PS7 - P(3,3)*PS12 + PS25 + PS88; -const float PS90 = P(1,2)*PS13; -const float PS91 = P(1,3)*PS12; -const float PS92 = P(1,1) - P(1,10)*PS34 + P(1,11)*PS9 - P(1,12)*PS7 + PS30 + PS90 - PS91; -const float PS93 = P(0,13)*PS11 + P(1,13) - P(10,13)*PS34 + P(11,13)*PS9 - P(12,13)*PS7 + P(2,13)*PS13 - P(3,13)*PS12; -const float PS94 = P(0,15)*PS11 + P(1,15) - P(10,15)*PS34 + P(11,15)*PS9 - P(12,15)*PS7 + P(2,15)*PS13 - P(3,15)*PS12; -const float PS95 = 2*PS94; -const float PS96 = P(0,14)*PS11 + P(1,14) - P(10,14)*PS34 + P(11,14)*PS9 - P(12,14)*PS7 + P(2,14)*PS13 - P(3,14)*PS12; -const float PS97 = 2*PS96; -const float PS98 = P(0,4)*PS11 + P(1,4) + P(2,4)*PS13 - P(3,4)*PS12 - P(4,10)*PS34 + P(4,11)*PS9 - P(4,12)*PS7; -const float PS99 = 2*PS93; -const float PS100 = P(0,5)*PS11 + P(1,5) + P(2,5)*PS13 - P(3,5)*PS12 - P(5,10)*PS34 + P(5,11)*PS9 - P(5,12)*PS7; -const float PS101 = P(0,6)*PS11 + P(1,6) + P(2,6)*PS13 - P(3,6)*PS12 - P(6,10)*PS34 + P(6,11)*PS9 - P(6,12)*PS7; -const float PS102 = -P(11,12)*PS34; -const float PS103 = P(0,12)*PS12 - P(1,12)*PS13 + P(12,12)*PS6 + P(2,12) + P(3,12)*PS11 - PS10 + PS102; -const float PS104 = P(2,3) - P(3,10)*PS9 - P(3,11)*PS34 + P(3,12)*PS6 + P(3,3)*PS11 + PS22 + PS79; -const float PS105 = P(0,1)*PS13; -const float PS106 = P(0,0)*PS12 - P(0,10)*PS9 - P(0,11)*PS34 + P(0,12)*PS6 + P(0,2) - PS105 + PS88; -const float PS107 = P(11,12)*PS6; -const float PS108 = P(0,11)*PS12 - P(1,11)*PS13 - P(11,11)*PS34 + P(2,11) + P(3,11)*PS11 + PS107 - PS83; -const float PS109 = P(0,10)*PS12 - P(1,10)*PS13 - P(10,10)*PS9 + P(2,10) + P(3,10)*PS11 + PS18 + PS76; -const float PS110 = P(0,1)*PS12; -const float PS111 = -P(1,1)*PS13 - P(1,10)*PS9 - P(1,11)*PS34 + P(1,12)*PS6 + P(1,2) + PS110 + PS27; -const float PS112 = P(2,3)*PS11; -const float PS113 = -P(2,10)*PS9 - P(2,11)*PS34 + P(2,12)*PS6 + P(2,2) + PS112 + PS31 - PS90; -const float PS114 = P(0,13)*PS12 - P(1,13)*PS13 - P(10,13)*PS9 - P(11,13)*PS34 + P(12,13)*PS6 + P(2,13) + P(3,13)*PS11; -const float PS115 = P(0,15)*PS12 - P(1,15)*PS13 - P(10,15)*PS9 - P(11,15)*PS34 + P(12,15)*PS6 + P(2,15) + P(3,15)*PS11; -const float PS116 = 2*PS115; -const float PS117 = P(0,14)*PS12 - P(1,14)*PS13 - P(10,14)*PS9 - P(11,14)*PS34 + P(12,14)*PS6 + P(2,14) + P(3,14)*PS11; +const float PS41 = 2*PS2; +const float PS42 = 2*PS4 - 1; +const float PS43 = PS41 + PS42; +const float PS44 = P(0,13) - P(1,13)*PS11 + P(10,13)*PS6 + P(11,13)*PS7 + P(12,13)*PS9 - P(2,13)*PS12 - P(3,13)*PS13; +const float PS45 = PS37 + PS38; +const float PS46 = P(0,15) - P(1,15)*PS11 + P(10,15)*PS6 + P(11,15)*PS7 + P(12,15)*PS9 - P(2,15)*PS12 - P(3,15)*PS13; +const float PS47 = 2*PS46; +const float PS48 = dvy - dvy_b; +const float PS49 = PS48*q0; +const float PS50 = dvz - dvz_b; +const float PS51 = PS50*q1; +const float PS52 = dvx - dvx_b; +const float PS53 = PS52*q3; +const float PS54 = PS49 - PS51 + 2*PS53; +const float PS55 = 2*PS29; +const float PS56 = -PS39 + PS40; +const float PS57 = P(0,14) - P(1,14)*PS11 + P(10,14)*PS6 + P(11,14)*PS7 + P(12,14)*PS9 - P(2,14)*PS12 - P(3,14)*PS13; +const float PS58 = 2*PS57; +const float PS59 = PS48*q2; +const float PS60 = PS50*q3; +const float PS61 = PS59 + PS60; +const float PS62 = 2*PS23; +const float PS63 = PS50*q2; +const float PS64 = PS48*q3; +const float PS65 = -PS64; +const float PS66 = PS63 + PS65; +const float PS67 = 2*PS33; +const float PS68 = PS50*q0; +const float PS69 = PS48*q1; +const float PS70 = PS52*q2; +const float PS71 = PS68 + PS69 - 2*PS70; +const float PS72 = 2*PS26; +const float PS73 = P(0,4) - P(1,4)*PS11 - P(2,4)*PS12 - P(3,4)*PS13 + P(4,10)*PS6 + P(4,11)*PS7 + P(4,12)*PS9; +const float PS74 = 2*PS0; +const float PS75 = PS42 + PS74; +const float PS76 = PS39 + PS40; +const float PS77 = 2*PS44; +const float PS78 = PS51 - PS53; +const float PS79 = -PS70; +const float PS80 = PS68 + 2*PS69 + PS79; +const float PS81 = -PS35 + PS36; +const float PS82 = PS52*q1; +const float PS83 = PS60 + PS82; +const float PS84 = PS52*q0; +const float PS85 = PS63 - 2*PS64 + PS84; +const float PS86 = P(0,5) - P(1,5)*PS11 - P(2,5)*PS12 - P(3,5)*PS13 + P(5,10)*PS6 + P(5,11)*PS7 + P(5,12)*PS9; +const float PS87 = PS41 + PS74 - 1; +const float PS88 = PS35 + PS36; +const float PS89 = 2*PS63 + PS65 + PS84; +const float PS90 = -PS37 + PS38; +const float PS91 = PS59 + PS82; +const float PS92 = PS69 + PS79; +const float PS93 = PS49 - 2*PS51 + PS53; +const float PS94 = P(0,6) - P(1,6)*PS11 - P(2,6)*PS12 - P(3,6)*PS13 + P(6,10)*PS6 + P(6,11)*PS7 + P(6,12)*PS9; +const float PS95 = powf(q0, 2); +const float PS96 = -P(10,11)*PS34; +const float PS97 = P(0,11)*PS11 + P(1,11) + P(11,11)*PS9 + P(2,11)*PS13 - P(3,11)*PS12 - PS19 + PS96; +const float PS98 = P(0,2)*PS13; +const float PS99 = P(0,3)*PS12; +const float PS100 = P(0,0)*PS11 + P(0,1) - P(0,10)*PS34 + P(0,11)*PS9 - P(0,12)*PS7 + PS98 - PS99; +const float PS101 = P(0,2)*PS11; +const float PS102 = P(1,2) - P(2,10)*PS34 + P(2,11)*PS9 - P(2,12)*PS7 + P(2,2)*PS13 + PS101 + PS28; +const float PS103 = P(10,11)*PS9; +const float PS104 = P(10,12)*PS7; +const float PS105 = P(0,10)*PS11 + P(1,10) - P(10,10)*PS34 + P(2,10)*PS13 - P(3,10)*PS12 + PS103 - PS104; +const float PS106 = -P(10,12)*PS34; +const float PS107 = P(0,12)*PS11 + P(1,12) - P(12,12)*PS7 + P(2,12)*PS13 - P(3,12)*PS12 + PS106 + PS16; +const float PS108 = P(0,3)*PS11; +const float PS109 = P(1,3) - P(3,10)*PS34 + P(3,11)*PS9 - P(3,12)*PS7 - P(3,3)*PS12 + PS108 + PS25; +const float PS110 = P(1,2)*PS13; +const float PS111 = P(1,3)*PS12; +const float PS112 = P(1,1) - P(1,10)*PS34 + P(1,11)*PS9 - P(1,12)*PS7 + PS110 - PS111 + PS30; +const float PS113 = P(0,13)*PS11 + P(1,13) - P(10,13)*PS34 + P(11,13)*PS9 - P(12,13)*PS7 + P(2,13)*PS13 - P(3,13)*PS12; +const float PS114 = P(0,15)*PS11 + P(1,15) - P(10,15)*PS34 + P(11,15)*PS9 - P(12,15)*PS7 + P(2,15)*PS13 - P(3,15)*PS12; +const float PS115 = 2*PS114; +const float PS116 = 2*PS109; +const float PS117 = P(0,14)*PS11 + P(1,14) - P(10,14)*PS34 + P(11,14)*PS9 - P(12,14)*PS7 + P(2,14)*PS13 - P(3,14)*PS12; const float PS118 = 2*PS117; -const float PS119 = P(0,4)*PS12 - P(1,4)*PS13 + P(2,4) + P(3,4)*PS11 - P(4,10)*PS9 - P(4,11)*PS34 + P(4,12)*PS6; -const float PS120 = 2*PS114; -const float PS121 = P(0,5)*PS12 - P(1,5)*PS13 + P(2,5) + P(3,5)*PS11 - P(5,10)*PS9 - P(5,11)*PS34 + P(5,12)*PS6; -const float PS122 = P(0,6)*PS12 - P(1,6)*PS13 + P(2,6) + P(3,6)*PS11 - P(6,10)*PS9 - P(6,11)*PS34 + P(6,12)*PS6; -const float PS123 = P(0,10)*PS13 + P(1,10)*PS12 + P(10,10)*PS7 - P(2,10)*PS11 + P(3,10) - PS15 + PS86; -const float PS124 = P(1,1)*PS12 + P(1,10)*PS7 - P(1,11)*PS6 - P(1,12)*PS34 + P(1,3) + PS105 + PS24; -const float PS125 = P(0,0)*PS13 + P(0,10)*PS7 - P(0,11)*PS6 - P(0,12)*PS34 + P(0,3) + PS110 - PS81; -const float PS126 = P(0,12)*PS13 + P(1,12)*PS12 - P(12,12)*PS34 - P(2,12)*PS11 + P(3,12) - PS107 + PS84; -const float PS127 = P(0,11)*PS13 + P(1,11)*PS12 - P(11,11)*PS6 - P(2,11)*PS11 + P(3,11) + PS102 + PS8; -const float PS128 = P(2,10)*PS7 - P(2,11)*PS6 - P(2,12)*PS34 - P(2,2)*PS11 + P(2,3) + PS21 + PS78; -const float PS129 = P(3,10)*PS7 - P(3,11)*PS6 - P(3,12)*PS34 + P(3,3) - PS112 + PS32 + PS91; -const float PS130 = P(0,13)*PS13 + P(1,13)*PS12 + P(10,13)*PS7 - P(11,13)*PS6 - P(12,13)*PS34 - P(2,13)*PS11 + P(3,13); -const float PS131 = P(0,15)*PS13 + P(1,15)*PS12 + P(10,15)*PS7 - P(11,15)*PS6 - P(12,15)*PS34 - P(2,15)*PS11 + P(3,15); -const float PS132 = 2*PS131; -const float PS133 = P(0,14)*PS13 + P(1,14)*PS12 + P(10,14)*PS7 - P(11,14)*PS6 - P(12,14)*PS34 - P(2,14)*PS11 + P(3,14); -const float PS134 = 2*PS133; -const float PS135 = P(0,4)*PS13 + P(1,4)*PS12 - P(2,4)*PS11 + P(3,4) + P(4,10)*PS7 - P(4,11)*PS6 - P(4,12)*PS34; -const float PS136 = 2*PS130; -const float PS137 = P(0,5)*PS13 + P(1,5)*PS12 - P(2,5)*PS11 + P(3,5) + P(5,10)*PS7 - P(5,11)*PS6 - P(5,12)*PS34; -const float PS138 = P(0,6)*PS13 + P(1,6)*PS12 - P(2,6)*PS11 + P(3,6) + P(6,10)*PS7 - P(6,11)*PS6 - P(6,12)*PS34; -const float PS139 = 2*PS46; -const float PS140 = 2*PS54; -const float PS141 = P(0,13)*PS72 + P(1,13)*PS62 - P(13,13)*PS44 + P(13,14)*PS140 - P(13,15)*PS139 + P(2,13)*PS60 - P(3,13)*PS74 + P(4,13); -const float PS142 = P(0,15)*PS72 + P(1,15)*PS62 - P(13,15)*PS44 + P(14,15)*PS140 - P(15,15)*PS139 + P(2,15)*PS60 - P(3,15)*PS74 + P(4,15); -const float PS143 = P(1,3)*PS62; -const float PS144 = P(0,3)*PS72; -const float PS145 = P(2,3)*PS60 - P(3,13)*PS44 + P(3,14)*PS140 - P(3,15)*PS139 - P(3,3)*PS74 + P(3,4) + PS143 + PS144; -const float PS146 = P(0,14)*PS72 + P(1,14)*PS62 - P(13,14)*PS44 + P(14,14)*PS140 - P(14,15)*PS139 + P(2,14)*PS60 - P(3,14)*PS74 + P(4,14); -const float PS147 = P(0,2)*PS60; -const float PS148 = P(0,3)*PS74; -const float PS149 = P(0,0)*PS72 + P(0,1)*PS62 - P(0,13)*PS44 + P(0,14)*PS140 - P(0,15)*PS139 + P(0,4) + PS147 - PS148; -const float PS150 = P(1,2)*PS62; -const float PS151 = P(0,2)*PS72; -const float PS152 = -P(2,13)*PS44 + P(2,14)*PS140 - P(2,15)*PS139 + P(2,2)*PS60 - P(2,3)*PS74 + P(2,4) + PS150 + PS151; -const float PS153 = P(1,2)*PS60; -const float PS154 = P(1,3)*PS74; -const float PS155 = P(0,1)*PS72 + P(1,1)*PS62 - P(1,13)*PS44 + P(1,14)*PS140 - P(1,15)*PS139 + P(1,4) + PS153 - PS154; -const float PS156 = 4*dvyVar; -const float PS157 = 4*dvzVar; -const float PS158 = P(0,4)*PS72 + P(1,4)*PS62 + P(2,4)*PS60 - P(3,4)*PS74 - P(4,13)*PS44 + P(4,14)*PS140 - P(4,15)*PS139 + P(4,4); -const float PS159 = 2*PS141; -const float PS160 = 2*PS68; -const float PS161 = PS65*dvyVar; -const float PS162 = 2*PS66; -const float PS163 = PS44*dvxVar; -const float PS164 = P(0,5)*PS72 + P(1,5)*PS62 + P(2,5)*PS60 - P(3,5)*PS74 + P(4,5) - P(5,13)*PS44 + P(5,14)*PS140 - P(5,15)*PS139; -const float PS165 = 2*PS71; -const float PS166 = 2*PS73; -const float PS167 = PS70*dvzVar; -const float PS168 = P(0,6)*PS72 + P(1,6)*PS62 + P(2,6)*PS60 - P(3,6)*PS74 + P(4,6) - P(6,13)*PS44 + P(6,14)*PS140 - P(6,15)*PS139; -const float PS169 = P(0,14)*PS74 - P(1,14)*PS60 - P(13,14)*PS162 - P(14,14)*PS65 + P(14,15)*PS160 + P(2,14)*PS62 + P(3,14)*PS72 + P(5,14); -const float PS170 = P(0,13)*PS74 - P(1,13)*PS60 - P(13,13)*PS162 - P(13,14)*PS65 + P(13,15)*PS160 + P(2,13)*PS62 + P(3,13)*PS72 + P(5,13); -const float PS171 = P(0,1)*PS74; -const float PS172 = -P(1,1)*PS60 - P(1,13)*PS162 - P(1,14)*PS65 + P(1,15)*PS160 + P(1,3)*PS72 + P(1,5) + PS150 + PS171; -const float PS173 = P(0,15)*PS74 - P(1,15)*PS60 - P(13,15)*PS162 - P(14,15)*PS65 + P(15,15)*PS160 + P(2,15)*PS62 + P(3,15)*PS72 + P(5,15); -const float PS174 = P(2,3)*PS62; -const float PS175 = -P(1,3)*PS60 - P(3,13)*PS162 - P(3,14)*PS65 + P(3,15)*PS160 + P(3,3)*PS72 + P(3,5) + PS148 + PS174; -const float PS176 = P(0,1)*PS60; -const float PS177 = P(0,0)*PS74 - P(0,13)*PS162 - P(0,14)*PS65 + P(0,15)*PS160 + P(0,2)*PS62 + P(0,5) + PS144 - PS176; -const float PS178 = P(2,3)*PS72; -const float PS179 = P(0,2)*PS74 - P(2,13)*PS162 - P(2,14)*PS65 + P(2,15)*PS160 + P(2,2)*PS62 + P(2,5) - PS153 + PS178; -const float PS180 = 4*dvxVar; -const float PS181 = P(0,5)*PS74 - P(1,5)*PS60 + P(2,5)*PS62 + P(3,5)*PS72 - P(5,13)*PS162 - P(5,14)*PS65 + P(5,15)*PS160 + P(5,5); -const float PS182 = P(0,6)*PS74 - P(1,6)*PS60 + P(2,6)*PS62 + P(3,6)*PS72 + P(5,6) - P(6,13)*PS162 - P(6,14)*PS65 + P(6,15)*PS160; -const float PS183 = P(0,15)*PS60 + P(1,15)*PS74 + P(13,15)*PS166 - P(14,15)*PS165 - P(15,15)*PS70 - P(2,15)*PS72 + P(3,15)*PS62 + P(6,15); -const float PS184 = P(0,14)*PS60 + P(1,14)*PS74 + P(13,14)*PS166 - P(14,14)*PS165 - P(14,15)*PS70 - P(2,14)*PS72 + P(3,14)*PS62 + P(6,14); -const float PS185 = P(0,13)*PS60 + P(1,13)*PS74 + P(13,13)*PS166 - P(13,14)*PS165 - P(13,15)*PS70 - P(2,13)*PS72 + P(3,13)*PS62 + P(6,13); -const float PS186 = P(0,6)*PS60 + P(1,6)*PS74 - P(2,6)*PS72 + P(3,6)*PS62 + P(6,13)*PS166 - P(6,14)*PS165 - P(6,15)*PS70 + P(6,6); +const float PS119 = 2*PS112; +const float PS120 = 2*PS100; +const float PS121 = 2*PS102; +const float PS122 = P(0,4)*PS11 + P(1,4) + P(2,4)*PS13 - P(3,4)*PS12 - P(4,10)*PS34 + P(4,11)*PS9 - P(4,12)*PS7; +const float PS123 = 2*PS113; +const float PS124 = P(0,5)*PS11 + P(1,5) + P(2,5)*PS13 - P(3,5)*PS12 - P(5,10)*PS34 + P(5,11)*PS9 - P(5,12)*PS7; +const float PS125 = P(0,6)*PS11 + P(1,6) + P(2,6)*PS13 - P(3,6)*PS12 - P(6,10)*PS34 + P(6,11)*PS9 - P(6,12)*PS7; +const float PS126 = -P(11,12)*PS34; +const float PS127 = P(0,12)*PS12 - P(1,12)*PS13 + P(12,12)*PS6 + P(2,12) + P(3,12)*PS11 - PS10 + PS126; +const float PS128 = P(2,3) - P(3,10)*PS9 - P(3,11)*PS34 + P(3,12)*PS6 + P(3,3)*PS11 + PS22 + PS99; +const float PS129 = P(0,1)*PS13; +const float PS130 = P(0,0)*PS12 - P(0,10)*PS9 - P(0,11)*PS34 + P(0,12)*PS6 + P(0,2) + PS108 - PS129; +const float PS131 = P(11,12)*PS6; +const float PS132 = P(0,11)*PS12 - P(1,11)*PS13 - P(11,11)*PS34 + P(2,11) + P(3,11)*PS11 - PS103 + PS131; +const float PS133 = P(0,10)*PS12 - P(1,10)*PS13 - P(10,10)*PS9 + P(2,10) + P(3,10)*PS11 + PS18 + PS96; +const float PS134 = P(0,1)*PS12; +const float PS135 = -P(1,1)*PS13 - P(1,10)*PS9 - P(1,11)*PS34 + P(1,12)*PS6 + P(1,2) + PS134 + PS27; +const float PS136 = P(2,3)*PS11; +const float PS137 = -P(2,10)*PS9 - P(2,11)*PS34 + P(2,12)*PS6 + P(2,2) - PS110 + PS136 + PS31; +const float PS138 = P(0,13)*PS12 - P(1,13)*PS13 - P(10,13)*PS9 - P(11,13)*PS34 + P(12,13)*PS6 + P(2,13) + P(3,13)*PS11; +const float PS139 = P(0,15)*PS12 - P(1,15)*PS13 - P(10,15)*PS9 - P(11,15)*PS34 + P(12,15)*PS6 + P(2,15) + P(3,15)*PS11; +const float PS140 = 2*PS139; +const float PS141 = 2*PS128; +const float PS142 = P(0,14)*PS12 - P(1,14)*PS13 - P(10,14)*PS9 - P(11,14)*PS34 + P(12,14)*PS6 + P(2,14) + P(3,14)*PS11; +const float PS143 = 2*PS142; +const float PS144 = 2*PS135; +const float PS145 = 2*PS130; +const float PS146 = 2*PS137; +const float PS147 = P(0,4)*PS12 - P(1,4)*PS13 + P(2,4) + P(3,4)*PS11 - P(4,10)*PS9 - P(4,11)*PS34 + P(4,12)*PS6; +const float PS148 = 2*PS138; +const float PS149 = P(0,5)*PS12 - P(1,5)*PS13 + P(2,5) + P(3,5)*PS11 - P(5,10)*PS9 - P(5,11)*PS34 + P(5,12)*PS6; +const float PS150 = P(0,6)*PS12 - P(1,6)*PS13 + P(2,6) + P(3,6)*PS11 - P(6,10)*PS9 - P(6,11)*PS34 + P(6,12)*PS6; +const float PS151 = P(0,10)*PS13 + P(1,10)*PS12 + P(10,10)*PS7 - P(2,10)*PS11 + P(3,10) + PS106 - PS15; +const float PS152 = P(1,1)*PS12 + P(1,10)*PS7 - P(1,11)*PS6 - P(1,12)*PS34 + P(1,3) + PS129 + PS24; +const float PS153 = P(0,0)*PS13 + P(0,10)*PS7 - P(0,11)*PS6 - P(0,12)*PS34 + P(0,3) - PS101 + PS134; +const float PS154 = P(0,12)*PS13 + P(1,12)*PS12 - P(12,12)*PS34 - P(2,12)*PS11 + P(3,12) + PS104 - PS131; +const float PS155 = P(0,11)*PS13 + P(1,11)*PS12 - P(11,11)*PS6 - P(2,11)*PS11 + P(3,11) + PS126 + PS8; +const float PS156 = P(2,10)*PS7 - P(2,11)*PS6 - P(2,12)*PS34 - P(2,2)*PS11 + P(2,3) + PS21 + PS98; +const float PS157 = P(3,10)*PS7 - P(3,11)*PS6 - P(3,12)*PS34 + P(3,3) + PS111 - PS136 + PS32; +const float PS158 = P(0,13)*PS13 + P(1,13)*PS12 + P(10,13)*PS7 - P(11,13)*PS6 - P(12,13)*PS34 - P(2,13)*PS11 + P(3,13); +const float PS159 = P(0,15)*PS13 + P(1,15)*PS12 + P(10,15)*PS7 - P(11,15)*PS6 - P(12,15)*PS34 - P(2,15)*PS11 + P(3,15); +const float PS160 = 2*PS159; +const float PS161 = 2*PS157; +const float PS162 = P(0,14)*PS13 + P(1,14)*PS12 + P(10,14)*PS7 - P(11,14)*PS6 - P(12,14)*PS34 - P(2,14)*PS11 + P(3,14); +const float PS163 = 2*PS162; +const float PS164 = 2*PS152; +const float PS165 = 2*PS153; +const float PS166 = 2*PS156; +const float PS167 = P(0,4)*PS13 + P(1,4)*PS12 - P(2,4)*PS11 + P(3,4) + P(4,10)*PS7 - P(4,11)*PS6 - P(4,12)*PS34; +const float PS168 = 2*PS158; +const float PS169 = P(0,5)*PS13 + P(1,5)*PS12 - P(2,5)*PS11 + P(3,5) + P(5,10)*PS7 - P(5,11)*PS6 - P(5,12)*PS34; +const float PS170 = P(0,6)*PS13 + P(1,6)*PS12 - P(2,6)*PS11 + P(3,6) + P(6,10)*PS7 - P(6,11)*PS6 - P(6,12)*PS34; +const float PS171 = 2*PS45; +const float PS172 = 2*PS56; +const float PS173 = 2*PS61; +const float PS174 = 2*PS66; +const float PS175 = 2*PS71; +const float PS176 = 2*PS54; +const float PS177 = P(0,13)*PS174 + P(1,13)*PS173 + P(13,13)*PS43 + P(13,14)*PS172 - P(13,15)*PS171 + P(2,13)*PS175 - P(3,13)*PS176 + P(4,13); +const float PS178 = P(0,15)*PS174 + P(1,15)*PS173 + P(13,15)*PS43 + P(14,15)*PS172 - P(15,15)*PS171 + P(2,15)*PS175 - P(3,15)*PS176 + P(4,15); +const float PS179 = P(0,3)*PS174 + P(1,3)*PS173 + P(2,3)*PS175 + P(3,13)*PS43 + P(3,14)*PS172 - P(3,15)*PS171 - P(3,3)*PS176 + P(3,4); +const float PS180 = P(0,14)*PS174 + P(1,14)*PS173 + P(13,14)*PS43 + P(14,14)*PS172 - P(14,15)*PS171 + P(2,14)*PS175 - P(3,14)*PS176 + P(4,14); +const float PS181 = P(0,1)*PS174 + P(1,1)*PS173 + P(1,13)*PS43 + P(1,14)*PS172 - P(1,15)*PS171 + P(1,2)*PS175 - P(1,3)*PS176 + P(1,4); +const float PS182 = P(0,0)*PS174 + P(0,1)*PS173 + P(0,13)*PS43 + P(0,14)*PS172 - P(0,15)*PS171 + P(0,2)*PS175 - P(0,3)*PS176 + P(0,4); +const float PS183 = P(0,2)*PS174 + P(1,2)*PS173 + P(2,13)*PS43 + P(2,14)*PS172 - P(2,15)*PS171 + P(2,2)*PS175 - P(2,3)*PS176 + P(2,4); +const float PS184 = 4*dvyVar; +const float PS185 = 4*dvzVar; +const float PS186 = P(0,4)*PS174 + P(1,4)*PS173 + P(2,4)*PS175 - P(3,4)*PS176 + P(4,13)*PS43 + P(4,14)*PS172 - P(4,15)*PS171 + P(4,4); +const float PS187 = 2*PS177; +const float PS188 = 2*PS182; +const float PS189 = 2*PS181; +const float PS190 = 2*PS81; +const float PS191 = 2*PS183; +const float PS192 = 2*PS179; +const float PS193 = 2*PS76; +const float PS194 = PS43*dvxVar; +const float PS195 = PS75*dvyVar; +const float PS196 = P(0,5)*PS174 + P(1,5)*PS173 + P(2,5)*PS175 - P(3,5)*PS176 + P(4,5) + P(5,13)*PS43 + P(5,14)*PS172 - P(5,15)*PS171; +const float PS197 = 2*PS88; +const float PS198 = PS87*dvzVar; +const float PS199 = 2*PS90; +const float PS200 = P(0,6)*PS174 + P(1,6)*PS173 + P(2,6)*PS175 - P(3,6)*PS176 + P(4,6) + P(6,13)*PS43 + P(6,14)*PS172 - P(6,15)*PS171; +const float PS201 = 2*PS83; +const float PS202 = 2*PS78; +const float PS203 = 2*PS85; +const float PS204 = 2*PS80; +const float PS205 = -P(0,14)*PS202 - P(1,14)*PS204 - P(13,14)*PS193 + P(14,14)*PS75 + P(14,15)*PS190 + P(2,14)*PS201 + P(3,14)*PS203 + P(5,14); +const float PS206 = -P(0,13)*PS202 - P(1,13)*PS204 - P(13,13)*PS193 + P(13,14)*PS75 + P(13,15)*PS190 + P(2,13)*PS201 + P(3,13)*PS203 + P(5,13); +const float PS207 = -P(0,0)*PS202 - P(0,1)*PS204 - P(0,13)*PS193 + P(0,14)*PS75 + P(0,15)*PS190 + P(0,2)*PS201 + P(0,3)*PS203 + P(0,5); +const float PS208 = -P(0,1)*PS202 - P(1,1)*PS204 - P(1,13)*PS193 + P(1,14)*PS75 + P(1,15)*PS190 + P(1,2)*PS201 + P(1,3)*PS203 + P(1,5); +const float PS209 = -P(0,15)*PS202 - P(1,15)*PS204 - P(13,15)*PS193 + P(14,15)*PS75 + P(15,15)*PS190 + P(2,15)*PS201 + P(3,15)*PS203 + P(5,15); +const float PS210 = -P(0,2)*PS202 - P(1,2)*PS204 - P(2,13)*PS193 + P(2,14)*PS75 + P(2,15)*PS190 + P(2,2)*PS201 + P(2,3)*PS203 + P(2,5); +const float PS211 = -P(0,3)*PS202 - P(1,3)*PS204 + P(2,3)*PS201 - P(3,13)*PS193 + P(3,14)*PS75 + P(3,15)*PS190 + P(3,3)*PS203 + P(3,5); +const float PS212 = 4*dvxVar; +const float PS213 = -P(0,5)*PS202 - P(1,5)*PS204 + P(2,5)*PS201 + P(3,5)*PS203 - P(5,13)*PS193 + P(5,14)*PS75 + P(5,15)*PS190 + P(5,5); +const float PS214 = 2*PS89; +const float PS215 = 2*PS91; +const float PS216 = 2*PS92; +const float PS217 = 2*PS93; +const float PS218 = -P(0,6)*PS202 - P(1,6)*PS204 + P(2,6)*PS201 + P(3,6)*PS203 + P(5,6) - P(6,13)*PS193 + P(6,14)*PS75 + P(6,15)*PS190; +const float PS219 = P(0,15)*PS216 + P(1,15)*PS217 + P(13,15)*PS199 - P(14,15)*PS197 + P(15,15)*PS87 - P(2,15)*PS214 + P(3,15)*PS215 + P(6,15); +const float PS220 = P(0,14)*PS216 + P(1,14)*PS217 + P(13,14)*PS199 - P(14,14)*PS197 + P(14,15)*PS87 - P(2,14)*PS214 + P(3,14)*PS215 + P(6,14); +const float PS221 = P(0,13)*PS216 + P(1,13)*PS217 + P(13,13)*PS199 - P(13,14)*PS197 + P(13,15)*PS87 - P(2,13)*PS214 + P(3,13)*PS215 + P(6,13); +const float PS222 = P(0,6)*PS216 + P(1,6)*PS217 - P(2,6)*PS214 + P(3,6)*PS215 + P(6,13)*PS199 - P(6,14)*PS197 + P(6,15)*PS87 + P(6,6); nextP(0,0) = PS0*PS1 - PS11*PS23 - PS12*PS26 - PS13*PS29 + PS14*PS6 + PS17*PS7 + PS2*PS3 + PS20*PS9 + PS33 + PS4*PS5; nextP(0,1) = -PS1*PS36 + PS11*PS33 - PS12*PS29 + PS13*PS26 - PS14*PS34 + PS17*PS9 - PS20*PS7 + PS23 + PS3*PS35 - PS35*PS5; -nextP(1,1) = PS1*PS42 + PS11*PS80 - PS12*PS89 + PS13*PS82 + PS2*PS5 + PS3*PS4 - PS34*PS85 - PS7*PS87 + PS77*PS9 + PS92; +nextP(1,1) = PS1*PS95 + PS100*PS11 + PS102*PS13 - PS105*PS34 - PS107*PS7 - PS109*PS12 + PS112 + PS2*PS5 + PS3*PS4 + PS9*PS97; nextP(0,2) = -PS1*PS37 + PS11*PS29 + PS12*PS33 - PS13*PS23 - PS14*PS9 - PS17*PS34 + PS20*PS6 + PS26 - PS3*PS38 + PS37*PS5; -nextP(1,2) = PS1*PS40 + PS11*PS89 + PS12*PS80 - PS13*PS92 - PS3*PS40 - PS34*PS77 - PS39*PS5 + PS6*PS87 + PS82 - PS85*PS9; -nextP(2,2) = PS0*PS5 + PS1*PS4 + PS103*PS6 + PS104*PS11 + PS106*PS12 - PS108*PS34 - PS109*PS9 - PS111*PS13 + PS113 + PS3*PS42; +nextP(1,2) = PS1*PS40 + PS100*PS12 + PS102 - PS105*PS9 + PS107*PS6 + PS109*PS11 - PS112*PS13 - PS3*PS40 - PS34*PS97 - PS39*PS5; +nextP(2,2) = PS0*PS5 + PS1*PS4 + PS11*PS128 + PS12*PS130 + PS127*PS6 - PS13*PS135 - PS132*PS34 - PS133*PS9 + PS137 + PS3*PS95; nextP(0,3) = PS1*PS39 - PS11*PS26 + PS12*PS23 + PS13*PS33 + PS14*PS7 - PS17*PS6 - PS20*PS34 + PS29 - PS3*PS39 - PS40*PS5; -nextP(1,3) = -PS1*PS38 - PS11*PS82 + PS12*PS92 + PS13*PS80 - PS3*PS37 - PS34*PS87 + PS38*PS5 - PS6*PS77 + PS7*PS85 + PS89; -nextP(2,3) = -PS1*PS35 - PS103*PS34 + PS104 + PS106*PS13 - PS108*PS6 + PS109*PS7 - PS11*PS113 + PS111*PS12 + PS3*PS36 - PS36*PS5; -nextP(3,3) = PS0*PS3 + PS1*PS2 - PS11*PS128 + PS12*PS124 + PS123*PS7 + PS125*PS13 - PS126*PS34 - PS127*PS6 + PS129 + PS42*PS5; -nextP(0,4) = PS23*PS62 + PS26*PS60 - PS44*PS45 - PS46*PS48 - PS52*PS53 + PS54*PS56 + PS57*PS58 + PS63; -nextP(1,4) = -PS44*PS93 - PS46*PS95 + PS54*PS97 + PS60*PS82 + PS62*PS92 + PS72*PS80 - PS74*PS89 + PS98; -nextP(2,4) = -PS104*PS74 + PS106*PS72 + PS111*PS62 + PS113*PS60 - PS114*PS44 - PS116*PS46 + PS118*PS54 + PS119; -nextP(3,4) = PS124*PS62 + PS125*PS72 + PS128*PS60 - PS129*PS74 - PS130*PS44 - PS132*PS46 + PS134*PS54 + PS135; -nextP(4,4) = -PS139*PS142 + PS140*PS146 - PS141*PS44 - PS145*PS74 + PS149*PS72 + PS152*PS60 + PS155*PS62 + PS156*powf(PS54, 2) + PS157*powf(PS46, 2) + PS158 + powf(PS44, 2)*dvxVar; -nextP(0,5) = -PS23*PS60 + PS26*PS62 + PS48*PS68 + PS52*PS58 + PS53*PS57 - PS55*PS65 - PS66*PS67 + PS69; -nextP(1,5) = PS100 - PS60*PS92 + PS62*PS82 - PS65*PS96 - PS66*PS99 + PS68*PS95 + PS72*PS89 + PS74*PS80; -nextP(2,5) = PS104*PS72 + PS106*PS74 - PS111*PS60 + PS113*PS62 + PS116*PS68 - PS117*PS65 - PS120*PS66 + PS121; -nextP(3,5) = -PS124*PS60 + PS125*PS74 + PS128*PS62 + PS129*PS72 + PS132*PS68 - PS133*PS65 - PS136*PS66 + PS137; -nextP(4,5) = -PS140*PS161 + PS142*PS160 + PS145*PS72 - PS146*PS65 + PS149*PS74 + PS152*PS62 - PS155*PS60 - PS157*PS46*PS68 - PS159*PS66 + PS162*PS163 + PS164; -nextP(5,5) = PS157*powf(PS68, 2) + PS160*PS173 - PS162*PS170 - PS169*PS65 - PS172*PS60 + PS175*PS72 + PS177*PS74 + PS179*PS62 + PS180*powf(PS66, 2) + PS181 + powf(PS65, 2)*dvyVar; -nextP(0,6) = PS23*PS74 - PS26*PS72 - PS47*PS70 + PS53*PS61 - PS56*PS71 + PS58*PS59 + PS67*PS73 + PS75; -nextP(1,6) = PS101 + PS60*PS80 + PS62*PS89 - PS70*PS94 - PS71*PS97 - PS72*PS82 + PS73*PS99 + PS74*PS92; -nextP(2,6) = PS104*PS62 + PS106*PS60 + PS111*PS74 - PS113*PS72 - PS115*PS70 - PS118*PS71 + PS120*PS73 + PS122; -nextP(3,6) = PS124*PS74 + PS125*PS60 - PS128*PS72 + PS129*PS62 - PS131*PS70 - PS134*PS71 + PS136*PS73 + PS138; -nextP(4,6) = PS139*PS167 - PS142*PS70 + PS145*PS62 - PS146*PS165 + PS149*PS60 - PS152*PS72 + PS155*PS74 - PS156*PS54*PS71 + PS159*PS73 - PS163*PS166 + PS168; -nextP(5,6) = -PS160*PS167 + PS161*PS165 - PS165*PS169 + PS166*PS170 + PS172*PS74 - PS173*PS70 + PS175*PS62 + PS177*PS60 - PS179*PS72 - PS180*PS66*PS73 + PS182; -nextP(6,6) = PS156*powf(PS71, 2) - PS165*PS184 + PS166*PS185 + PS180*powf(PS73, 2) - PS183*PS70 + PS186 + PS60*(P(0,0)*PS60 + P(0,13)*PS166 - P(0,14)*PS165 - P(0,15)*PS70 + P(0,3)*PS62 + P(0,6) - PS151 + PS171) + PS62*(P(0,3)*PS60 + P(3,13)*PS166 - P(3,14)*PS165 - P(3,15)*PS70 + P(3,3)*PS62 + P(3,6) + PS154 - PS178) + powf(PS70, 2)*dvzVar - PS72*(P(1,2)*PS74 + P(2,13)*PS166 - P(2,14)*PS165 - P(2,15)*PS70 - P(2,2)*PS72 + P(2,6) + PS147 + PS174) + PS74*(P(1,1)*PS74 + P(1,13)*PS166 - P(1,14)*PS165 - P(1,15)*PS70 - P(1,2)*PS72 + P(1,6) + PS143 + PS176); -nextP(0,7) = P(0,7) - P(1,7)*PS11 - P(2,7)*PS12 - P(3,7)*PS13 + P(7,10)*PS6 + P(7,11)*PS7 + P(7,12)*PS9 + PS63*dt; -nextP(1,7) = P(0,7)*PS11 + P(1,7) + P(2,7)*PS13 - P(3,7)*PS12 - P(7,10)*PS34 + P(7,11)*PS9 - P(7,12)*PS7 + PS98*dt; -nextP(2,7) = P(0,7)*PS12 - P(1,7)*PS13 + P(2,7) + P(3,7)*PS11 - P(7,10)*PS9 - P(7,11)*PS34 + P(7,12)*PS6 + PS119*dt; -nextP(3,7) = P(0,7)*PS13 + P(1,7)*PS12 - P(2,7)*PS11 + P(3,7) + P(7,10)*PS7 - P(7,11)*PS6 - P(7,12)*PS34 + PS135*dt; -nextP(4,7) = P(0,7)*PS72 + P(1,7)*PS62 + P(2,7)*PS60 - P(3,7)*PS74 + P(4,7) - P(7,13)*PS44 + P(7,14)*PS140 - P(7,15)*PS139 + PS158*dt; -nextP(5,7) = P(0,7)*PS74 - P(1,7)*PS60 + P(2,7)*PS62 + P(3,7)*PS72 + P(5,7) - P(7,13)*PS162 - P(7,14)*PS65 + P(7,15)*PS160 + dt*(P(0,4)*PS74 - P(1,4)*PS60 + P(2,4)*PS62 + P(3,4)*PS72 - P(4,13)*PS162 - P(4,14)*PS65 + P(4,15)*PS160 + P(4,5)); -nextP(6,7) = P(0,7)*PS60 + P(1,7)*PS74 - P(2,7)*PS72 + P(3,7)*PS62 + P(6,7) + P(7,13)*PS166 - P(7,14)*PS165 - P(7,15)*PS70 + dt*(P(0,4)*PS60 + P(1,4)*PS74 - P(2,4)*PS72 + P(3,4)*PS62 + P(4,13)*PS166 - P(4,14)*PS165 - P(4,15)*PS70 + P(4,6)); +nextP(1,3) = -PS1*PS38 + PS100*PS13 - PS102*PS11 + PS105*PS7 - PS107*PS34 + PS109 + PS112*PS12 - PS3*PS37 + PS38*PS5 - PS6*PS97; +nextP(2,3) = -PS1*PS35 - PS11*PS137 + PS12*PS135 - PS127*PS34 + PS128 + PS13*PS130 - PS132*PS6 + PS133*PS7 + PS3*PS36 - PS36*PS5; +nextP(3,3) = PS0*PS3 + PS1*PS2 - PS11*PS156 + PS12*PS152 + PS13*PS153 + PS151*PS7 - PS154*PS34 - PS155*PS6 + PS157 + PS5*PS95; +nextP(0,4) = PS43*PS44 - PS45*PS47 - PS54*PS55 + PS56*PS58 + PS61*PS62 + PS66*PS67 + PS71*PS72 + PS73; +nextP(1,4) = PS113*PS43 - PS115*PS45 - PS116*PS54 + PS118*PS56 + PS119*PS61 + PS120*PS66 + PS121*PS71 + PS122; +nextP(2,4) = PS138*PS43 - PS140*PS45 - PS141*PS54 + PS143*PS56 + PS144*PS61 + PS145*PS66 + PS146*PS71 + PS147; +nextP(3,4) = PS158*PS43 - PS160*PS45 - PS161*PS54 + PS163*PS56 + PS164*PS61 + PS165*PS66 + PS166*PS71 + PS167; +nextP(4,4) = -PS171*PS178 + PS172*PS180 + PS173*PS181 + PS174*PS182 + PS175*PS183 - PS176*PS179 + PS177*PS43 + PS184*powf(PS56, 2) + PS185*powf(PS45, 2) + PS186 + powf(PS43, 2)*dvxVar; +nextP(0,5) = PS47*PS81 + PS55*PS85 + PS57*PS75 - PS62*PS80 - PS67*PS78 + PS72*PS83 - PS76*PS77 + PS86; +nextP(1,5) = PS115*PS81 + PS116*PS85 + PS117*PS75 - PS119*PS80 - PS120*PS78 + PS121*PS83 - PS123*PS76 + PS124; +nextP(2,5) = PS140*PS81 + PS141*PS85 + PS142*PS75 - PS144*PS80 - PS145*PS78 + PS146*PS83 - PS148*PS76 + PS149; +nextP(3,5) = PS160*PS81 + PS161*PS85 + PS162*PS75 - PS164*PS80 - PS165*PS78 + PS166*PS83 - PS168*PS76 + PS169; +nextP(4,5) = PS172*PS195 + PS178*PS190 + PS180*PS75 - PS185*PS45*PS81 - PS187*PS76 - PS188*PS78 - PS189*PS80 + PS191*PS83 + PS192*PS85 - PS193*PS194 + PS196; +nextP(5,5) = PS185*powf(PS81, 2) + PS190*PS209 - PS193*PS206 + PS201*PS210 - PS202*PS207 + PS203*PS211 - PS204*PS208 + PS205*PS75 + PS212*powf(PS76, 2) + PS213 + powf(PS75, 2)*dvyVar; +nextP(0,6) = PS46*PS87 + PS55*PS91 - PS58*PS88 + PS62*PS93 + PS67*PS92 - PS72*PS89 + PS77*PS90 + PS94; +nextP(1,6) = PS114*PS87 + PS116*PS91 - PS118*PS88 + PS119*PS93 + PS120*PS92 - PS121*PS89 + PS123*PS90 + PS125; +nextP(2,6) = PS139*PS87 + PS141*PS91 - PS143*PS88 + PS144*PS93 + PS145*PS92 - PS146*PS89 + PS148*PS90 + PS150; +nextP(3,6) = PS159*PS87 + PS161*PS91 - PS163*PS88 + PS164*PS93 + PS165*PS92 - PS166*PS89 + PS168*PS90 + PS170; +nextP(4,6) = -PS171*PS198 + PS178*PS87 - PS180*PS197 - PS184*PS56*PS88 + PS187*PS90 + PS188*PS92 + PS189*PS93 - PS191*PS89 + PS192*PS91 + PS194*PS199 + PS200; +nextP(5,6) = PS190*PS198 - PS195*PS197 - PS197*PS205 + PS199*PS206 + PS207*PS216 + PS208*PS217 + PS209*PS87 - PS210*PS214 + PS211*PS215 - PS212*PS76*PS90 + PS218; +nextP(6,6) = PS184*powf(PS88, 2) - PS197*PS220 + PS199*PS221 + PS212*powf(PS90, 2) - PS214*(P(0,2)*PS216 + P(1,2)*PS217 + P(2,13)*PS199 - P(2,14)*PS197 + P(2,15)*PS87 - P(2,2)*PS214 + P(2,3)*PS215 + P(2,6)) + PS215*(P(0,3)*PS216 + P(1,3)*PS217 - P(2,3)*PS214 + P(3,13)*PS199 - P(3,14)*PS197 + P(3,15)*PS87 + P(3,3)*PS215 + P(3,6)) + PS216*(P(0,0)*PS216 + P(0,1)*PS217 + P(0,13)*PS199 - P(0,14)*PS197 + P(0,15)*PS87 - P(0,2)*PS214 + P(0,3)*PS215 + P(0,6)) + PS217*(P(0,1)*PS216 + P(1,1)*PS217 + P(1,13)*PS199 - P(1,14)*PS197 + P(1,15)*PS87 - P(1,2)*PS214 + P(1,3)*PS215 + P(1,6)) + PS219*PS87 + PS222 + powf(PS87, 2)*dvzVar; +nextP(0,7) = P(0,7) - P(1,7)*PS11 - P(2,7)*PS12 - P(3,7)*PS13 + P(7,10)*PS6 + P(7,11)*PS7 + P(7,12)*PS9 + PS73*dt; +nextP(1,7) = P(0,7)*PS11 + P(1,7) + P(2,7)*PS13 - P(3,7)*PS12 - P(7,10)*PS34 + P(7,11)*PS9 - P(7,12)*PS7 + PS122*dt; +nextP(2,7) = P(0,7)*PS12 - P(1,7)*PS13 + P(2,7) + P(3,7)*PS11 - P(7,10)*PS9 - P(7,11)*PS34 + P(7,12)*PS6 + PS147*dt; +nextP(3,7) = P(0,7)*PS13 + P(1,7)*PS12 - P(2,7)*PS11 + P(3,7) + P(7,10)*PS7 - P(7,11)*PS6 - P(7,12)*PS34 + PS167*dt; +nextP(4,7) = P(0,7)*PS174 + P(1,7)*PS173 + P(2,7)*PS175 - P(3,7)*PS176 + P(4,7) + P(7,13)*PS43 + P(7,14)*PS172 - P(7,15)*PS171 + PS186*dt; +nextP(5,7) = -P(0,7)*PS202 - P(1,7)*PS204 + P(2,7)*PS201 + P(3,7)*PS203 + P(5,7) - P(7,13)*PS193 + P(7,14)*PS75 + P(7,15)*PS190 + dt*(-P(0,4)*PS202 - P(1,4)*PS204 + P(2,4)*PS201 + P(3,4)*PS203 - P(4,13)*PS193 + P(4,14)*PS75 + P(4,15)*PS190 + P(4,5)); +nextP(6,7) = P(0,7)*PS216 + P(1,7)*PS217 - P(2,7)*PS214 + P(3,7)*PS215 + P(6,7) + P(7,13)*PS199 - P(7,14)*PS197 + P(7,15)*PS87 + dt*(P(0,4)*PS216 + P(1,4)*PS217 - P(2,4)*PS214 + P(3,4)*PS215 + P(4,13)*PS199 - P(4,14)*PS197 + P(4,15)*PS87 + P(4,6)); nextP(7,7) = P(4,7)*dt + P(7,7) + dt*(P(4,4)*dt + P(4,7)); -nextP(0,8) = P(0,8) - P(1,8)*PS11 - P(2,8)*PS12 - P(3,8)*PS13 + P(8,10)*PS6 + P(8,11)*PS7 + P(8,12)*PS9 + PS69*dt; -nextP(1,8) = P(0,8)*PS11 + P(1,8) + P(2,8)*PS13 - P(3,8)*PS12 - P(8,10)*PS34 + P(8,11)*PS9 - P(8,12)*PS7 + PS100*dt; -nextP(2,8) = P(0,8)*PS12 - P(1,8)*PS13 + P(2,8) + P(3,8)*PS11 - P(8,10)*PS9 - P(8,11)*PS34 + P(8,12)*PS6 + PS121*dt; -nextP(3,8) = P(0,8)*PS13 + P(1,8)*PS12 - P(2,8)*PS11 + P(3,8) + P(8,10)*PS7 - P(8,11)*PS6 - P(8,12)*PS34 + PS137*dt; -nextP(4,8) = P(0,8)*PS72 + P(1,8)*PS62 + P(2,8)*PS60 - P(3,8)*PS74 + P(4,8) - P(8,13)*PS44 + P(8,14)*PS140 - P(8,15)*PS139 + PS164*dt; -nextP(5,8) = P(0,8)*PS74 - P(1,8)*PS60 + P(2,8)*PS62 + P(3,8)*PS72 + P(5,8) - P(8,13)*PS162 - P(8,14)*PS65 + P(8,15)*PS160 + PS181*dt; -nextP(6,8) = P(0,8)*PS60 + P(1,8)*PS74 - P(2,8)*PS72 + P(3,8)*PS62 + P(6,8) + P(8,13)*PS166 - P(8,14)*PS165 - P(8,15)*PS70 + dt*(P(0,5)*PS60 + P(1,5)*PS74 - P(2,5)*PS72 + P(3,5)*PS62 + P(5,13)*PS166 - P(5,14)*PS165 - P(5,15)*PS70 + P(5,6)); +nextP(0,8) = P(0,8) - P(1,8)*PS11 - P(2,8)*PS12 - P(3,8)*PS13 + P(8,10)*PS6 + P(8,11)*PS7 + P(8,12)*PS9 + PS86*dt; +nextP(1,8) = P(0,8)*PS11 + P(1,8) + P(2,8)*PS13 - P(3,8)*PS12 - P(8,10)*PS34 + P(8,11)*PS9 - P(8,12)*PS7 + PS124*dt; +nextP(2,8) = P(0,8)*PS12 - P(1,8)*PS13 + P(2,8) + P(3,8)*PS11 - P(8,10)*PS9 - P(8,11)*PS34 + P(8,12)*PS6 + PS149*dt; +nextP(3,8) = P(0,8)*PS13 + P(1,8)*PS12 - P(2,8)*PS11 + P(3,8) + P(8,10)*PS7 - P(8,11)*PS6 - P(8,12)*PS34 + PS169*dt; +nextP(4,8) = P(0,8)*PS174 + P(1,8)*PS173 + P(2,8)*PS175 - P(3,8)*PS176 + P(4,8) + P(8,13)*PS43 + P(8,14)*PS172 - P(8,15)*PS171 + PS196*dt; +nextP(5,8) = -P(0,8)*PS202 - P(1,8)*PS204 + P(2,8)*PS201 + P(3,8)*PS203 + P(5,8) - P(8,13)*PS193 + P(8,14)*PS75 + P(8,15)*PS190 + PS213*dt; +nextP(6,8) = P(0,8)*PS216 + P(1,8)*PS217 - P(2,8)*PS214 + P(3,8)*PS215 + P(6,8) + P(8,13)*PS199 - P(8,14)*PS197 + P(8,15)*PS87 + dt*(P(0,5)*PS216 + P(1,5)*PS217 - P(2,5)*PS214 + P(3,5)*PS215 + P(5,13)*PS199 - P(5,14)*PS197 + P(5,15)*PS87 + P(5,6)); nextP(7,8) = P(4,8)*dt + P(7,8) + dt*(P(4,5)*dt + P(5,7)); nextP(8,8) = P(5,8)*dt + P(8,8) + dt*(P(5,5)*dt + P(5,8)); -nextP(0,9) = P(0,9) - P(1,9)*PS11 - P(2,9)*PS12 - P(3,9)*PS13 + P(9,10)*PS6 + P(9,11)*PS7 + P(9,12)*PS9 + PS75*dt; -nextP(1,9) = P(0,9)*PS11 + P(1,9) + P(2,9)*PS13 - P(3,9)*PS12 - P(9,10)*PS34 + P(9,11)*PS9 - P(9,12)*PS7 + PS101*dt; -nextP(2,9) = P(0,9)*PS12 - P(1,9)*PS13 + P(2,9) + P(3,9)*PS11 - P(9,10)*PS9 - P(9,11)*PS34 + P(9,12)*PS6 + PS122*dt; -nextP(3,9) = P(0,9)*PS13 + P(1,9)*PS12 - P(2,9)*PS11 + P(3,9) + P(9,10)*PS7 - P(9,11)*PS6 - P(9,12)*PS34 + PS138*dt; -nextP(4,9) = P(0,9)*PS72 + P(1,9)*PS62 + P(2,9)*PS60 - P(3,9)*PS74 + P(4,9) - P(9,13)*PS44 + P(9,14)*PS140 - P(9,15)*PS139 + PS168*dt; -nextP(5,9) = P(0,9)*PS74 - P(1,9)*PS60 + P(2,9)*PS62 + P(3,9)*PS72 + P(5,9) - P(9,13)*PS162 - P(9,14)*PS65 + P(9,15)*PS160 + PS182*dt; -nextP(6,9) = P(0,9)*PS60 + P(1,9)*PS74 - P(2,9)*PS72 + P(3,9)*PS62 + P(6,9) + P(9,13)*PS166 - P(9,14)*PS165 - P(9,15)*PS70 + PS186*dt; +nextP(0,9) = P(0,9) - P(1,9)*PS11 - P(2,9)*PS12 - P(3,9)*PS13 + P(9,10)*PS6 + P(9,11)*PS7 + P(9,12)*PS9 + PS94*dt; +nextP(1,9) = P(0,9)*PS11 + P(1,9) + P(2,9)*PS13 - P(3,9)*PS12 - P(9,10)*PS34 + P(9,11)*PS9 - P(9,12)*PS7 + PS125*dt; +nextP(2,9) = P(0,9)*PS12 - P(1,9)*PS13 + P(2,9) + P(3,9)*PS11 - P(9,10)*PS9 - P(9,11)*PS34 + P(9,12)*PS6 + PS150*dt; +nextP(3,9) = P(0,9)*PS13 + P(1,9)*PS12 - P(2,9)*PS11 + P(3,9) + P(9,10)*PS7 - P(9,11)*PS6 - P(9,12)*PS34 + PS170*dt; +nextP(4,9) = P(0,9)*PS174 + P(1,9)*PS173 + P(2,9)*PS175 - P(3,9)*PS176 + P(4,9) + P(9,13)*PS43 + P(9,14)*PS172 - P(9,15)*PS171 + PS200*dt; +nextP(5,9) = -P(0,9)*PS202 - P(1,9)*PS204 + P(2,9)*PS201 + P(3,9)*PS203 + P(5,9) - P(9,13)*PS193 + P(9,14)*PS75 + P(9,15)*PS190 + PS218*dt; +nextP(6,9) = P(0,9)*PS216 + P(1,9)*PS217 - P(2,9)*PS214 + P(3,9)*PS215 + P(6,9) + P(9,13)*PS199 - P(9,14)*PS197 + P(9,15)*PS87 + PS222*dt; nextP(7,9) = P(4,9)*dt + P(7,9) + dt*(P(4,6)*dt + P(6,7)); nextP(8,9) = P(5,9)*dt + P(8,9) + dt*(P(5,6)*dt + P(6,8)); nextP(9,9) = P(6,9)*dt + P(9,9) + dt*(P(6,6)*dt + P(6,9)); nextP(0,10) = PS14; -nextP(1,10) = PS85; -nextP(2,10) = PS109; -nextP(3,10) = PS123; -nextP(4,10) = P(0,10)*PS72 + P(1,10)*PS62 - P(10,13)*PS44 + P(10,14)*PS140 - P(10,15)*PS139 + P(2,10)*PS60 - P(3,10)*PS74 + P(4,10); -nextP(5,10) = P(0,10)*PS74 - P(1,10)*PS60 - P(10,13)*PS162 - P(10,14)*PS65 + P(10,15)*PS160 + P(2,10)*PS62 + P(3,10)*PS72 + P(5,10); -nextP(6,10) = P(0,10)*PS60 + P(1,10)*PS74 + P(10,13)*PS166 - P(10,14)*PS165 - P(10,15)*PS70 - P(2,10)*PS72 + P(3,10)*PS62 + P(6,10); +nextP(1,10) = PS105; +nextP(2,10) = PS133; +nextP(3,10) = PS151; +nextP(4,10) = P(0,10)*PS174 + P(1,10)*PS173 + P(10,13)*PS43 + P(10,14)*PS172 - P(10,15)*PS171 + P(2,10)*PS175 - P(3,10)*PS176 + P(4,10); +nextP(5,10) = -P(0,10)*PS202 - P(1,10)*PS204 - P(10,13)*PS193 + P(10,14)*PS75 + P(10,15)*PS190 + P(2,10)*PS201 + P(3,10)*PS203 + P(5,10); +nextP(6,10) = P(0,10)*PS216 + P(1,10)*PS217 + P(10,13)*PS199 - P(10,14)*PS197 + P(10,15)*PS87 - P(2,10)*PS214 + P(3,10)*PS215 + P(6,10); nextP(7,10) = P(4,10)*dt + P(7,10); nextP(8,10) = P(5,10)*dt + P(8,10); nextP(9,10) = P(6,10)*dt + P(9,10); nextP(10,10) = P(10,10); nextP(0,11) = PS17; -nextP(1,11) = PS77; -nextP(2,11) = PS108; -nextP(3,11) = PS127; -nextP(4,11) = P(0,11)*PS72 + P(1,11)*PS62 - P(11,13)*PS44 + P(11,14)*PS140 - P(11,15)*PS139 + P(2,11)*PS60 - P(3,11)*PS74 + P(4,11); -nextP(5,11) = P(0,11)*PS74 - P(1,11)*PS60 - P(11,13)*PS162 - P(11,14)*PS65 + P(11,15)*PS160 + P(2,11)*PS62 + P(3,11)*PS72 + P(5,11); -nextP(6,11) = P(0,11)*PS60 + P(1,11)*PS74 + P(11,13)*PS166 - P(11,14)*PS165 - P(11,15)*PS70 - P(2,11)*PS72 + P(3,11)*PS62 + P(6,11); +nextP(1,11) = PS97; +nextP(2,11) = PS132; +nextP(3,11) = PS155; +nextP(4,11) = P(0,11)*PS174 + P(1,11)*PS173 + P(11,13)*PS43 + P(11,14)*PS172 - P(11,15)*PS171 + P(2,11)*PS175 - P(3,11)*PS176 + P(4,11); +nextP(5,11) = -P(0,11)*PS202 - P(1,11)*PS204 - P(11,13)*PS193 + P(11,14)*PS75 + P(11,15)*PS190 + P(2,11)*PS201 + P(3,11)*PS203 + P(5,11); +nextP(6,11) = P(0,11)*PS216 + P(1,11)*PS217 + P(11,13)*PS199 - P(11,14)*PS197 + P(11,15)*PS87 - P(2,11)*PS214 + P(3,11)*PS215 + P(6,11); nextP(7,11) = P(4,11)*dt + P(7,11); nextP(8,11) = P(5,11)*dt + P(8,11); nextP(9,11) = P(6,11)*dt + P(9,11); nextP(10,11) = P(10,11); nextP(11,11) = P(11,11); nextP(0,12) = PS20; -nextP(1,12) = PS87; -nextP(2,12) = PS103; -nextP(3,12) = PS126; -nextP(4,12) = P(0,12)*PS72 + P(1,12)*PS62 - P(12,13)*PS44 + P(12,14)*PS140 - P(12,15)*PS139 + P(2,12)*PS60 - P(3,12)*PS74 + P(4,12); -nextP(5,12) = P(0,12)*PS74 - P(1,12)*PS60 - P(12,13)*PS162 - P(12,14)*PS65 + P(12,15)*PS160 + P(2,12)*PS62 + P(3,12)*PS72 + P(5,12); -nextP(6,12) = P(0,12)*PS60 + P(1,12)*PS74 + P(12,13)*PS166 - P(12,14)*PS165 - P(12,15)*PS70 - P(2,12)*PS72 + P(3,12)*PS62 + P(6,12); +nextP(1,12) = PS107; +nextP(2,12) = PS127; +nextP(3,12) = PS154; +nextP(4,12) = P(0,12)*PS174 + P(1,12)*PS173 + P(12,13)*PS43 + P(12,14)*PS172 - P(12,15)*PS171 + P(2,12)*PS175 - P(3,12)*PS176 + P(4,12); +nextP(5,12) = -P(0,12)*PS202 - P(1,12)*PS204 - P(12,13)*PS193 + P(12,14)*PS75 + P(12,15)*PS190 + P(2,12)*PS201 + P(3,12)*PS203 + P(5,12); +nextP(6,12) = P(0,12)*PS216 + P(1,12)*PS217 + P(12,13)*PS199 - P(12,14)*PS197 + P(12,15)*PS87 - P(2,12)*PS214 + P(3,12)*PS215 + P(6,12); nextP(7,12) = P(4,12)*dt + P(7,12); nextP(8,12) = P(5,12)*dt + P(8,12); nextP(9,12) = P(6,12)*dt + P(9,12); nextP(10,12) = P(10,12); nextP(11,12) = P(11,12); nextP(12,12) = P(12,12); -nextP(0,13) = PS45; -nextP(1,13) = PS93; -nextP(2,13) = PS114; -nextP(3,13) = PS130; -nextP(4,13) = PS141; -nextP(5,13) = PS170; -nextP(6,13) = PS185; +nextP(0,13) = PS44; +nextP(1,13) = PS113; +nextP(2,13) = PS138; +nextP(3,13) = PS158; +nextP(4,13) = PS177; +nextP(5,13) = PS206; +nextP(6,13) = PS221; nextP(7,13) = P(4,13)*dt + P(7,13); nextP(8,13) = P(5,13)*dt + P(8,13); nextP(9,13) = P(6,13)*dt + P(9,13); @@ -293,13 +329,13 @@ nextP(10,13) = P(10,13); nextP(11,13) = P(11,13); nextP(12,13) = P(12,13); nextP(13,13) = P(13,13); -nextP(0,14) = PS55; -nextP(1,14) = PS96; -nextP(2,14) = PS117; -nextP(3,14) = PS133; -nextP(4,14) = PS146; -nextP(5,14) = PS169; -nextP(6,14) = PS184; +nextP(0,14) = PS57; +nextP(1,14) = PS117; +nextP(2,14) = PS142; +nextP(3,14) = PS162; +nextP(4,14) = PS180; +nextP(5,14) = PS205; +nextP(6,14) = PS220; nextP(7,14) = P(4,14)*dt + P(7,14); nextP(8,14) = P(5,14)*dt + P(8,14); nextP(9,14) = P(6,14)*dt + P(9,14); @@ -308,13 +344,13 @@ nextP(11,14) = P(11,14); nextP(12,14) = P(12,14); nextP(13,14) = P(13,14); nextP(14,14) = P(14,14); -nextP(0,15) = PS47; -nextP(1,15) = PS94; -nextP(2,15) = PS115; -nextP(3,15) = PS131; -nextP(4,15) = PS142; -nextP(5,15) = PS173; -nextP(6,15) = PS183; +nextP(0,15) = PS46; +nextP(1,15) = PS114; +nextP(2,15) = PS139; +nextP(3,15) = PS159; +nextP(4,15) = PS178; +nextP(5,15) = PS209; +nextP(6,15) = PS219; nextP(7,15) = P(4,15)*dt + P(7,15); nextP(8,15) = P(5,15)*dt + P(8,15); nextP(9,15) = P(6,15)*dt + P(9,15); @@ -328,9 +364,9 @@ nextP(0,16) = P(0,16) - P(1,16)*PS11 + P(10,16)*PS6 + P(11,16)*PS7 + P(12,16)*PS nextP(1,16) = P(0,16)*PS11 + P(1,16) - P(10,16)*PS34 + P(11,16)*PS9 - P(12,16)*PS7 + P(2,16)*PS13 - P(3,16)*PS12; nextP(2,16) = P(0,16)*PS12 - P(1,16)*PS13 - P(10,16)*PS9 - P(11,16)*PS34 + P(12,16)*PS6 + P(2,16) + P(3,16)*PS11; nextP(3,16) = P(0,16)*PS13 + P(1,16)*PS12 + P(10,16)*PS7 - P(11,16)*PS6 - P(12,16)*PS34 - P(2,16)*PS11 + P(3,16); -nextP(4,16) = P(0,16)*PS72 + P(1,16)*PS62 - P(13,16)*PS44 + P(14,16)*PS140 - P(15,16)*PS139 + P(2,16)*PS60 - P(3,16)*PS74 + P(4,16); -nextP(5,16) = P(0,16)*PS74 - P(1,16)*PS60 - P(13,16)*PS162 - P(14,16)*PS65 + P(15,16)*PS160 + P(2,16)*PS62 + P(3,16)*PS72 + P(5,16); -nextP(6,16) = P(0,16)*PS60 + P(1,16)*PS74 + P(13,16)*PS166 - P(14,16)*PS165 - P(15,16)*PS70 - P(2,16)*PS72 + P(3,16)*PS62 + P(6,16); +nextP(4,16) = P(0,16)*PS174 + P(1,16)*PS173 + P(13,16)*PS43 + P(14,16)*PS172 - P(15,16)*PS171 + P(2,16)*PS175 - P(3,16)*PS176 + P(4,16); +nextP(5,16) = -P(0,16)*PS202 - P(1,16)*PS204 - P(13,16)*PS193 + P(14,16)*PS75 + P(15,16)*PS190 + P(2,16)*PS201 + P(3,16)*PS203 + P(5,16); +nextP(6,16) = P(0,16)*PS216 + P(1,16)*PS217 + P(13,16)*PS199 - P(14,16)*PS197 + P(15,16)*PS87 - P(2,16)*PS214 + P(3,16)*PS215 + P(6,16); nextP(7,16) = P(4,16)*dt + P(7,16); nextP(8,16) = P(5,16)*dt + P(8,16); nextP(9,16) = P(6,16)*dt + P(9,16); @@ -345,9 +381,9 @@ nextP(0,17) = P(0,17) - P(1,17)*PS11 + P(10,17)*PS6 + P(11,17)*PS7 + P(12,17)*PS nextP(1,17) = P(0,17)*PS11 + P(1,17) - P(10,17)*PS34 + P(11,17)*PS9 - P(12,17)*PS7 + P(2,17)*PS13 - P(3,17)*PS12; nextP(2,17) = P(0,17)*PS12 - P(1,17)*PS13 - P(10,17)*PS9 - P(11,17)*PS34 + P(12,17)*PS6 + P(2,17) + P(3,17)*PS11; nextP(3,17) = P(0,17)*PS13 + P(1,17)*PS12 + P(10,17)*PS7 - P(11,17)*PS6 - P(12,17)*PS34 - P(2,17)*PS11 + P(3,17); -nextP(4,17) = P(0,17)*PS72 + P(1,17)*PS62 - P(13,17)*PS44 + P(14,17)*PS140 - P(15,17)*PS139 + P(2,17)*PS60 - P(3,17)*PS74 + P(4,17); -nextP(5,17) = P(0,17)*PS74 - P(1,17)*PS60 - P(13,17)*PS162 - P(14,17)*PS65 + P(15,17)*PS160 + P(2,17)*PS62 + P(3,17)*PS72 + P(5,17); -nextP(6,17) = P(0,17)*PS60 + P(1,17)*PS74 + P(13,17)*PS166 - P(14,17)*PS165 - P(15,17)*PS70 - P(2,17)*PS72 + P(3,17)*PS62 + P(6,17); +nextP(4,17) = P(0,17)*PS174 + P(1,17)*PS173 + P(13,17)*PS43 + P(14,17)*PS172 - P(15,17)*PS171 + P(2,17)*PS175 - P(3,17)*PS176 + P(4,17); +nextP(5,17) = -P(0,17)*PS202 - P(1,17)*PS204 - P(13,17)*PS193 + P(14,17)*PS75 + P(15,17)*PS190 + P(2,17)*PS201 + P(3,17)*PS203 + P(5,17); +nextP(6,17) = P(0,17)*PS216 + P(1,17)*PS217 + P(13,17)*PS199 - P(14,17)*PS197 + P(15,17)*PS87 - P(2,17)*PS214 + P(3,17)*PS215 + P(6,17); nextP(7,17) = P(4,17)*dt + P(7,17); nextP(8,17) = P(5,17)*dt + P(8,17); nextP(9,17) = P(6,17)*dt + P(9,17); @@ -363,9 +399,9 @@ nextP(0,18) = P(0,18) - P(1,18)*PS11 + P(10,18)*PS6 + P(11,18)*PS7 + P(12,18)*PS nextP(1,18) = P(0,18)*PS11 + P(1,18) - P(10,18)*PS34 + P(11,18)*PS9 - P(12,18)*PS7 + P(2,18)*PS13 - P(3,18)*PS12; nextP(2,18) = P(0,18)*PS12 - P(1,18)*PS13 - P(10,18)*PS9 - P(11,18)*PS34 + P(12,18)*PS6 + P(2,18) + P(3,18)*PS11; nextP(3,18) = P(0,18)*PS13 + P(1,18)*PS12 + P(10,18)*PS7 - P(11,18)*PS6 - P(12,18)*PS34 - P(2,18)*PS11 + P(3,18); -nextP(4,18) = P(0,18)*PS72 + P(1,18)*PS62 - P(13,18)*PS44 + P(14,18)*PS140 - P(15,18)*PS139 + P(2,18)*PS60 - P(3,18)*PS74 + P(4,18); -nextP(5,18) = P(0,18)*PS74 - P(1,18)*PS60 - P(13,18)*PS162 - P(14,18)*PS65 + P(15,18)*PS160 + P(2,18)*PS62 + P(3,18)*PS72 + P(5,18); -nextP(6,18) = P(0,18)*PS60 + P(1,18)*PS74 + P(13,18)*PS166 - P(14,18)*PS165 - P(15,18)*PS70 - P(2,18)*PS72 + P(3,18)*PS62 + P(6,18); +nextP(4,18) = P(0,18)*PS174 + P(1,18)*PS173 + P(13,18)*PS43 + P(14,18)*PS172 - P(15,18)*PS171 + P(2,18)*PS175 - P(3,18)*PS176 + P(4,18); +nextP(5,18) = -P(0,18)*PS202 - P(1,18)*PS204 - P(13,18)*PS193 + P(14,18)*PS75 + P(15,18)*PS190 + P(2,18)*PS201 + P(3,18)*PS203 + P(5,18); +nextP(6,18) = P(0,18)*PS216 + P(1,18)*PS217 + P(13,18)*PS199 - P(14,18)*PS197 + P(15,18)*PS87 - P(2,18)*PS214 + P(3,18)*PS215 + P(6,18); nextP(7,18) = P(4,18)*dt + P(7,18); nextP(8,18) = P(5,18)*dt + P(8,18); nextP(9,18) = P(6,18)*dt + P(9,18); @@ -382,9 +418,9 @@ nextP(0,19) = P(0,19) - P(1,19)*PS11 + P(10,19)*PS6 + P(11,19)*PS7 + P(12,19)*PS nextP(1,19) = P(0,19)*PS11 + P(1,19) - P(10,19)*PS34 + P(11,19)*PS9 - P(12,19)*PS7 + P(2,19)*PS13 - P(3,19)*PS12; nextP(2,19) = P(0,19)*PS12 - P(1,19)*PS13 - P(10,19)*PS9 - P(11,19)*PS34 + P(12,19)*PS6 + P(2,19) + P(3,19)*PS11; nextP(3,19) = P(0,19)*PS13 + P(1,19)*PS12 + P(10,19)*PS7 - P(11,19)*PS6 - P(12,19)*PS34 - P(2,19)*PS11 + P(3,19); -nextP(4,19) = P(0,19)*PS72 + P(1,19)*PS62 - P(13,19)*PS44 + P(14,19)*PS140 - P(15,19)*PS139 + P(2,19)*PS60 - P(3,19)*PS74 + P(4,19); -nextP(5,19) = P(0,19)*PS74 - P(1,19)*PS60 - P(13,19)*PS162 - P(14,19)*PS65 + P(15,19)*PS160 + P(2,19)*PS62 + P(3,19)*PS72 + P(5,19); -nextP(6,19) = P(0,19)*PS60 + P(1,19)*PS74 + P(13,19)*PS166 - P(14,19)*PS165 - P(15,19)*PS70 - P(2,19)*PS72 + P(3,19)*PS62 + P(6,19); +nextP(4,19) = P(0,19)*PS174 + P(1,19)*PS173 + P(13,19)*PS43 + P(14,19)*PS172 - P(15,19)*PS171 + P(2,19)*PS175 - P(3,19)*PS176 + P(4,19); +nextP(5,19) = -P(0,19)*PS202 - P(1,19)*PS204 - P(13,19)*PS193 + P(14,19)*PS75 + P(15,19)*PS190 + P(2,19)*PS201 + P(3,19)*PS203 + P(5,19); +nextP(6,19) = P(0,19)*PS216 + P(1,19)*PS217 + P(13,19)*PS199 - P(14,19)*PS197 + P(15,19)*PS87 - P(2,19)*PS214 + P(3,19)*PS215 + P(6,19); nextP(7,19) = P(4,19)*dt + P(7,19); nextP(8,19) = P(5,19)*dt + P(8,19); nextP(9,19) = P(6,19)*dt + P(9,19); @@ -402,9 +438,9 @@ nextP(0,20) = P(0,20) - P(1,20)*PS11 + P(10,20)*PS6 + P(11,20)*PS7 + P(12,20)*PS nextP(1,20) = P(0,20)*PS11 + P(1,20) - P(10,20)*PS34 + P(11,20)*PS9 - P(12,20)*PS7 + P(2,20)*PS13 - P(3,20)*PS12; nextP(2,20) = P(0,20)*PS12 - P(1,20)*PS13 - P(10,20)*PS9 - P(11,20)*PS34 + P(12,20)*PS6 + P(2,20) + P(3,20)*PS11; nextP(3,20) = P(0,20)*PS13 + P(1,20)*PS12 + P(10,20)*PS7 - P(11,20)*PS6 - P(12,20)*PS34 - P(2,20)*PS11 + P(3,20); -nextP(4,20) = P(0,20)*PS72 + P(1,20)*PS62 - P(13,20)*PS44 + P(14,20)*PS140 - P(15,20)*PS139 + P(2,20)*PS60 - P(3,20)*PS74 + P(4,20); -nextP(5,20) = P(0,20)*PS74 - P(1,20)*PS60 - P(13,20)*PS162 - P(14,20)*PS65 + P(15,20)*PS160 + P(2,20)*PS62 + P(3,20)*PS72 + P(5,20); -nextP(6,20) = P(0,20)*PS60 + P(1,20)*PS74 + P(13,20)*PS166 - P(14,20)*PS165 - P(15,20)*PS70 - P(2,20)*PS72 + P(3,20)*PS62 + P(6,20); +nextP(4,20) = P(0,20)*PS174 + P(1,20)*PS173 + P(13,20)*PS43 + P(14,20)*PS172 - P(15,20)*PS171 + P(2,20)*PS175 - P(3,20)*PS176 + P(4,20); +nextP(5,20) = -P(0,20)*PS202 - P(1,20)*PS204 - P(13,20)*PS193 + P(14,20)*PS75 + P(15,20)*PS190 + P(2,20)*PS201 + P(3,20)*PS203 + P(5,20); +nextP(6,20) = P(0,20)*PS216 + P(1,20)*PS217 + P(13,20)*PS199 - P(14,20)*PS197 + P(15,20)*PS87 - P(2,20)*PS214 + P(3,20)*PS215 + P(6,20); nextP(7,20) = P(4,20)*dt + P(7,20); nextP(8,20) = P(5,20)*dt + P(8,20); nextP(9,20) = P(6,20)*dt + P(9,20); @@ -423,9 +459,9 @@ nextP(0,21) = P(0,21) - P(1,21)*PS11 + P(10,21)*PS6 + P(11,21)*PS7 + P(12,21)*PS nextP(1,21) = P(0,21)*PS11 + P(1,21) - P(10,21)*PS34 + P(11,21)*PS9 - P(12,21)*PS7 + P(2,21)*PS13 - P(3,21)*PS12; nextP(2,21) = P(0,21)*PS12 - P(1,21)*PS13 - P(10,21)*PS9 - P(11,21)*PS34 + P(12,21)*PS6 + P(2,21) + P(3,21)*PS11; nextP(3,21) = P(0,21)*PS13 + P(1,21)*PS12 + P(10,21)*PS7 - P(11,21)*PS6 - P(12,21)*PS34 - P(2,21)*PS11 + P(3,21); -nextP(4,21) = P(0,21)*PS72 + P(1,21)*PS62 - P(13,21)*PS44 + P(14,21)*PS140 - P(15,21)*PS139 + P(2,21)*PS60 - P(3,21)*PS74 + P(4,21); -nextP(5,21) = P(0,21)*PS74 - P(1,21)*PS60 - P(13,21)*PS162 - P(14,21)*PS65 + P(15,21)*PS160 + P(2,21)*PS62 + P(3,21)*PS72 + P(5,21); -nextP(6,21) = P(0,21)*PS60 + P(1,21)*PS74 + P(13,21)*PS166 - P(14,21)*PS165 - P(15,21)*PS70 - P(2,21)*PS72 + P(3,21)*PS62 + P(6,21); +nextP(4,21) = P(0,21)*PS174 + P(1,21)*PS173 + P(13,21)*PS43 + P(14,21)*PS172 - P(15,21)*PS171 + P(2,21)*PS175 - P(3,21)*PS176 + P(4,21); +nextP(5,21) = -P(0,21)*PS202 - P(1,21)*PS204 - P(13,21)*PS193 + P(14,21)*PS75 + P(15,21)*PS190 + P(2,21)*PS201 + P(3,21)*PS203 + P(5,21); +nextP(6,21) = P(0,21)*PS216 + P(1,21)*PS217 + P(13,21)*PS199 - P(14,21)*PS197 + P(15,21)*PS87 - P(2,21)*PS214 + P(3,21)*PS215 + P(6,21); nextP(7,21) = P(4,21)*dt + P(7,21); nextP(8,21) = P(5,21)*dt + P(8,21); nextP(9,21) = P(6,21)*dt + P(9,21); @@ -445,9 +481,9 @@ nextP(0,22) = P(0,22) - P(1,22)*PS11 + P(10,22)*PS6 + P(11,22)*PS7 + P(12,22)*PS nextP(1,22) = P(0,22)*PS11 + P(1,22) - P(10,22)*PS34 + P(11,22)*PS9 - P(12,22)*PS7 + P(2,22)*PS13 - P(3,22)*PS12; nextP(2,22) = P(0,22)*PS12 - P(1,22)*PS13 - P(10,22)*PS9 - P(11,22)*PS34 + P(12,22)*PS6 + P(2,22) + P(3,22)*PS11; nextP(3,22) = P(0,22)*PS13 + P(1,22)*PS12 + P(10,22)*PS7 - P(11,22)*PS6 - P(12,22)*PS34 - P(2,22)*PS11 + P(3,22); -nextP(4,22) = P(0,22)*PS72 + P(1,22)*PS62 - P(13,22)*PS44 + P(14,22)*PS140 - P(15,22)*PS139 + P(2,22)*PS60 - P(3,22)*PS74 + P(4,22); -nextP(5,22) = P(0,22)*PS74 - P(1,22)*PS60 - P(13,22)*PS162 - P(14,22)*PS65 + P(15,22)*PS160 + P(2,22)*PS62 + P(3,22)*PS72 + P(5,22); -nextP(6,22) = P(0,22)*PS60 + P(1,22)*PS74 + P(13,22)*PS166 - P(14,22)*PS165 - P(15,22)*PS70 - P(2,22)*PS72 + P(3,22)*PS62 + P(6,22); +nextP(4,22) = P(0,22)*PS174 + P(1,22)*PS173 + P(13,22)*PS43 + P(14,22)*PS172 - P(15,22)*PS171 + P(2,22)*PS175 - P(3,22)*PS176 + P(4,22); +nextP(5,22) = -P(0,22)*PS202 - P(1,22)*PS204 - P(13,22)*PS193 + P(14,22)*PS75 + P(15,22)*PS190 + P(2,22)*PS201 + P(3,22)*PS203 + P(5,22); +nextP(6,22) = P(0,22)*PS216 + P(1,22)*PS217 + P(13,22)*PS199 - P(14,22)*PS197 + P(15,22)*PS87 - P(2,22)*PS214 + P(3,22)*PS215 + P(6,22); nextP(7,22) = P(4,22)*dt + P(7,22); nextP(8,22) = P(5,22)*dt + P(8,22); nextP(9,22) = P(6,22)*dt + P(9,22); @@ -468,9 +504,9 @@ nextP(0,23) = P(0,23) - P(1,23)*PS11 + P(10,23)*PS6 + P(11,23)*PS7 + P(12,23)*PS nextP(1,23) = P(0,23)*PS11 + P(1,23) - P(10,23)*PS34 + P(11,23)*PS9 - P(12,23)*PS7 + P(2,23)*PS13 - P(3,23)*PS12; nextP(2,23) = P(0,23)*PS12 - P(1,23)*PS13 - P(10,23)*PS9 - P(11,23)*PS34 + P(12,23)*PS6 + P(2,23) + P(3,23)*PS11; nextP(3,23) = P(0,23)*PS13 + P(1,23)*PS12 + P(10,23)*PS7 - P(11,23)*PS6 - P(12,23)*PS34 - P(2,23)*PS11 + P(3,23); -nextP(4,23) = P(0,23)*PS72 + P(1,23)*PS62 - P(13,23)*PS44 + P(14,23)*PS140 - P(15,23)*PS139 + P(2,23)*PS60 - P(3,23)*PS74 + P(4,23); -nextP(5,23) = P(0,23)*PS74 - P(1,23)*PS60 - P(13,23)*PS162 - P(14,23)*PS65 + P(15,23)*PS160 + P(2,23)*PS62 + P(3,23)*PS72 + P(5,23); -nextP(6,23) = P(0,23)*PS60 + P(1,23)*PS74 + P(13,23)*PS166 - P(14,23)*PS165 - P(15,23)*PS70 - P(2,23)*PS72 + P(3,23)*PS62 + P(6,23); +nextP(4,23) = P(0,23)*PS174 + P(1,23)*PS173 + P(13,23)*PS43 + P(14,23)*PS172 - P(15,23)*PS171 + P(2,23)*PS175 - P(3,23)*PS176 + P(4,23); +nextP(5,23) = -P(0,23)*PS202 - P(1,23)*PS204 - P(13,23)*PS193 + P(14,23)*PS75 + P(15,23)*PS190 + P(2,23)*PS201 + P(3,23)*PS203 + P(5,23); +nextP(6,23) = P(0,23)*PS216 + P(1,23)*PS217 + P(13,23)*PS199 - P(14,23)*PS197 + P(15,23)*PS87 - P(2,23)*PS214 + P(3,23)*PS215 + P(6,23); nextP(7,23) = P(4,23)*dt + P(7,23); nextP(8,23) = P(5,23)*dt + P(8,23); nextP(9,23) = P(6,23)*dt + P(9,23); diff --git a/EKF/python/ekf_derivation/generated/flow_generated.cpp b/EKF/python/ekf_derivation/generated/flow_generated.cpp index 7e7de331e3..03b144eb95 100644 --- a/EKF/python/ekf_derivation/generated/flow_generated.cpp +++ b/EKF/python/ekf_derivation/generated/flow_generated.cpp @@ -1,57 +1,62 @@ // X Axis Equations // Sub Expressions -const float HK0 = -Tbs(1,0)*q2 + Tbs(1,1)*q1 + Tbs(1,2)*q0; -const float HK1 = Tbs(1,0)*q3 + Tbs(1,1)*q0 - Tbs(1,2)*q1; -const float HK2 = Tbs(1,0)*q0 - Tbs(1,1)*q3 + Tbs(1,2)*q2; -const float HK3 = HK0*vd + HK1*ve + HK2*vn; -const float HK4 = 1.0F/range; -const float HK5 = 2*HK4; -const float HK6 = Tbs(1,0)*q1 + Tbs(1,1)*q2 + Tbs(1,2)*q3; -const float HK7 = -HK0*ve + HK1*vd + HK6*vn; -const float HK8 = HK0*vn - HK2*vd + HK6*ve; -const float HK9 = -HK1*vn + HK2*ve + HK6*vd; -const float HK10 = q0*q2; -const float HK11 = q1*q3; -const float HK12 = 2*Tbs(1,2); -const float HK13 = q0*q3; -const float HK14 = q1*q2; -const float HK15 = 2*Tbs(1,1); -const float HK16 = powf(q1, 2); -const float HK17 = powf(q2, 2); -const float HK18 = -HK17; -const float HK19 = powf(q0, 2); -const float HK20 = powf(q3, 2); -const float HK21 = HK19 - HK20; -const float HK22 = HK12*(HK10 + HK11) - HK15*(HK13 - HK14) + Tbs(1,0)*(HK16 + HK18 + HK21); -const float HK23 = 2*Tbs(1,0); -const float HK24 = q0*q1; -const float HK25 = q2*q3; -const float HK26 = -HK16; -const float HK27 = -HK12*(HK24 - HK25) + HK23*(HK13 + HK14) + Tbs(1,1)*(HK17 + HK21 + HK26); -const float HK28 = HK15*(HK24 + HK25) - HK23*(HK10 - HK11) + Tbs(1,2)*(HK18 + HK19 + HK20 + HK26); -const float HK29 = 2*HK3; -const float HK30 = 2*HK7; -const float HK31 = 2*HK8; -const float HK32 = 2*HK9; -const float HK33 = HK22*P(0,4) + HK27*P(0,5) + HK28*P(0,6) + HK29*P(0,0) + HK30*P(0,1) + HK31*P(0,2) + HK32*P(0,3); -const float HK34 = powf(range, -2); -const float HK35 = HK22*P(4,6) + HK27*P(5,6) + HK28*P(6,6) + HK29*P(0,6) + HK30*P(1,6) + HK31*P(2,6) + HK32*P(3,6); -const float HK36 = HK22*P(4,5) + HK27*P(5,5) + HK28*P(5,6) + HK29*P(0,5) + HK30*P(1,5) + HK31*P(2,5) + HK32*P(3,5); -const float HK37 = HK22*P(4,4) + HK27*P(4,5) + HK28*P(4,6) + HK29*P(0,4) + HK30*P(1,4) + HK31*P(2,4) + HK32*P(3,4); -const float HK38 = HK22*P(2,4) + HK27*P(2,5) + HK28*P(2,6) + HK29*P(0,2) + HK30*P(1,2) + HK31*P(2,2) + HK32*P(2,3); -const float HK39 = HK22*P(3,4) + HK27*P(3,5) + HK28*P(3,6) + HK29*P(0,3) + HK30*P(1,3) + HK31*P(2,3) + HK32*P(3,3); -const float HK40 = HK22*P(1,4) + HK27*P(1,5) + HK28*P(1,6) + HK29*P(0,1) + HK30*P(1,1) + HK31*P(1,2) + HK32*P(1,3); -const float HK41 = HK4/(HK22*HK34*HK37 + HK27*HK34*HK36 + HK28*HK34*HK35 + HK29*HK33*HK34 + HK30*HK34*HK40 + HK31*HK34*HK38 + HK32*HK34*HK39 + R_LOS); +const float HK0 = Tbs(1,0)*q2; +const float HK1 = Tbs(1,1)*q1; +const float HK2 = Tbs(1,1)*q3; +const float HK3 = Tbs(1,2)*q2; +const float HK4 = Tbs(1,0)*q3; +const float HK5 = Tbs(1,2)*q1; +const float HK6 = -HK5; +const float HK7 = vd*(HK0 - HK1) - ve*(HK4 + HK6) + vn*(HK2 - HK3); +const float HK8 = 1.0F/range; +const float HK9 = 2*HK8; +const float HK10 = Tbs(1,1)*q2; +const float HK11 = Tbs(1,2)*q3; +const float HK12 = Tbs(1,1)*q0; +const float HK13 = Tbs(1,2)*q0; +const float HK14 = vd*(HK12 + HK4 - 2*HK5) - ve*(-HK0 + 2*HK1 + HK13) + vn*(HK10 + HK11); +const float HK15 = Tbs(1,0)*q1; +const float HK16 = Tbs(1,0)*q0; +const float HK17 = -vd*(HK16 - HK2 + 2*HK3) + ve*(HK11 + HK15) + vn*(-2*HK0 + HK1 + HK13); +const float HK18 = vd*(HK10 + HK15) + ve*(HK16 - 2*HK2 + HK3) - vn*(HK12 + 2*HK4 + HK6); +const float HK19 = q0*q2; +const float HK20 = q1*q3; +const float HK21 = 2*Tbs(1,2); +const float HK22 = q0*q3; +const float HK23 = q1*q2; +const float HK24 = 2*Tbs(1,1); +const float HK25 = 2*powf(q2, 2); +const float HK26 = 2*powf(q3, 2) - 1; +const float HK27 = -HK21*(HK19 + HK20) + HK24*(HK22 - HK23) + Tbs(1,0)*(HK25 + HK26); +const float HK28 = 2*Tbs(1,0); +const float HK29 = q0*q1; +const float HK30 = q2*q3; +const float HK31 = 2*powf(q1, 2); +const float HK32 = HK21*(HK29 - HK30) - HK28*(HK22 + HK23) + Tbs(1,1)*(HK26 + HK31); +const float HK33 = -HK24*(HK29 + HK30) + HK28*(HK19 - HK20) + Tbs(1,2)*(HK25 + HK31 - 1); +const float HK34 = 2*HK7; +const float HK35 = 2*HK14; +const float HK36 = 2*HK17; +const float HK37 = 2*HK18; +const float HK38 = -HK27*P(0,4) - HK32*P(0,5) - HK33*P(0,6) - HK34*P(0,0) + HK35*P(0,1) + HK36*P(0,2) + HK37*P(0,3); +const float HK39 = powf(range, -2); +const float HK40 = -HK27*P(4,6) - HK32*P(5,6) - HK33*P(6,6) - HK34*P(0,6) + HK35*P(1,6) + HK36*P(2,6) + HK37*P(3,6); +const float HK41 = -HK27*P(4,5) - HK32*P(5,5) - HK33*P(5,6) - HK34*P(0,5) + HK35*P(1,5) + HK36*P(2,5) + HK37*P(3,5); +const float HK42 = -HK27*P(4,4) - HK32*P(4,5) - HK33*P(4,6) - HK34*P(0,4) + HK35*P(1,4) + HK36*P(2,4) + HK37*P(3,4); +const float HK43 = -HK27*P(3,4) - HK32*P(3,5) - HK33*P(3,6) - HK34*P(0,3) + HK35*P(1,3) + HK36*P(2,3) + HK37*P(3,3); +const float HK44 = -HK27*P(2,4) - HK32*P(2,5) - HK33*P(2,6) - HK34*P(0,2) + HK35*P(1,2) + HK36*P(2,2) + HK37*P(2,3); +const float HK45 = -HK27*P(1,4) - HK32*P(1,5) - HK33*P(1,6) - HK34*P(0,1) + HK35*P(1,1) + HK36*P(1,2) + HK37*P(1,3); +const float HK46 = HK8/(-HK27*HK39*HK42 - HK32*HK39*HK41 - HK33*HK39*HK40 - HK34*HK38*HK39 + HK35*HK39*HK45 + HK36*HK39*HK44 + HK37*HK39*HK43 + R_LOS); // Observation Jacobians -Hfusion.at<0>() = HK3*HK5; -Hfusion.at<1>() = HK5*HK7; -Hfusion.at<2>() = HK5*HK8; -Hfusion.at<3>() = HK5*HK9; -Hfusion.at<4>() = HK22*HK4; -Hfusion.at<5>() = HK27*HK4; -Hfusion.at<6>() = HK28*HK4; +Hfusion.at<0>() = -HK7*HK9; +Hfusion.at<1>() = HK14*HK9; +Hfusion.at<2>() = HK17*HK9; +Hfusion.at<3>() = HK18*HK9; +Hfusion.at<4>() = -HK27*HK8; +Hfusion.at<5>() = -HK32*HK8; +Hfusion.at<6>() = -HK33*HK8; Hfusion.at<7>() = 0; Hfusion.at<8>() = 0; Hfusion.at<9>() = 0; @@ -72,105 +77,111 @@ Hfusion.at<23>() = 0; // Kalman gains -Kfusion(0) = HK33*HK41; -Kfusion(1) = HK40*HK41; -Kfusion(2) = HK38*HK41; -Kfusion(3) = HK39*HK41; -Kfusion(4) = HK37*HK41; -Kfusion(5) = HK36*HK41; -Kfusion(6) = HK35*HK41; -Kfusion(7) = HK41*(HK22*P(4,7) + HK27*P(5,7) + HK28*P(6,7) + HK29*P(0,7) + HK30*P(1,7) + HK31*P(2,7) + HK32*P(3,7)); -Kfusion(8) = HK41*(HK22*P(4,8) + HK27*P(5,8) + HK28*P(6,8) + HK29*P(0,8) + HK30*P(1,8) + HK31*P(2,8) + HK32*P(3,8)); -Kfusion(9) = HK41*(HK22*P(4,9) + HK27*P(5,9) + HK28*P(6,9) + HK29*P(0,9) + HK30*P(1,9) + HK31*P(2,9) + HK32*P(3,9)); -Kfusion(10) = HK41*(HK22*P(4,10) + HK27*P(5,10) + HK28*P(6,10) + HK29*P(0,10) + HK30*P(1,10) + HK31*P(2,10) + HK32*P(3,10)); -Kfusion(11) = HK41*(HK22*P(4,11) + HK27*P(5,11) + HK28*P(6,11) + HK29*P(0,11) + HK30*P(1,11) + HK31*P(2,11) + HK32*P(3,11)); -Kfusion(12) = HK41*(HK22*P(4,12) + HK27*P(5,12) + HK28*P(6,12) + HK29*P(0,12) + HK30*P(1,12) + HK31*P(2,12) + HK32*P(3,12)); -Kfusion(13) = HK41*(HK22*P(4,13) + HK27*P(5,13) + HK28*P(6,13) + HK29*P(0,13) + HK30*P(1,13) + HK31*P(2,13) + HK32*P(3,13)); -Kfusion(14) = HK41*(HK22*P(4,14) + HK27*P(5,14) + HK28*P(6,14) + HK29*P(0,14) + HK30*P(1,14) + HK31*P(2,14) + HK32*P(3,14)); -Kfusion(15) = HK41*(HK22*P(4,15) + HK27*P(5,15) + HK28*P(6,15) + HK29*P(0,15) + HK30*P(1,15) + HK31*P(2,15) + HK32*P(3,15)); -Kfusion(16) = HK41*(HK22*P(4,16) + HK27*P(5,16) + HK28*P(6,16) + HK29*P(0,16) + HK30*P(1,16) + HK31*P(2,16) + HK32*P(3,16)); -Kfusion(17) = HK41*(HK22*P(4,17) + HK27*P(5,17) + HK28*P(6,17) + HK29*P(0,17) + HK30*P(1,17) + HK31*P(2,17) + HK32*P(3,17)); -Kfusion(18) = HK41*(HK22*P(4,18) + HK27*P(5,18) + HK28*P(6,18) + HK29*P(0,18) + HK30*P(1,18) + HK31*P(2,18) + HK32*P(3,18)); -Kfusion(19) = HK41*(HK22*P(4,19) + HK27*P(5,19) + HK28*P(6,19) + HK29*P(0,19) + HK30*P(1,19) + HK31*P(2,19) + HK32*P(3,19)); -Kfusion(20) = HK41*(HK22*P(4,20) + HK27*P(5,20) + HK28*P(6,20) + HK29*P(0,20) + HK30*P(1,20) + HK31*P(2,20) + HK32*P(3,20)); -Kfusion(21) = HK41*(HK22*P(4,21) + HK27*P(5,21) + HK28*P(6,21) + HK29*P(0,21) + HK30*P(1,21) + HK31*P(2,21) + HK32*P(3,21)); -Kfusion(22) = HK41*(HK22*P(4,22) + HK27*P(5,22) + HK28*P(6,22) + HK29*P(0,22) + HK30*P(1,22) + HK31*P(2,22) + HK32*P(3,22)); -Kfusion(23) = HK41*(HK22*P(4,23) + HK27*P(5,23) + HK28*P(6,23) + HK29*P(0,23) + HK30*P(1,23) + HK31*P(2,23) + HK32*P(3,23)); +Kfusion(0) = HK38*HK46; +Kfusion(1) = HK45*HK46; +Kfusion(2) = HK44*HK46; +Kfusion(3) = HK43*HK46; +Kfusion(4) = HK42*HK46; +Kfusion(5) = HK41*HK46; +Kfusion(6) = HK40*HK46; +Kfusion(7) = HK46*(-HK27*P(4,7) - HK32*P(5,7) - HK33*P(6,7) - HK34*P(0,7) + HK35*P(1,7) + HK36*P(2,7) + HK37*P(3,7)); +Kfusion(8) = HK46*(-HK27*P(4,8) - HK32*P(5,8) - HK33*P(6,8) - HK34*P(0,8) + HK35*P(1,8) + HK36*P(2,8) + HK37*P(3,8)); +Kfusion(9) = HK46*(-HK27*P(4,9) - HK32*P(5,9) - HK33*P(6,9) - HK34*P(0,9) + HK35*P(1,9) + HK36*P(2,9) + HK37*P(3,9)); +Kfusion(10) = HK46*(-HK27*P(4,10) - HK32*P(5,10) - HK33*P(6,10) - HK34*P(0,10) + HK35*P(1,10) + HK36*P(2,10) + HK37*P(3,10)); +Kfusion(11) = HK46*(-HK27*P(4,11) - HK32*P(5,11) - HK33*P(6,11) - HK34*P(0,11) + HK35*P(1,11) + HK36*P(2,11) + HK37*P(3,11)); +Kfusion(12) = HK46*(-HK27*P(4,12) - HK32*P(5,12) - HK33*P(6,12) - HK34*P(0,12) + HK35*P(1,12) + HK36*P(2,12) + HK37*P(3,12)); +Kfusion(13) = HK46*(-HK27*P(4,13) - HK32*P(5,13) - HK33*P(6,13) - HK34*P(0,13) + HK35*P(1,13) + HK36*P(2,13) + HK37*P(3,13)); +Kfusion(14) = HK46*(-HK27*P(4,14) - HK32*P(5,14) - HK33*P(6,14) - HK34*P(0,14) + HK35*P(1,14) + HK36*P(2,14) + HK37*P(3,14)); +Kfusion(15) = HK46*(-HK27*P(4,15) - HK32*P(5,15) - HK33*P(6,15) - HK34*P(0,15) + HK35*P(1,15) + HK36*P(2,15) + HK37*P(3,15)); +Kfusion(16) = HK46*(-HK27*P(4,16) - HK32*P(5,16) - HK33*P(6,16) - HK34*P(0,16) + HK35*P(1,16) + HK36*P(2,16) + HK37*P(3,16)); +Kfusion(17) = HK46*(-HK27*P(4,17) - HK32*P(5,17) - HK33*P(6,17) - HK34*P(0,17) + HK35*P(1,17) + HK36*P(2,17) + HK37*P(3,17)); +Kfusion(18) = HK46*(-HK27*P(4,18) - HK32*P(5,18) - HK33*P(6,18) - HK34*P(0,18) + HK35*P(1,18) + HK36*P(2,18) + HK37*P(3,18)); +Kfusion(19) = HK46*(-HK27*P(4,19) - HK32*P(5,19) - HK33*P(6,19) - HK34*P(0,19) + HK35*P(1,19) + HK36*P(2,19) + HK37*P(3,19)); +Kfusion(20) = HK46*(-HK27*P(4,20) - HK32*P(5,20) - HK33*P(6,20) - HK34*P(0,20) + HK35*P(1,20) + HK36*P(2,20) + HK37*P(3,20)); +Kfusion(21) = HK46*(-HK27*P(4,21) - HK32*P(5,21) - HK33*P(6,21) - HK34*P(0,21) + HK35*P(1,21) + HK36*P(2,21) + HK37*P(3,21)); +Kfusion(22) = HK46*(-HK27*P(4,22) - HK32*P(5,22) - HK33*P(6,22) - HK34*P(0,22) + HK35*P(1,22) + HK36*P(2,22) + HK37*P(3,22)); +Kfusion(23) = HK46*(-HK27*P(4,23) - HK32*P(5,23) - HK33*P(6,23) - HK34*P(0,23) + HK35*P(1,23) + HK36*P(2,23) + HK37*P(3,23)); // Y Axis Equations // Sub Expressions -const float HK0 = Tbs(0,1)*q1; -const float HK1 = Tbs(0,2)*q0; -const float HK2 = Tbs(0,0)*q2; -const float HK3 = HK0 + HK1 - HK2; -const float HK4 = Tbs(0,0)*q3; -const float HK5 = Tbs(0,1)*q0; -const float HK6 = Tbs(0,2)*q1; -const float HK7 = HK4 + HK5 - HK6; -const float HK8 = Tbs(0,0)*q0; -const float HK9 = Tbs(0,2)*q2; -const float HK10 = Tbs(0,1)*q3; -const float HK11 = -HK10 + HK8 + HK9; -const float HK12 = HK11*vn + HK3*vd + HK7*ve; -const float HK13 = 1.0F/range; -const float HK14 = 2*HK13; -const float HK15 = Tbs(0,0)*q1 + Tbs(0,1)*q2 + Tbs(0,2)*q3; -const float HK16 = HK15*vn + HK7*vd; -const float HK17 = HK16 - HK3*ve; -const float HK18 = HK15*ve + HK3*vn; -const float HK19 = -HK11*vd + HK18; -const float HK20 = HK11*ve + HK15*vd; -const float HK21 = HK20 - HK7*vn; -const float HK22 = q0*q3; -const float HK23 = q1*q2; -const float HK24 = 2*Tbs(0,1); -const float HK25 = powf(q1, 2); -const float HK26 = powf(q2, 2); -const float HK27 = -HK26; -const float HK28 = powf(q0, 2); -const float HK29 = powf(q3, 2); -const float HK30 = HK28 - HK29; -const float HK31 = q0*q2; -const float HK32 = q1*q3; -const float HK33 = 2*Tbs(0,2); -const float HK34 = HK33*(HK31 + HK32) + Tbs(0,0)*(HK25 + HK27 + HK30); -const float HK35 = -HK24*(HK22 - HK23) + HK34; -const float HK36 = q0*q1; -const float HK37 = q2*q3; -const float HK38 = -HK25; +const float HK0 = Tbs(0,0)*q3; +const float HK1 = Tbs(0,2)*q1; +const float HK2 = -HK1; +const float HK3 = ve*(HK0 + HK2); +const float HK4 = Tbs(0,0)*q2; +const float HK5 = Tbs(0,1)*q1; +const float HK6 = Tbs(0,1)*q3; +const float HK7 = Tbs(0,2)*q2; +const float HK8 = HK3 - vd*(HK4 - HK5) - vn*(HK6 - HK7); +const float HK9 = 1.0F/range; +const float HK10 = 2*HK9; +const float HK11 = Tbs(0,2)*q0; +const float HK12 = -HK4; +const float HK13 = 2*HK5; +const float HK14 = Tbs(0,1)*q0; +const float HK15 = Tbs(0,1)*q2; +const float HK16 = Tbs(0,2)*q3; +const float HK17 = vd*(HK0 - 2*HK1 + HK14) + vn*(HK15 + HK16); +const float HK18 = HK17 - ve*(HK11 + HK12 + HK13); +const float HK19 = Tbs(0,0)*q0; +const float HK20 = -HK6; +const float HK21 = 2*HK7; +const float HK22 = Tbs(0,0)*q1; +const float HK23 = ve*(HK16 + HK22) + vn*(HK11 - 2*HK4 + HK5); +const float HK24 = HK23 - vd*(HK19 + HK20 + HK21); +const float HK25 = 2*HK0; +const float HK26 = vd*(HK15 + HK22) + ve*(HK19 - 2*HK6 + HK7); +const float HK27 = HK26 - vn*(HK14 + HK2 + HK25); +const float HK28 = q0*q2; +const float HK29 = q1*q3; +const float HK30 = 2*Tbs(0,2); +const float HK31 = HK30*(HK28 + HK29); +const float HK32 = q0*q3; +const float HK33 = q1*q2; +const float HK34 = 2*Tbs(0,1); +const float HK35 = 2*powf(q2, 2); +const float HK36 = 2*powf(q3, 2); +const float HK37 = HK36 - 1; +const float HK38 = HK31 - HK34*(HK32 - HK33) - Tbs(0,0)*(HK35 + HK37); const float HK39 = 2*Tbs(0,0); -const float HK40 = HK39*(HK22 + HK23) + Tbs(0,1)*(HK26 + HK30 + HK38); -const float HK41 = -HK33*(HK36 - HK37) + HK40; -const float HK42 = HK24*(HK36 + HK37) + Tbs(0,2)*(HK27 + HK28 + HK29 + HK38); -const float HK43 = -HK39*(HK31 - HK32) + HK42; -const float HK44 = 2*HK12; -const float HK45 = 2*HK16 + 2*ve*(-HK0 - HK1 + HK2); -const float HK46 = 2*HK18 + 2*vd*(HK10 - HK8 - HK9); -const float HK47 = 2*HK20 + 2*vn*(-HK4 - HK5 + HK6); -const float HK48 = HK24*(-HK22 + HK23) + HK34; -const float HK49 = HK33*(-HK36 + HK37) + HK40; -const float HK50 = HK39*(-HK31 + HK32) + HK42; -const float HK51 = HK44*P(0,0) + HK45*P(0,1) + HK46*P(0,2) + HK47*P(0,3) + HK48*P(0,4) + HK49*P(0,5) + HK50*P(0,6); -const float HK52 = powf(range, -2); -const float HK53 = HK44*P(0,6) + HK45*P(1,6) + HK46*P(2,6) + HK47*P(3,6) + HK48*P(4,6) + HK49*P(5,6) + HK50*P(6,6); -const float HK54 = HK44*P(0,5) + HK45*P(1,5) + HK46*P(2,5) + HK47*P(3,5) + HK48*P(4,5) + HK49*P(5,5) + HK50*P(5,6); -const float HK55 = HK44*P(0,4) + HK45*P(1,4) + HK46*P(2,4) + HK47*P(3,4) + HK48*P(4,4) + HK49*P(4,5) + HK50*P(4,6); -const float HK56 = HK44*P(0,2) + HK45*P(1,2) + HK46*P(2,2) + HK47*P(2,3) + HK48*P(2,4) + HK49*P(2,5) + HK50*P(2,6); -const float HK57 = 2*HK52; -const float HK58 = HK44*P(0,3) + HK45*P(1,3) + HK46*P(2,3) + HK47*P(3,3) + HK48*P(3,4) + HK49*P(3,5) + HK50*P(3,6); -const float HK59 = HK44*P(0,1) + HK45*P(1,1) + HK46*P(1,2) + HK47*P(1,3) + HK48*P(1,4) + HK49*P(1,5) + HK50*P(1,6); -const float HK60 = HK13/(HK17*HK57*HK59 + HK19*HK56*HK57 + HK21*HK57*HK58 + HK35*HK52*HK55 + HK41*HK52*HK54 + HK43*HK52*HK53 + HK44*HK51*HK52 + R_LOS); +const float HK40 = HK39*(HK32 + HK33); +const float HK41 = q0*q1; +const float HK42 = q2*q3; +const float HK43 = 2*powf(q1, 2); +const float HK44 = -HK30*(HK41 - HK42) + HK40 - Tbs(0,1)*(HK37 + HK43); +const float HK45 = HK34*(HK41 + HK42); +const float HK46 = -HK39*(HK28 - HK29) + HK45 - Tbs(0,2)*(HK35 + HK43 - 1); +const float HK47 = 2*HK3 + 2*vd*(HK12 + HK5) + 2*vn*(HK20 + HK7); +const float HK48 = -HK35; +const float HK49 = 1 - HK36; +const float HK50 = HK31 + HK34*(-HK32 + HK33) + Tbs(0,0)*(HK48 + HK49); +const float HK51 = -HK43; +const float HK52 = HK30*(-HK41 + HK42) + HK40 + Tbs(0,1)*(HK49 + HK51); +const float HK53 = HK39*(-HK28 + HK29) + HK45 + Tbs(0,2)*(HK48 + HK51 + 1); +const float HK54 = 2*HK17 + 2*ve*(-HK11 - HK13 + HK4); +const float HK55 = 2*HK23 + 2*vd*(-HK19 - HK21 + HK6); +const float HK56 = 2*HK26 + 2*vn*(HK1 - HK14 - HK25); +const float HK57 = HK47*P(0,0) + HK50*P(0,4) + HK52*P(0,5) + HK53*P(0,6) + HK54*P(0,1) + HK55*P(0,2) + HK56*P(0,3); +const float HK58 = powf(range, -2); +const float HK59 = 2*HK58; +const float HK60 = HK47*P(0,6) + HK50*P(4,6) + HK52*P(5,6) + HK53*P(6,6) + HK54*P(1,6) + HK55*P(2,6) + HK56*P(3,6); +const float HK61 = HK47*P(0,5) + HK50*P(4,5) + HK52*P(5,5) + HK53*P(5,6) + HK54*P(1,5) + HK55*P(2,5) + HK56*P(3,5); +const float HK62 = HK47*P(0,4) + HK50*P(4,4) + HK52*P(4,5) + HK53*P(4,6) + HK54*P(1,4) + HK55*P(2,4) + HK56*P(3,4); +const float HK63 = HK47*P(0,3) + HK50*P(3,4) + HK52*P(3,5) + HK53*P(3,6) + HK54*P(1,3) + HK55*P(2,3) + HK56*P(3,3); +const float HK64 = HK47*P(0,2) + HK50*P(2,4) + HK52*P(2,5) + HK53*P(2,6) + HK54*P(1,2) + HK55*P(2,2) + HK56*P(2,3); +const float HK65 = HK47*P(0,1) + HK50*P(1,4) + HK52*P(1,5) + HK53*P(1,6) + HK54*P(1,1) + HK55*P(1,2) + HK56*P(1,3); +const float HK66 = HK9/(HK18*HK59*HK65 + HK24*HK59*HK64 + HK27*HK59*HK63 + HK38*HK58*HK62 + HK44*HK58*HK61 + HK46*HK58*HK60 + HK57*HK59*HK8 + R_LOS); // Observation Jacobians -Hfusion.at<0>() = -HK12*HK14; -Hfusion.at<1>() = -HK14*HK17; -Hfusion.at<2>() = -HK14*HK19; -Hfusion.at<3>() = -HK14*HK21; -Hfusion.at<4>() = -HK13*HK35; -Hfusion.at<5>() = -HK13*HK41; -Hfusion.at<6>() = -HK13*HK43; +Hfusion.at<0>() = -HK10*HK8; +Hfusion.at<1>() = -HK10*HK18; +Hfusion.at<2>() = -HK10*HK24; +Hfusion.at<3>() = -HK10*HK27; +Hfusion.at<4>() = -HK38*HK9; +Hfusion.at<5>() = -HK44*HK9; +Hfusion.at<6>() = -HK46*HK9; Hfusion.at<7>() = 0; Hfusion.at<8>() = 0; Hfusion.at<9>() = 0; @@ -191,29 +202,29 @@ Hfusion.at<23>() = 0; // Kalman gains -Kfusion(0) = -HK51*HK60; -Kfusion(1) = -HK59*HK60; -Kfusion(2) = -HK56*HK60; -Kfusion(3) = -HK58*HK60; -Kfusion(4) = -HK55*HK60; -Kfusion(5) = -HK54*HK60; -Kfusion(6) = -HK53*HK60; -Kfusion(7) = -HK60*(HK44*P(0,7) + HK45*P(1,7) + HK46*P(2,7) + HK47*P(3,7) + HK48*P(4,7) + HK49*P(5,7) + HK50*P(6,7)); -Kfusion(8) = -HK60*(HK44*P(0,8) + HK45*P(1,8) + HK46*P(2,8) + HK47*P(3,8) + HK48*P(4,8) + HK49*P(5,8) + HK50*P(6,8)); -Kfusion(9) = -HK60*(HK44*P(0,9) + HK45*P(1,9) + HK46*P(2,9) + HK47*P(3,9) + HK48*P(4,9) + HK49*P(5,9) + HK50*P(6,9)); -Kfusion(10) = -HK60*(HK44*P(0,10) + HK45*P(1,10) + HK46*P(2,10) + HK47*P(3,10) + HK48*P(4,10) + HK49*P(5,10) + HK50*P(6,10)); -Kfusion(11) = -HK60*(HK44*P(0,11) + HK45*P(1,11) + HK46*P(2,11) + HK47*P(3,11) + HK48*P(4,11) + HK49*P(5,11) + HK50*P(6,11)); -Kfusion(12) = -HK60*(HK44*P(0,12) + HK45*P(1,12) + HK46*P(2,12) + HK47*P(3,12) + HK48*P(4,12) + HK49*P(5,12) + HK50*P(6,12)); -Kfusion(13) = -HK60*(HK44*P(0,13) + HK45*P(1,13) + HK46*P(2,13) + HK47*P(3,13) + HK48*P(4,13) + HK49*P(5,13) + HK50*P(6,13)); -Kfusion(14) = -HK60*(HK44*P(0,14) + HK45*P(1,14) + HK46*P(2,14) + HK47*P(3,14) + HK48*P(4,14) + HK49*P(5,14) + HK50*P(6,14)); -Kfusion(15) = -HK60*(HK44*P(0,15) + HK45*P(1,15) + HK46*P(2,15) + HK47*P(3,15) + HK48*P(4,15) + HK49*P(5,15) + HK50*P(6,15)); -Kfusion(16) = -HK60*(HK44*P(0,16) + HK45*P(1,16) + HK46*P(2,16) + HK47*P(3,16) + HK48*P(4,16) + HK49*P(5,16) + HK50*P(6,16)); -Kfusion(17) = -HK60*(HK44*P(0,17) + HK45*P(1,17) + HK46*P(2,17) + HK47*P(3,17) + HK48*P(4,17) + HK49*P(5,17) + HK50*P(6,17)); -Kfusion(18) = -HK60*(HK44*P(0,18) + HK45*P(1,18) + HK46*P(2,18) + HK47*P(3,18) + HK48*P(4,18) + HK49*P(5,18) + HK50*P(6,18)); -Kfusion(19) = -HK60*(HK44*P(0,19) + HK45*P(1,19) + HK46*P(2,19) + HK47*P(3,19) + HK48*P(4,19) + HK49*P(5,19) + HK50*P(6,19)); -Kfusion(20) = -HK60*(HK44*P(0,20) + HK45*P(1,20) + HK46*P(2,20) + HK47*P(3,20) + HK48*P(4,20) + HK49*P(5,20) + HK50*P(6,20)); -Kfusion(21) = -HK60*(HK44*P(0,21) + HK45*P(1,21) + HK46*P(2,21) + HK47*P(3,21) + HK48*P(4,21) + HK49*P(5,21) + HK50*P(6,21)); -Kfusion(22) = -HK60*(HK44*P(0,22) + HK45*P(1,22) + HK46*P(2,22) + HK47*P(3,22) + HK48*P(4,22) + HK49*P(5,22) + HK50*P(6,22)); -Kfusion(23) = -HK60*(HK44*P(0,23) + HK45*P(1,23) + HK46*P(2,23) + HK47*P(3,23) + HK48*P(4,23) + HK49*P(5,23) + HK50*P(6,23)); +Kfusion(0) = -HK57*HK66; +Kfusion(1) = -HK65*HK66; +Kfusion(2) = -HK64*HK66; +Kfusion(3) = -HK63*HK66; +Kfusion(4) = -HK62*HK66; +Kfusion(5) = -HK61*HK66; +Kfusion(6) = -HK60*HK66; +Kfusion(7) = -HK66*(HK47*P(0,7) + HK50*P(4,7) + HK52*P(5,7) + HK53*P(6,7) + HK54*P(1,7) + HK55*P(2,7) + HK56*P(3,7)); +Kfusion(8) = -HK66*(HK47*P(0,8) + HK50*P(4,8) + HK52*P(5,8) + HK53*P(6,8) + HK54*P(1,8) + HK55*P(2,8) + HK56*P(3,8)); +Kfusion(9) = -HK66*(HK47*P(0,9) + HK50*P(4,9) + HK52*P(5,9) + HK53*P(6,9) + HK54*P(1,9) + HK55*P(2,9) + HK56*P(3,9)); +Kfusion(10) = -HK66*(HK47*P(0,10) + HK50*P(4,10) + HK52*P(5,10) + HK53*P(6,10) + HK54*P(1,10) + HK55*P(2,10) + HK56*P(3,10)); +Kfusion(11) = -HK66*(HK47*P(0,11) + HK50*P(4,11) + HK52*P(5,11) + HK53*P(6,11) + HK54*P(1,11) + HK55*P(2,11) + HK56*P(3,11)); +Kfusion(12) = -HK66*(HK47*P(0,12) + HK50*P(4,12) + HK52*P(5,12) + HK53*P(6,12) + HK54*P(1,12) + HK55*P(2,12) + HK56*P(3,12)); +Kfusion(13) = -HK66*(HK47*P(0,13) + HK50*P(4,13) + HK52*P(5,13) + HK53*P(6,13) + HK54*P(1,13) + HK55*P(2,13) + HK56*P(3,13)); +Kfusion(14) = -HK66*(HK47*P(0,14) + HK50*P(4,14) + HK52*P(5,14) + HK53*P(6,14) + HK54*P(1,14) + HK55*P(2,14) + HK56*P(3,14)); +Kfusion(15) = -HK66*(HK47*P(0,15) + HK50*P(4,15) + HK52*P(5,15) + HK53*P(6,15) + HK54*P(1,15) + HK55*P(2,15) + HK56*P(3,15)); +Kfusion(16) = -HK66*(HK47*P(0,16) + HK50*P(4,16) + HK52*P(5,16) + HK53*P(6,16) + HK54*P(1,16) + HK55*P(2,16) + HK56*P(3,16)); +Kfusion(17) = -HK66*(HK47*P(0,17) + HK50*P(4,17) + HK52*P(5,17) + HK53*P(6,17) + HK54*P(1,17) + HK55*P(2,17) + HK56*P(3,17)); +Kfusion(18) = -HK66*(HK47*P(0,18) + HK50*P(4,18) + HK52*P(5,18) + HK53*P(6,18) + HK54*P(1,18) + HK55*P(2,18) + HK56*P(3,18)); +Kfusion(19) = -HK66*(HK47*P(0,19) + HK50*P(4,19) + HK52*P(5,19) + HK53*P(6,19) + HK54*P(1,19) + HK55*P(2,19) + HK56*P(3,19)); +Kfusion(20) = -HK66*(HK47*P(0,20) + HK50*P(4,20) + HK52*P(5,20) + HK53*P(6,20) + HK54*P(1,20) + HK55*P(2,20) + HK56*P(3,20)); +Kfusion(21) = -HK66*(HK47*P(0,21) + HK50*P(4,21) + HK52*P(5,21) + HK53*P(6,21) + HK54*P(1,21) + HK55*P(2,21) + HK56*P(3,21)); +Kfusion(22) = -HK66*(HK47*P(0,22) + HK50*P(4,22) + HK52*P(5,22) + HK53*P(6,22) + HK54*P(1,22) + HK55*P(2,22) + HK56*P(3,22)); +Kfusion(23) = -HK66*(HK47*P(0,23) + HK50*P(4,23) + HK52*P(5,23) + HK53*P(6,23) + HK54*P(1,23) + HK55*P(2,23) + HK56*P(3,23)); diff --git a/EKF/python/ekf_derivation/generated/flow_generated_alt.cpp b/EKF/python/ekf_derivation/generated/flow_generated_alt.cpp index 5731ab6118..d13faa5c5d 100644 --- a/EKF/python/ekf_derivation/generated/flow_generated_alt.cpp +++ b/EKF/python/ekf_derivation/generated/flow_generated_alt.cpp @@ -1,110 +1,125 @@ // Sub Expressions -const float HK0 = -Tbs(1,0)*q2 + Tbs(1,1)*q1 + Tbs(1,2)*q0; -const float HK1 = Tbs(1,0)*q3 + Tbs(1,1)*q0 - Tbs(1,2)*q1; -const float HK2 = Tbs(1,0)*q0 - Tbs(1,1)*q3 + Tbs(1,2)*q2; -const float HK3 = HK0*vd + HK1*ve + HK2*vn; -const float HK4 = 1.0F/range; -const float HK5 = 2*HK4; -const float HK6 = Tbs(1,0)*q1 + Tbs(1,1)*q2 + Tbs(1,2)*q3; -const float HK7 = -HK0*ve + HK1*vd + HK6*vn; -const float HK8 = HK0*vn - HK2*vd + HK6*ve; -const float HK9 = -HK1*vn + HK2*ve + HK6*vd; -const float HK10 = q0*q2; -const float HK11 = q1*q3; -const float HK12 = HK10 + HK11; -const float HK13 = 2*Tbs(1,2); -const float HK14 = q0*q3; -const float HK15 = q1*q2; -const float HK16 = HK14 - HK15; -const float HK17 = 2*Tbs(1,1); -const float HK18 = powf(q1, 2); -const float HK19 = powf(q2, 2); -const float HK20 = -HK19; -const float HK21 = powf(q0, 2); -const float HK22 = powf(q3, 2); -const float HK23 = HK21 - HK22; -const float HK24 = HK18 + HK20 + HK23; -const float HK25 = HK12*HK13 - HK16*HK17 + HK24*Tbs(1,0); -const float HK26 = HK14 + HK15; -const float HK27 = 2*Tbs(1,0); -const float HK28 = q0*q1; -const float HK29 = q2*q3; -const float HK30 = HK28 - HK29; -const float HK31 = -HK18; -const float HK32 = HK19 + HK23 + HK31; -const float HK33 = -HK13*HK30 + HK26*HK27 + HK32*Tbs(1,1); -const float HK34 = HK28 + HK29; -const float HK35 = HK10 - HK11; -const float HK36 = HK20 + HK21 + HK22 + HK31; -const float HK37 = HK17*HK34 - HK27*HK35 + HK36*Tbs(1,2); -const float HK38 = 2*HK3; -const float HK39 = 2*HK7; -const float HK40 = 2*HK8; -const float HK41 = 2*HK9; -const float HK42 = HK25*P(0,4) + HK33*P(0,5) + HK37*P(0,6) + HK38*P(0,0) + HK39*P(0,1) + HK40*P(0,2) + HK41*P(0,3); -const float HK43 = powf(range, -2); -const float HK44 = HK25*P(4,6) + HK33*P(5,6) + HK37*P(6,6) + HK38*P(0,6) + HK39*P(1,6) + HK40*P(2,6) + HK41*P(3,6); -const float HK45 = HK25*P(4,5) + HK33*P(5,5) + HK37*P(5,6) + HK38*P(0,5) + HK39*P(1,5) + HK40*P(2,5) + HK41*P(3,5); -const float HK46 = HK25*P(4,4) + HK33*P(4,5) + HK37*P(4,6) + HK38*P(0,4) + HK39*P(1,4) + HK40*P(2,4) + HK41*P(3,4); -const float HK47 = HK25*P(2,4) + HK33*P(2,5) + HK37*P(2,6) + HK38*P(0,2) + HK39*P(1,2) + HK40*P(2,2) + HK41*P(2,3); -const float HK48 = HK25*P(3,4) + HK33*P(3,5) + HK37*P(3,6) + HK38*P(0,3) + HK39*P(1,3) + HK40*P(2,3) + HK41*P(3,3); -const float HK49 = HK25*P(1,4) + HK33*P(1,5) + HK37*P(1,6) + HK38*P(0,1) + HK39*P(1,1) + HK40*P(1,2) + HK41*P(1,3); -const float HK50 = HK4/(HK25*HK43*HK46 + HK33*HK43*HK45 + HK37*HK43*HK44 + HK38*HK42*HK43 + HK39*HK43*HK49 + HK40*HK43*HK47 + HK41*HK43*HK48 + R_LOS); -const float HK51 = Tbs(0,1)*q1; -const float HK52 = Tbs(0,2)*q0; -const float HK53 = Tbs(0,0)*q2; -const float HK54 = HK51 + HK52 - HK53; -const float HK55 = Tbs(0,0)*q3; -const float HK56 = Tbs(0,1)*q0; -const float HK57 = Tbs(0,2)*q1; -const float HK58 = HK55 + HK56 - HK57; -const float HK59 = Tbs(0,0)*q0; -const float HK60 = Tbs(0,2)*q2; -const float HK61 = Tbs(0,1)*q3; -const float HK62 = HK59 + HK60 - HK61; -const float HK63 = HK54*vd + HK58*ve + HK62*vn; -const float HK64 = Tbs(0,0)*q1 + Tbs(0,1)*q2 + Tbs(0,2)*q3; -const float HK65 = HK58*vd + HK64*vn; -const float HK66 = -HK54*ve + HK65; -const float HK67 = HK54*vn + HK64*ve; -const float HK68 = -HK62*vd + HK67; -const float HK69 = HK62*ve + HK64*vd; -const float HK70 = -HK58*vn + HK69; -const float HK71 = 2*Tbs(0,1); -const float HK72 = 2*Tbs(0,2); -const float HK73 = HK12*HK72 + HK24*Tbs(0,0); -const float HK74 = -HK16*HK71 + HK73; -const float HK75 = 2*Tbs(0,0); -const float HK76 = HK26*HK75 + HK32*Tbs(0,1); -const float HK77 = -HK30*HK72 + HK76; -const float HK78 = HK34*HK71 + HK36*Tbs(0,2); -const float HK79 = -HK35*HK75 + HK78; -const float HK80 = 2*HK63; -const float HK81 = 2*HK65 + 2*ve*(-HK51 - HK52 + HK53); -const float HK82 = 2*HK67 + 2*vd*(-HK59 - HK60 + HK61); -const float HK83 = 2*HK69 + 2*vn*(-HK55 - HK56 + HK57); -const float HK84 = HK71*(-HK14 + HK15) + HK73; -const float HK85 = HK72*(-HK28 + HK29) + HK76; -const float HK86 = HK75*(-HK10 + HK11) + HK78; -const float HK87 = HK80*P(0,0) + HK81*P(0,1) + HK82*P(0,2) + HK83*P(0,3) + HK84*P(0,4) + HK85*P(0,5) + HK86*P(0,6); -const float HK88 = HK80*P(0,6) + HK81*P(1,6) + HK82*P(2,6) + HK83*P(3,6) + HK84*P(4,6) + HK85*P(5,6) + HK86*P(6,6); -const float HK89 = HK80*P(0,5) + HK81*P(1,5) + HK82*P(2,5) + HK83*P(3,5) + HK84*P(4,5) + HK85*P(5,5) + HK86*P(5,6); -const float HK90 = HK80*P(0,4) + HK81*P(1,4) + HK82*P(2,4) + HK83*P(3,4) + HK84*P(4,4) + HK85*P(4,5) + HK86*P(4,6); -const float HK91 = HK80*P(0,2) + HK81*P(1,2) + HK82*P(2,2) + HK83*P(2,3) + HK84*P(2,4) + HK85*P(2,5) + HK86*P(2,6); -const float HK92 = 2*HK43; -const float HK93 = HK80*P(0,3) + HK81*P(1,3) + HK82*P(2,3) + HK83*P(3,3) + HK84*P(3,4) + HK85*P(3,5) + HK86*P(3,6); -const float HK94 = HK80*P(0,1) + HK81*P(1,1) + HK82*P(1,2) + HK83*P(1,3) + HK84*P(1,4) + HK85*P(1,5) + HK86*P(1,6); -const float HK95 = HK4/(HK43*HK74*HK90 + HK43*HK77*HK89 + HK43*HK79*HK88 + HK43*HK80*HK87 + HK66*HK92*HK94 + HK68*HK91*HK92 + HK70*HK92*HK93 + R_LOS); +const float HK0 = Tbs(1,0)*q2; +const float HK1 = Tbs(1,1)*q1; +const float HK2 = Tbs(1,1)*q3; +const float HK3 = Tbs(1,2)*q2; +const float HK4 = Tbs(1,0)*q3; +const float HK5 = Tbs(1,2)*q1; +const float HK6 = -HK5; +const float HK7 = vd*(HK0 - HK1) - ve*(HK4 + HK6) + vn*(HK2 - HK3); +const float HK8 = 1.0F/range; +const float HK9 = 2*HK8; +const float HK10 = Tbs(1,1)*q2; +const float HK11 = Tbs(1,2)*q3; +const float HK12 = Tbs(1,1)*q0; +const float HK13 = Tbs(1,2)*q0; +const float HK14 = vd*(HK12 + HK4 - 2*HK5) - ve*(-HK0 + 2*HK1 + HK13) + vn*(HK10 + HK11); +const float HK15 = Tbs(1,0)*q1; +const float HK16 = Tbs(1,0)*q0; +const float HK17 = -vd*(HK16 - HK2 + 2*HK3) + ve*(HK11 + HK15) + vn*(-2*HK0 + HK1 + HK13); +const float HK18 = vd*(HK10 + HK15) + ve*(HK16 - 2*HK2 + HK3) - vn*(HK12 + 2*HK4 + HK6); +const float HK19 = q0*q2; +const float HK20 = q1*q3; +const float HK21 = HK19 + HK20; +const float HK22 = 2*Tbs(1,2); +const float HK23 = q0*q3; +const float HK24 = q1*q2; +const float HK25 = HK23 - HK24; +const float HK26 = 2*Tbs(1,1); +const float HK27 = 2*powf(q2, 2); +const float HK28 = 2*powf(q3, 2); +const float HK29 = HK28 - 1; +const float HK30 = HK27 + HK29; +const float HK31 = -HK21*HK22 + HK25*HK26 + HK30*Tbs(1,0); +const float HK32 = HK23 + HK24; +const float HK33 = 2*Tbs(1,0); +const float HK34 = q0*q1; +const float HK35 = q2*q3; +const float HK36 = HK34 - HK35; +const float HK37 = 2*powf(q1, 2); +const float HK38 = HK29 + HK37; +const float HK39 = HK22*HK36 - HK32*HK33 + HK38*Tbs(1,1); +const float HK40 = HK34 + HK35; +const float HK41 = HK19 - HK20; +const float HK42 = HK27 + HK37 - 1; +const float HK43 = -HK26*HK40 + HK33*HK41 + HK42*Tbs(1,2); +const float HK44 = 2*HK7; +const float HK45 = 2*HK14; +const float HK46 = 2*HK17; +const float HK47 = 2*HK18; +const float HK48 = -HK31*P(0,4) - HK39*P(0,5) - HK43*P(0,6) - HK44*P(0,0) + HK45*P(0,1) + HK46*P(0,2) + HK47*P(0,3); +const float HK49 = powf(range, -2); +const float HK50 = -HK31*P(4,6) - HK39*P(5,6) - HK43*P(6,6) - HK44*P(0,6) + HK45*P(1,6) + HK46*P(2,6) + HK47*P(3,6); +const float HK51 = -HK31*P(4,5) - HK39*P(5,5) - HK43*P(5,6) - HK44*P(0,5) + HK45*P(1,5) + HK46*P(2,5) + HK47*P(3,5); +const float HK52 = -HK31*P(4,4) - HK39*P(4,5) - HK43*P(4,6) - HK44*P(0,4) + HK45*P(1,4) + HK46*P(2,4) + HK47*P(3,4); +const float HK53 = -HK31*P(3,4) - HK39*P(3,5) - HK43*P(3,6) - HK44*P(0,3) + HK45*P(1,3) + HK46*P(2,3) + HK47*P(3,3); +const float HK54 = -HK31*P(2,4) - HK39*P(2,5) - HK43*P(2,6) - HK44*P(0,2) + HK45*P(1,2) + HK46*P(2,2) + HK47*P(2,3); +const float HK55 = -HK31*P(1,4) - HK39*P(1,5) - HK43*P(1,6) - HK44*P(0,1) + HK45*P(1,1) + HK46*P(1,2) + HK47*P(1,3); +const float HK56 = HK8/(-HK31*HK49*HK52 - HK39*HK49*HK51 - HK43*HK49*HK50 - HK44*HK48*HK49 + HK45*HK49*HK55 + HK46*HK49*HK54 + HK47*HK49*HK53 + R_LOS); +const float HK57 = Tbs(0,0)*q3; +const float HK58 = Tbs(0,2)*q1; +const float HK59 = -HK58; +const float HK60 = ve*(HK57 + HK59); +const float HK61 = Tbs(0,0)*q2; +const float HK62 = Tbs(0,1)*q1; +const float HK63 = Tbs(0,1)*q3; +const float HK64 = Tbs(0,2)*q2; +const float HK65 = HK60 - vd*(HK61 - HK62) - vn*(HK63 - HK64); +const float HK66 = Tbs(0,2)*q0; +const float HK67 = -HK61; +const float HK68 = 2*HK62; +const float HK69 = Tbs(0,1)*q0; +const float HK70 = Tbs(0,1)*q2; +const float HK71 = Tbs(0,2)*q3; +const float HK72 = vd*(HK57 - 2*HK58 + HK69) + vn*(HK70 + HK71); +const float HK73 = HK72 - ve*(HK66 + HK67 + HK68); +const float HK74 = Tbs(0,0)*q0; +const float HK75 = -HK63; +const float HK76 = 2*HK64; +const float HK77 = Tbs(0,0)*q1; +const float HK78 = ve*(HK71 + HK77) + vn*(-2*HK61 + HK62 + HK66); +const float HK79 = HK78 - vd*(HK74 + HK75 + HK76); +const float HK80 = 2*HK57; +const float HK81 = vd*(HK70 + HK77) + ve*(-2*HK63 + HK64 + HK74); +const float HK82 = HK81 - vn*(HK59 + HK69 + HK80); +const float HK83 = 2*Tbs(0,2); +const float HK84 = HK21*HK83; +const float HK85 = 2*Tbs(0,1); +const float HK86 = -HK25*HK85 - HK30*Tbs(0,0) + HK84; +const float HK87 = 2*Tbs(0,0); +const float HK88 = HK32*HK87; +const float HK89 = -HK36*HK83 - HK38*Tbs(0,1) + HK88; +const float HK90 = HK40*HK85; +const float HK91 = -HK41*HK87 - HK42*Tbs(0,2) + HK90; +const float HK92 = 2*HK60 + 2*vd*(HK62 + HK67) + 2*vn*(HK64 + HK75); +const float HK93 = -HK27; +const float HK94 = 1 - HK28; +const float HK95 = HK84 + HK85*(-HK23 + HK24) + Tbs(0,0)*(HK93 + HK94); +const float HK96 = -HK37; +const float HK97 = HK83*(-HK34 + HK35) + HK88 + Tbs(0,1)*(HK94 + HK96); +const float HK98 = HK87*(-HK19 + HK20) + HK90 + Tbs(0,2)*(HK93 + HK96 + 1); +const float HK99 = 2*HK72 + 2*ve*(HK61 - HK66 - HK68); +const float HK100 = 2*HK78 + 2*vd*(HK63 - HK74 - HK76); +const float HK101 = 2*HK81 + 2*vn*(HK58 - HK69 - HK80); +const float HK102 = HK100*P(0,2) + HK101*P(0,3) + HK92*P(0,0) + HK95*P(0,4) + HK97*P(0,5) + HK98*P(0,6) + HK99*P(0,1); +const float HK103 = 2*HK49; +const float HK104 = HK100*P(2,6) + HK101*P(3,6) + HK92*P(0,6) + HK95*P(4,6) + HK97*P(5,6) + HK98*P(6,6) + HK99*P(1,6); +const float HK105 = HK100*P(2,5) + HK101*P(3,5) + HK92*P(0,5) + HK95*P(4,5) + HK97*P(5,5) + HK98*P(5,6) + HK99*P(1,5); +const float HK106 = HK100*P(2,4) + HK101*P(3,4) + HK92*P(0,4) + HK95*P(4,4) + HK97*P(4,5) + HK98*P(4,6) + HK99*P(1,4); +const float HK107 = HK100*P(2,3) + HK101*P(3,3) + HK92*P(0,3) + HK95*P(3,4) + HK97*P(3,5) + HK98*P(3,6) + HK99*P(1,3); +const float HK108 = HK100*P(2,2) + HK101*P(2,3) + HK92*P(0,2) + HK95*P(2,4) + HK97*P(2,5) + HK98*P(2,6) + HK99*P(1,2); +const float HK109 = HK100*P(1,2) + HK101*P(1,3) + HK92*P(0,1) + HK95*P(1,4) + HK97*P(1,5) + HK98*P(1,6) + HK99*P(1,1); +const float HK110 = HK8/(HK102*HK103*HK65 + HK103*HK107*HK82 + HK103*HK108*HK79 + HK103*HK109*HK73 + HK104*HK49*HK91 + HK105*HK49*HK89 + HK106*HK49*HK86 + R_LOS); // Observation Jacobians - axis 0 -Hfusion.at<0>() = HK3*HK5; -Hfusion.at<1>() = HK5*HK7; -Hfusion.at<2>() = HK5*HK8; -Hfusion.at<3>() = HK5*HK9; -Hfusion.at<4>() = HK25*HK4; -Hfusion.at<5>() = HK33*HK4; -Hfusion.at<6>() = HK37*HK4; +Hfusion.at<0>() = -HK7*HK9; +Hfusion.at<1>() = HK14*HK9; +Hfusion.at<2>() = HK17*HK9; +Hfusion.at<3>() = HK18*HK9; +Hfusion.at<4>() = -HK31*HK8; +Hfusion.at<5>() = -HK39*HK8; +Hfusion.at<6>() = -HK43*HK8; Hfusion.at<7>() = 0; Hfusion.at<8>() = 0; Hfusion.at<9>() = 0; @@ -125,40 +140,40 @@ Hfusion.at<23>() = 0; // Kalman gains - axis 0 -Kfusion(0) = HK42*HK50; -Kfusion(1) = HK49*HK50; -Kfusion(2) = HK47*HK50; -Kfusion(3) = HK48*HK50; -Kfusion(4) = HK46*HK50; -Kfusion(5) = HK45*HK50; -Kfusion(6) = HK44*HK50; -Kfusion(7) = HK50*(HK25*P(4,7) + HK33*P(5,7) + HK37*P(6,7) + HK38*P(0,7) + HK39*P(1,7) + HK40*P(2,7) + HK41*P(3,7)); -Kfusion(8) = HK50*(HK25*P(4,8) + HK33*P(5,8) + HK37*P(6,8) + HK38*P(0,8) + HK39*P(1,8) + HK40*P(2,8) + HK41*P(3,8)); -Kfusion(9) = HK50*(HK25*P(4,9) + HK33*P(5,9) + HK37*P(6,9) + HK38*P(0,9) + HK39*P(1,9) + HK40*P(2,9) + HK41*P(3,9)); -Kfusion(10) = HK50*(HK25*P(4,10) + HK33*P(5,10) + HK37*P(6,10) + HK38*P(0,10) + HK39*P(1,10) + HK40*P(2,10) + HK41*P(3,10)); -Kfusion(11) = HK50*(HK25*P(4,11) + HK33*P(5,11) + HK37*P(6,11) + HK38*P(0,11) + HK39*P(1,11) + HK40*P(2,11) + HK41*P(3,11)); -Kfusion(12) = HK50*(HK25*P(4,12) + HK33*P(5,12) + HK37*P(6,12) + HK38*P(0,12) + HK39*P(1,12) + HK40*P(2,12) + HK41*P(3,12)); -Kfusion(13) = HK50*(HK25*P(4,13) + HK33*P(5,13) + HK37*P(6,13) + HK38*P(0,13) + HK39*P(1,13) + HK40*P(2,13) + HK41*P(3,13)); -Kfusion(14) = HK50*(HK25*P(4,14) + HK33*P(5,14) + HK37*P(6,14) + HK38*P(0,14) + HK39*P(1,14) + HK40*P(2,14) + HK41*P(3,14)); -Kfusion(15) = HK50*(HK25*P(4,15) + HK33*P(5,15) + HK37*P(6,15) + HK38*P(0,15) + HK39*P(1,15) + HK40*P(2,15) + HK41*P(3,15)); -Kfusion(16) = HK50*(HK25*P(4,16) + HK33*P(5,16) + HK37*P(6,16) + HK38*P(0,16) + HK39*P(1,16) + HK40*P(2,16) + HK41*P(3,16)); -Kfusion(17) = HK50*(HK25*P(4,17) + HK33*P(5,17) + HK37*P(6,17) + HK38*P(0,17) + HK39*P(1,17) + HK40*P(2,17) + HK41*P(3,17)); -Kfusion(18) = HK50*(HK25*P(4,18) + HK33*P(5,18) + HK37*P(6,18) + HK38*P(0,18) + HK39*P(1,18) + HK40*P(2,18) + HK41*P(3,18)); -Kfusion(19) = HK50*(HK25*P(4,19) + HK33*P(5,19) + HK37*P(6,19) + HK38*P(0,19) + HK39*P(1,19) + HK40*P(2,19) + HK41*P(3,19)); -Kfusion(20) = HK50*(HK25*P(4,20) + HK33*P(5,20) + HK37*P(6,20) + HK38*P(0,20) + HK39*P(1,20) + HK40*P(2,20) + HK41*P(3,20)); -Kfusion(21) = HK50*(HK25*P(4,21) + HK33*P(5,21) + HK37*P(6,21) + HK38*P(0,21) + HK39*P(1,21) + HK40*P(2,21) + HK41*P(3,21)); -Kfusion(22) = HK50*(HK25*P(4,22) + HK33*P(5,22) + HK37*P(6,22) + HK38*P(0,22) + HK39*P(1,22) + HK40*P(2,22) + HK41*P(3,22)); -Kfusion(23) = HK50*(HK25*P(4,23) + HK33*P(5,23) + HK37*P(6,23) + HK38*P(0,23) + HK39*P(1,23) + HK40*P(2,23) + HK41*P(3,23)); +Kfusion(0) = HK48*HK56; +Kfusion(1) = HK55*HK56; +Kfusion(2) = HK54*HK56; +Kfusion(3) = HK53*HK56; +Kfusion(4) = HK52*HK56; +Kfusion(5) = HK51*HK56; +Kfusion(6) = HK50*HK56; +Kfusion(7) = HK56*(-HK31*P(4,7) - HK39*P(5,7) - HK43*P(6,7) - HK44*P(0,7) + HK45*P(1,7) + HK46*P(2,7) + HK47*P(3,7)); +Kfusion(8) = HK56*(-HK31*P(4,8) - HK39*P(5,8) - HK43*P(6,8) - HK44*P(0,8) + HK45*P(1,8) + HK46*P(2,8) + HK47*P(3,8)); +Kfusion(9) = HK56*(-HK31*P(4,9) - HK39*P(5,9) - HK43*P(6,9) - HK44*P(0,9) + HK45*P(1,9) + HK46*P(2,9) + HK47*P(3,9)); +Kfusion(10) = HK56*(-HK31*P(4,10) - HK39*P(5,10) - HK43*P(6,10) - HK44*P(0,10) + HK45*P(1,10) + HK46*P(2,10) + HK47*P(3,10)); +Kfusion(11) = HK56*(-HK31*P(4,11) - HK39*P(5,11) - HK43*P(6,11) - HK44*P(0,11) + HK45*P(1,11) + HK46*P(2,11) + HK47*P(3,11)); +Kfusion(12) = HK56*(-HK31*P(4,12) - HK39*P(5,12) - HK43*P(6,12) - HK44*P(0,12) + HK45*P(1,12) + HK46*P(2,12) + HK47*P(3,12)); +Kfusion(13) = HK56*(-HK31*P(4,13) - HK39*P(5,13) - HK43*P(6,13) - HK44*P(0,13) + HK45*P(1,13) + HK46*P(2,13) + HK47*P(3,13)); +Kfusion(14) = HK56*(-HK31*P(4,14) - HK39*P(5,14) - HK43*P(6,14) - HK44*P(0,14) + HK45*P(1,14) + HK46*P(2,14) + HK47*P(3,14)); +Kfusion(15) = HK56*(-HK31*P(4,15) - HK39*P(5,15) - HK43*P(6,15) - HK44*P(0,15) + HK45*P(1,15) + HK46*P(2,15) + HK47*P(3,15)); +Kfusion(16) = HK56*(-HK31*P(4,16) - HK39*P(5,16) - HK43*P(6,16) - HK44*P(0,16) + HK45*P(1,16) + HK46*P(2,16) + HK47*P(3,16)); +Kfusion(17) = HK56*(-HK31*P(4,17) - HK39*P(5,17) - HK43*P(6,17) - HK44*P(0,17) + HK45*P(1,17) + HK46*P(2,17) + HK47*P(3,17)); +Kfusion(18) = HK56*(-HK31*P(4,18) - HK39*P(5,18) - HK43*P(6,18) - HK44*P(0,18) + HK45*P(1,18) + HK46*P(2,18) + HK47*P(3,18)); +Kfusion(19) = HK56*(-HK31*P(4,19) - HK39*P(5,19) - HK43*P(6,19) - HK44*P(0,19) + HK45*P(1,19) + HK46*P(2,19) + HK47*P(3,19)); +Kfusion(20) = HK56*(-HK31*P(4,20) - HK39*P(5,20) - HK43*P(6,20) - HK44*P(0,20) + HK45*P(1,20) + HK46*P(2,20) + HK47*P(3,20)); +Kfusion(21) = HK56*(-HK31*P(4,21) - HK39*P(5,21) - HK43*P(6,21) - HK44*P(0,21) + HK45*P(1,21) + HK46*P(2,21) + HK47*P(3,21)); +Kfusion(22) = HK56*(-HK31*P(4,22) - HK39*P(5,22) - HK43*P(6,22) - HK44*P(0,22) + HK45*P(1,22) + HK46*P(2,22) + HK47*P(3,22)); +Kfusion(23) = HK56*(-HK31*P(4,23) - HK39*P(5,23) - HK43*P(6,23) - HK44*P(0,23) + HK45*P(1,23) + HK46*P(2,23) + HK47*P(3,23)); // Observation Jacobians - axis 1 -Hfusion.at<0>() = -HK5*HK63; -Hfusion.at<1>() = -HK5*HK66; -Hfusion.at<2>() = -HK5*HK68; -Hfusion.at<3>() = -HK5*HK70; -Hfusion.at<4>() = -HK4*HK74; -Hfusion.at<5>() = -HK4*HK77; -Hfusion.at<6>() = -HK4*HK79; +Hfusion.at<0>() = -HK65*HK9; +Hfusion.at<1>() = -HK73*HK9; +Hfusion.at<2>() = -HK79*HK9; +Hfusion.at<3>() = -HK82*HK9; +Hfusion.at<4>() = -HK8*HK86; +Hfusion.at<5>() = -HK8*HK89; +Hfusion.at<6>() = -HK8*HK91; Hfusion.at<7>() = 0; Hfusion.at<8>() = 0; Hfusion.at<9>() = 0; @@ -179,29 +194,29 @@ Hfusion.at<23>() = 0; // Kalman gains - axis 1 -Kfusion(0) = -HK87*HK95; -Kfusion(1) = -HK94*HK95; -Kfusion(2) = -HK91*HK95; -Kfusion(3) = -HK93*HK95; -Kfusion(4) = -HK90*HK95; -Kfusion(5) = -HK89*HK95; -Kfusion(6) = -HK88*HK95; -Kfusion(7) = -HK95*(HK80*P(0,7) + HK81*P(1,7) + HK82*P(2,7) + HK83*P(3,7) + HK84*P(4,7) + HK85*P(5,7) + HK86*P(6,7)); -Kfusion(8) = -HK95*(HK80*P(0,8) + HK81*P(1,8) + HK82*P(2,8) + HK83*P(3,8) + HK84*P(4,8) + HK85*P(5,8) + HK86*P(6,8)); -Kfusion(9) = -HK95*(HK80*P(0,9) + HK81*P(1,9) + HK82*P(2,9) + HK83*P(3,9) + HK84*P(4,9) + HK85*P(5,9) + HK86*P(6,9)); -Kfusion(10) = -HK95*(HK80*P(0,10) + HK81*P(1,10) + HK82*P(2,10) + HK83*P(3,10) + HK84*P(4,10) + HK85*P(5,10) + HK86*P(6,10)); -Kfusion(11) = -HK95*(HK80*P(0,11) + HK81*P(1,11) + HK82*P(2,11) + HK83*P(3,11) + HK84*P(4,11) + HK85*P(5,11) + HK86*P(6,11)); -Kfusion(12) = -HK95*(HK80*P(0,12) + HK81*P(1,12) + HK82*P(2,12) + HK83*P(3,12) + HK84*P(4,12) + HK85*P(5,12) + HK86*P(6,12)); -Kfusion(13) = -HK95*(HK80*P(0,13) + HK81*P(1,13) + HK82*P(2,13) + HK83*P(3,13) + HK84*P(4,13) + HK85*P(5,13) + HK86*P(6,13)); -Kfusion(14) = -HK95*(HK80*P(0,14) + HK81*P(1,14) + HK82*P(2,14) + HK83*P(3,14) + HK84*P(4,14) + HK85*P(5,14) + HK86*P(6,14)); -Kfusion(15) = -HK95*(HK80*P(0,15) + HK81*P(1,15) + HK82*P(2,15) + HK83*P(3,15) + HK84*P(4,15) + HK85*P(5,15) + HK86*P(6,15)); -Kfusion(16) = -HK95*(HK80*P(0,16) + HK81*P(1,16) + HK82*P(2,16) + HK83*P(3,16) + HK84*P(4,16) + HK85*P(5,16) + HK86*P(6,16)); -Kfusion(17) = -HK95*(HK80*P(0,17) + HK81*P(1,17) + HK82*P(2,17) + HK83*P(3,17) + HK84*P(4,17) + HK85*P(5,17) + HK86*P(6,17)); -Kfusion(18) = -HK95*(HK80*P(0,18) + HK81*P(1,18) + HK82*P(2,18) + HK83*P(3,18) + HK84*P(4,18) + HK85*P(5,18) + HK86*P(6,18)); -Kfusion(19) = -HK95*(HK80*P(0,19) + HK81*P(1,19) + HK82*P(2,19) + HK83*P(3,19) + HK84*P(4,19) + HK85*P(5,19) + HK86*P(6,19)); -Kfusion(20) = -HK95*(HK80*P(0,20) + HK81*P(1,20) + HK82*P(2,20) + HK83*P(3,20) + HK84*P(4,20) + HK85*P(5,20) + HK86*P(6,20)); -Kfusion(21) = -HK95*(HK80*P(0,21) + HK81*P(1,21) + HK82*P(2,21) + HK83*P(3,21) + HK84*P(4,21) + HK85*P(5,21) + HK86*P(6,21)); -Kfusion(22) = -HK95*(HK80*P(0,22) + HK81*P(1,22) + HK82*P(2,22) + HK83*P(3,22) + HK84*P(4,22) + HK85*P(5,22) + HK86*P(6,22)); -Kfusion(23) = -HK95*(HK80*P(0,23) + HK81*P(1,23) + HK82*P(2,23) + HK83*P(3,23) + HK84*P(4,23) + HK85*P(5,23) + HK86*P(6,23)); +Kfusion(0) = -HK102*HK110; +Kfusion(1) = -HK109*HK110; +Kfusion(2) = -HK108*HK110; +Kfusion(3) = -HK107*HK110; +Kfusion(4) = -HK106*HK110; +Kfusion(5) = -HK105*HK110; +Kfusion(6) = -HK104*HK110; +Kfusion(7) = -HK110*(HK100*P(2,7) + HK101*P(3,7) + HK92*P(0,7) + HK95*P(4,7) + HK97*P(5,7) + HK98*P(6,7) + HK99*P(1,7)); +Kfusion(8) = -HK110*(HK100*P(2,8) + HK101*P(3,8) + HK92*P(0,8) + HK95*P(4,8) + HK97*P(5,8) + HK98*P(6,8) + HK99*P(1,8)); +Kfusion(9) = -HK110*(HK100*P(2,9) + HK101*P(3,9) + HK92*P(0,9) + HK95*P(4,9) + HK97*P(5,9) + HK98*P(6,9) + HK99*P(1,9)); +Kfusion(10) = -HK110*(HK100*P(2,10) + HK101*P(3,10) + HK92*P(0,10) + HK95*P(4,10) + HK97*P(5,10) + HK98*P(6,10) + HK99*P(1,10)); +Kfusion(11) = -HK110*(HK100*P(2,11) + HK101*P(3,11) + HK92*P(0,11) + HK95*P(4,11) + HK97*P(5,11) + HK98*P(6,11) + HK99*P(1,11)); +Kfusion(12) = -HK110*(HK100*P(2,12) + HK101*P(3,12) + HK92*P(0,12) + HK95*P(4,12) + HK97*P(5,12) + HK98*P(6,12) + HK99*P(1,12)); +Kfusion(13) = -HK110*(HK100*P(2,13) + HK101*P(3,13) + HK92*P(0,13) + HK95*P(4,13) + HK97*P(5,13) + HK98*P(6,13) + HK99*P(1,13)); +Kfusion(14) = -HK110*(HK100*P(2,14) + HK101*P(3,14) + HK92*P(0,14) + HK95*P(4,14) + HK97*P(5,14) + HK98*P(6,14) + HK99*P(1,14)); +Kfusion(15) = -HK110*(HK100*P(2,15) + HK101*P(3,15) + HK92*P(0,15) + HK95*P(4,15) + HK97*P(5,15) + HK98*P(6,15) + HK99*P(1,15)); +Kfusion(16) = -HK110*(HK100*P(2,16) + HK101*P(3,16) + HK92*P(0,16) + HK95*P(4,16) + HK97*P(5,16) + HK98*P(6,16) + HK99*P(1,16)); +Kfusion(17) = -HK110*(HK100*P(2,17) + HK101*P(3,17) + HK92*P(0,17) + HK95*P(4,17) + HK97*P(5,17) + HK98*P(6,17) + HK99*P(1,17)); +Kfusion(18) = -HK110*(HK100*P(2,18) + HK101*P(3,18) + HK92*P(0,18) + HK95*P(4,18) + HK97*P(5,18) + HK98*P(6,18) + HK99*P(1,18)); +Kfusion(19) = -HK110*(HK100*P(2,19) + HK101*P(3,19) + HK92*P(0,19) + HK95*P(4,19) + HK97*P(5,19) + HK98*P(6,19) + HK99*P(1,19)); +Kfusion(20) = -HK110*(HK100*P(2,20) + HK101*P(3,20) + HK92*P(0,20) + HK95*P(4,20) + HK97*P(5,20) + HK98*P(6,20) + HK99*P(1,20)); +Kfusion(21) = -HK110*(HK100*P(2,21) + HK101*P(3,21) + HK92*P(0,21) + HK95*P(4,21) + HK97*P(5,21) + HK98*P(6,21) + HK99*P(1,21)); +Kfusion(22) = -HK110*(HK100*P(2,22) + HK101*P(3,22) + HK92*P(0,22) + HK95*P(4,22) + HK97*P(5,22) + HK98*P(6,22) + HK99*P(1,22)); +Kfusion(23) = -HK110*(HK100*P(2,23) + HK101*P(3,23) + HK92*P(0,23) + HK95*P(4,23) + HK97*P(5,23) + HK98*P(6,23) + HK99*P(1,23)); diff --git a/EKF/python/ekf_derivation/generated/gps_yaw_generated.cpp b/EKF/python/ekf_derivation/generated/gps_yaw_generated.cpp index 5e4aa32240..f63b262958 100644 --- a/EKF/python/ekf_derivation/generated/gps_yaw_generated.cpp +++ b/EKF/python/ekf_derivation/generated/gps_yaw_generated.cpp @@ -1,44 +1,38 @@ // Sub Expressions -const float HK0 = sinf(ant_yaw); -const float HK1 = q0*q3; -const float HK2 = q1*q2; -const float HK3 = 2*HK0*(HK1 - HK2); -const float HK4 = cosf(ant_yaw); -const float HK5 = powf(q1, 2); -const float HK6 = powf(q2, 2); -const float HK7 = powf(q0, 2) - powf(q3, 2); -const float HK8 = HK4*(HK5 - HK6 + HK7); -const float HK9 = HK3 - HK8; -const float HK10 = 1.0F/HK9; -const float HK11 = HK4*q0; -const float HK12 = HK0*q3; -const float HK13 = HK0*(-HK5 + HK6 + HK7) + 2*HK4*(HK1 + HK2); -const float HK14 = HK10*HK13; -const float HK15 = HK0*q0 + HK4*q3; -const float HK16 = HK10*(HK14*(HK11 - HK12) + HK15); -const float HK17 = powf(HK13, 2)/powf(HK9, 2) + 1; -const float HK18 = 2/HK17; -const float HK19 = 1.0F/(-HK3 + HK8); -const float HK20 = HK4*q1; -const float HK21 = HK0*q2; -const float HK22 = HK13*HK19; -const float HK23 = HK0*q1 - HK4*q2; -const float HK24 = HK19*(HK22*(HK20 + HK21) + HK23); -const float HK25 = HK19*(-HK20 - HK21 + HK22*HK23); -const float HK26 = HK10*(-HK11 + HK12 + HK14*HK15); -const float HK27 = -HK16*P(0,0) - HK24*P(0,1) - HK25*P(0,2) + HK26*P(0,3); -const float HK28 = -HK16*P(0,1) - HK24*P(1,1) - HK25*P(1,2) + HK26*P(1,3); -const float HK29 = 4/powf(HK17, 2); -const float HK30 = -HK16*P(0,2) - HK24*P(1,2) - HK25*P(2,2) + HK26*P(2,3); -const float HK31 = -HK16*P(0,3) - HK24*P(1,3) - HK25*P(2,3) + HK26*P(3,3); -const float HK32 = HK18/(-HK16*HK27*HK29 - HK24*HK28*HK29 - HK25*HK29*HK30 + HK26*HK29*HK31 + R_YAW); +const float HK0 = cosf(ant_yaw); +const float HK1 = sinf(ant_yaw); +const float HK2 = q0*q3; +const float HK3 = q1*q2; +const float HK4 = 2*HK1; +const float HK5 = 2*powf(q3, 2) - 1; +const float HK6 = HK0*(HK5 + 2*powf(q2, 2)) + HK4*(HK2 - HK3); +const float HK7 = 1.0F/HK6; +const float HK8 = 2*HK0; +const float HK9 = -HK1*(HK5 + 2*powf(q1, 2)) + HK8*(HK2 + HK3); +const float HK10 = HK7*HK9; +const float HK11 = HK1*HK10; +const float HK12 = q3*(-HK0 + HK11); +const float HK13 = powf(HK6, -2); +const float HK14 = HK13*powf(HK9, 2) + 1; +const float HK15 = 2*HK7/HK14; +const float HK16 = HK0*q2; +const float HK17 = HK1*q1; +const float HK18 = HK11*q2 + HK16 - 2*HK17; +const float HK19 = HK0*q1 + HK10*(-2*HK16 + HK17); +const float HK20 = -HK0*q0 + HK10*(HK1*q0 + HK8*q3) + HK4*q3; +const float HK21 = HK12*P(0,0) - HK18*P(0,1) - HK19*P(0,2) + HK20*P(0,3); +const float HK22 = 4*HK13/powf(HK14, 2); +const float HK23 = HK12*P(0,1) - HK18*P(1,1) - HK19*P(1,2) + HK20*P(1,3); +const float HK24 = HK12*P(0,2) - HK18*P(1,2) - HK19*P(2,2) + HK20*P(2,3); +const float HK25 = HK12*P(0,3) - HK18*P(1,3) - HK19*P(2,3) + HK20*P(3,3); +const float HK26 = HK15/(HK12*HK21*HK22 - HK18*HK22*HK23 - HK19*HK22*HK24 + HK20*HK22*HK25 + R_YAW); // Observation Jacobians -Hfusion.at<0>() = -HK16*HK18; -Hfusion.at<1>() = -HK18*HK24; -Hfusion.at<2>() = -HK18*HK25; -Hfusion.at<3>() = HK18*HK26; +Hfusion.at<0>() = HK12*HK15; +Hfusion.at<1>() = -HK15*HK18; +Hfusion.at<2>() = -HK15*HK19; +Hfusion.at<3>() = HK15*HK20; Hfusion.at<4>() = 0; Hfusion.at<5>() = 0; Hfusion.at<6>() = 0; @@ -62,29 +56,29 @@ Hfusion.at<23>() = 0; // Kalman gains -Kfusion(0) = HK27*HK32; -Kfusion(1) = HK28*HK32; -Kfusion(2) = HK30*HK32; -Kfusion(3) = HK31*HK32; -Kfusion(4) = HK32*(-HK16*P(0,4) - HK24*P(1,4) - HK25*P(2,4) + HK26*P(3,4)); -Kfusion(5) = HK32*(-HK16*P(0,5) - HK24*P(1,5) - HK25*P(2,5) + HK26*P(3,5)); -Kfusion(6) = HK32*(-HK16*P(0,6) - HK24*P(1,6) - HK25*P(2,6) + HK26*P(3,6)); -Kfusion(7) = HK32*(-HK16*P(0,7) - HK24*P(1,7) - HK25*P(2,7) + HK26*P(3,7)); -Kfusion(8) = HK32*(-HK16*P(0,8) - HK24*P(1,8) - HK25*P(2,8) + HK26*P(3,8)); -Kfusion(9) = HK32*(-HK16*P(0,9) - HK24*P(1,9) - HK25*P(2,9) + HK26*P(3,9)); -Kfusion(10) = HK32*(-HK16*P(0,10) - HK24*P(1,10) - HK25*P(2,10) + HK26*P(3,10)); -Kfusion(11) = HK32*(-HK16*P(0,11) - HK24*P(1,11) - HK25*P(2,11) + HK26*P(3,11)); -Kfusion(12) = HK32*(-HK16*P(0,12) - HK24*P(1,12) - HK25*P(2,12) + HK26*P(3,12)); -Kfusion(13) = HK32*(-HK16*P(0,13) - HK24*P(1,13) - HK25*P(2,13) + HK26*P(3,13)); -Kfusion(14) = HK32*(-HK16*P(0,14) - HK24*P(1,14) - HK25*P(2,14) + HK26*P(3,14)); -Kfusion(15) = HK32*(-HK16*P(0,15) - HK24*P(1,15) - HK25*P(2,15) + HK26*P(3,15)); -Kfusion(16) = HK32*(-HK16*P(0,16) - HK24*P(1,16) - HK25*P(2,16) + HK26*P(3,16)); -Kfusion(17) = HK32*(-HK16*P(0,17) - HK24*P(1,17) - HK25*P(2,17) + HK26*P(3,17)); -Kfusion(18) = HK32*(-HK16*P(0,18) - HK24*P(1,18) - HK25*P(2,18) + HK26*P(3,18)); -Kfusion(19) = HK32*(-HK16*P(0,19) - HK24*P(1,19) - HK25*P(2,19) + HK26*P(3,19)); -Kfusion(20) = HK32*(-HK16*P(0,20) - HK24*P(1,20) - HK25*P(2,20) + HK26*P(3,20)); -Kfusion(21) = HK32*(-HK16*P(0,21) - HK24*P(1,21) - HK25*P(2,21) + HK26*P(3,21)); -Kfusion(22) = HK32*(-HK16*P(0,22) - HK24*P(1,22) - HK25*P(2,22) + HK26*P(3,22)); -Kfusion(23) = HK32*(-HK16*P(0,23) - HK24*P(1,23) - HK25*P(2,23) + HK26*P(3,23)); +Kfusion(0) = HK21*HK26; +Kfusion(1) = HK23*HK26; +Kfusion(2) = HK24*HK26; +Kfusion(3) = HK25*HK26; +Kfusion(4) = HK26*(HK12*P(0,4) - HK18*P(1,4) - HK19*P(2,4) + HK20*P(3,4)); +Kfusion(5) = HK26*(HK12*P(0,5) - HK18*P(1,5) - HK19*P(2,5) + HK20*P(3,5)); +Kfusion(6) = HK26*(HK12*P(0,6) - HK18*P(1,6) - HK19*P(2,6) + HK20*P(3,6)); +Kfusion(7) = HK26*(HK12*P(0,7) - HK18*P(1,7) - HK19*P(2,7) + HK20*P(3,7)); +Kfusion(8) = HK26*(HK12*P(0,8) - HK18*P(1,8) - HK19*P(2,8) + HK20*P(3,8)); +Kfusion(9) = HK26*(HK12*P(0,9) - HK18*P(1,9) - HK19*P(2,9) + HK20*P(3,9)); +Kfusion(10) = HK26*(HK12*P(0,10) - HK18*P(1,10) - HK19*P(2,10) + HK20*P(3,10)); +Kfusion(11) = HK26*(HK12*P(0,11) - HK18*P(1,11) - HK19*P(2,11) + HK20*P(3,11)); +Kfusion(12) = HK26*(HK12*P(0,12) - HK18*P(1,12) - HK19*P(2,12) + HK20*P(3,12)); +Kfusion(13) = HK26*(HK12*P(0,13) - HK18*P(1,13) - HK19*P(2,13) + HK20*P(3,13)); +Kfusion(14) = HK26*(HK12*P(0,14) - HK18*P(1,14) - HK19*P(2,14) + HK20*P(3,14)); +Kfusion(15) = HK26*(HK12*P(0,15) - HK18*P(1,15) - HK19*P(2,15) + HK20*P(3,15)); +Kfusion(16) = HK26*(HK12*P(0,16) - HK18*P(1,16) - HK19*P(2,16) + HK20*P(3,16)); +Kfusion(17) = HK26*(HK12*P(0,17) - HK18*P(1,17) - HK19*P(2,17) + HK20*P(3,17)); +Kfusion(18) = HK26*(HK12*P(0,18) - HK18*P(1,18) - HK19*P(2,18) + HK20*P(3,18)); +Kfusion(19) = HK26*(HK12*P(0,19) - HK18*P(1,19) - HK19*P(2,19) + HK20*P(3,19)); +Kfusion(20) = HK26*(HK12*P(0,20) - HK18*P(1,20) - HK19*P(2,20) + HK20*P(3,20)); +Kfusion(21) = HK26*(HK12*P(0,21) - HK18*P(1,21) - HK19*P(2,21) + HK20*P(3,21)); +Kfusion(22) = HK26*(HK12*P(0,22) - HK18*P(1,22) - HK19*P(2,22) + HK20*P(3,22)); +Kfusion(23) = HK26*(HK12*P(0,23) - HK18*P(1,23) - HK19*P(2,23) + HK20*P(3,23)); diff --git a/EKF/python/ekf_derivation/generated/vel_bf_generated.cpp b/EKF/python/ekf_derivation/generated/vel_bf_generated.cpp index 6e19b5a458..1f6f9a6b28 100644 --- a/EKF/python/ekf_derivation/generated/vel_bf_generated.cpp +++ b/EKF/python/ekf_derivation/generated/vel_bf_generated.cpp @@ -1,37 +1,41 @@ // axis 0 -const float HK0 = q0*vn - q2*vd + q3*ve; -const float HK1 = q1*vn + q2*ve + q3*vd; -const float HK2 = q1*ve; -const float HK3 = q0*vd; -const float HK4 = q2*vn; -const float HK5 = q0*ve + q1*vd - q3*vn; -const float HK6 = powf(q0, 2) + powf(q1, 2) - powf(q2, 2) - powf(q3, 2); -const float HK7 = q0*q3 + q1*q2; -const float HK8 = q1*q3; -const float HK9 = q0*q2; -const float HK10 = 2*HK7; -const float HK11 = -2*HK8 + 2*HK9; -const float HK12 = 2*HK1; -const float HK13 = 2*HK0; -const float HK14 = -2*HK2 + 2*HK3 + 2*HK4; -const float HK15 = 2*HK5; -const float HK16 = HK10*P(0,5) - HK11*P(0,6) + HK12*P(0,1) + HK13*P(0,0) - HK14*P(0,2) + HK15*P(0,3) + HK6*P(0,4); -const float HK17 = HK10*P(5,5) - HK11*P(5,6) + HK12*P(1,5) + HK13*P(0,5) - HK14*P(2,5) + HK15*P(3,5) + HK6*P(4,5); -const float HK18 = HK10*P(5,6) - HK11*P(6,6) + HK12*P(1,6) + HK13*P(0,6) - HK14*P(2,6) + HK15*P(3,6) + HK6*P(4,6); -const float HK19 = HK10*P(1,5) - HK11*P(1,6) + HK12*P(1,1) + HK13*P(0,1) - HK14*P(1,2) + HK15*P(1,3) + HK6*P(1,4); -const float HK20 = HK10*P(2,5) - HK11*P(2,6) + HK12*P(1,2) + HK13*P(0,2) - HK14*P(2,2) + HK15*P(2,3) + HK6*P(2,4); -const float HK21 = HK10*P(3,5) - HK11*P(3,6) + HK12*P(1,3) + HK13*P(0,3) - HK14*P(2,3) + HK15*P(3,3) + HK6*P(3,4); -const float HK22 = HK10*P(4,5) - HK11*P(4,6) + HK12*P(1,4) + HK13*P(0,4) - HK14*P(2,4) + HK15*P(3,4) + HK6*P(4,4); -const float HK23 = 1.0F/(HK10*HK17 - HK11*HK18 + HK12*HK19 + HK13*HK16 - HK14*HK20 + HK15*HK21 + HK22*HK6 + R_VEL); +const float HK0 = q3*ve; +const float HK1 = q2*vd; +const float HK2 = q2*ve + q3*vd; +const float HK3 = q1*ve; +const float HK4 = q0*vd; +const float HK5 = 2*vn; +const float HK6 = HK5*q2; +const float HK7 = -HK5*q3 + q0*ve + q1*vd; +const float HK8 = 2*powf(q2, 2); +const float HK9 = 2*powf(q3, 2); +const float HK10 = q0*q3 + q1*q2; +const float HK11 = q1*q3; +const float HK12 = q0*q2; +const float HK13 = 2*HK2; +const float HK14 = 2*HK10; +const float HK15 = -2*HK0 + 2*HK1; +const float HK16 = -2*HK11 + 2*HK12; +const float HK17 = 2*HK7; +const float HK18 = HK8 + HK9 - 1; +const float HK19 = -2*HK3 + 2*HK4 + 2*HK6; +const float HK20 = -HK13*P(0,1) - HK14*P(0,5) + HK15*P(0,0) + HK16*P(0,6) - HK17*P(0,3) + HK18*P(0,4) + HK19*P(0,2); +const float HK21 = -HK13*P(1,5) - HK14*P(5,5) + HK15*P(0,5) + HK16*P(5,6) - HK17*P(3,5) + HK18*P(4,5) + HK19*P(2,5); +const float HK22 = -HK13*P(1,1) - HK14*P(1,5) + HK15*P(0,1) + HK16*P(1,6) - HK17*P(1,3) + HK18*P(1,4) + HK19*P(1,2); +const float HK23 = -HK13*P(1,6) - HK14*P(5,6) + HK15*P(0,6) + HK16*P(6,6) - HK17*P(3,6) + HK18*P(4,6) + HK19*P(2,6); +const float HK24 = -HK13*P(1,4) - HK14*P(4,5) + HK15*P(0,4) + HK16*P(4,6) - HK17*P(3,4) + HK18*P(4,4) + HK19*P(2,4); +const float HK25 = -HK13*P(1,3) - HK14*P(3,5) + HK15*P(0,3) + HK16*P(3,6) - HK17*P(3,3) + HK18*P(3,4) + HK19*P(2,3); +const float HK26 = -HK13*P(1,2) - HK14*P(2,5) + HK15*P(0,2) + HK16*P(2,6) - HK17*P(2,3) + HK18*P(2,4) + HK19*P(2,2); +const float HK27 = 1.0F/(-HK13*HK22 - HK14*HK21 + HK15*HK20 + HK16*HK23 - HK17*HK25 + HK18*HK24 + HK19*HK26 + R_VEL); -H_VEL(0) = 2*HK0; -H_VEL(1) = 2*HK1; -H_VEL(2) = 2*HK2 - 2*HK3 - 2*HK4; -H_VEL(3) = 2*HK5; -H_VEL(4) = HK6; -H_VEL(5) = 2*HK7; -H_VEL(6) = 2*HK8 - 2*HK9; +H_VEL(0) = 2*HK0 - 2*HK1; +H_VEL(1) = 2*HK2; +H_VEL(2) = 2*HK3 - 2*HK4 - 2*HK6; +H_VEL(3) = 2*HK7; +H_VEL(4) = -HK8 - HK9 + 1; +H_VEL(5) = 2*HK10; +H_VEL(6) = 2*HK11 - 2*HK12; H_VEL(7) = 0; H_VEL(8) = 0; H_VEL(9) = 0; @@ -51,66 +55,69 @@ H_VEL(22) = 0; H_VEL(23) = 0; -Kfusion(0) = HK16*HK23; -Kfusion(1) = HK19*HK23; -Kfusion(2) = HK20*HK23; -Kfusion(3) = HK21*HK23; -Kfusion(4) = HK22*HK23; -Kfusion(5) = HK17*HK23; -Kfusion(6) = HK18*HK23; -Kfusion(7) = HK23*(HK10*P(5,7) - HK11*P(6,7) + HK12*P(1,7) + HK13*P(0,7) - HK14*P(2,7) + HK15*P(3,7) + HK6*P(4,7)); -Kfusion(8) = HK23*(HK10*P(5,8) - HK11*P(6,8) + HK12*P(1,8) + HK13*P(0,8) - HK14*P(2,8) + HK15*P(3,8) + HK6*P(4,8)); -Kfusion(9) = HK23*(HK10*P(5,9) - HK11*P(6,9) + HK12*P(1,9) + HK13*P(0,9) - HK14*P(2,9) + HK15*P(3,9) + HK6*P(4,9)); -Kfusion(10) = HK23*(HK10*P(5,10) - HK11*P(6,10) + HK12*P(1,10) + HK13*P(0,10) - HK14*P(2,10) + HK15*P(3,10) + HK6*P(4,10)); -Kfusion(11) = HK23*(HK10*P(5,11) - HK11*P(6,11) + HK12*P(1,11) + HK13*P(0,11) - HK14*P(2,11) + HK15*P(3,11) + HK6*P(4,11)); -Kfusion(12) = HK23*(HK10*P(5,12) - HK11*P(6,12) + HK12*P(1,12) + HK13*P(0,12) - HK14*P(2,12) + HK15*P(3,12) + HK6*P(4,12)); -Kfusion(13) = HK23*(HK10*P(5,13) - HK11*P(6,13) + HK12*P(1,13) + HK13*P(0,13) - HK14*P(2,13) + HK15*P(3,13) + HK6*P(4,13)); -Kfusion(14) = HK23*(HK10*P(5,14) - HK11*P(6,14) + HK12*P(1,14) + HK13*P(0,14) - HK14*P(2,14) + HK15*P(3,14) + HK6*P(4,14)); -Kfusion(15) = HK23*(HK10*P(5,15) - HK11*P(6,15) + HK12*P(1,15) + HK13*P(0,15) - HK14*P(2,15) + HK15*P(3,15) + HK6*P(4,15)); -Kfusion(16) = HK23*(HK10*P(5,16) - HK11*P(6,16) + HK12*P(1,16) + HK13*P(0,16) - HK14*P(2,16) + HK15*P(3,16) + HK6*P(4,16)); -Kfusion(17) = HK23*(HK10*P(5,17) - HK11*P(6,17) + HK12*P(1,17) + HK13*P(0,17) - HK14*P(2,17) + HK15*P(3,17) + HK6*P(4,17)); -Kfusion(18) = HK23*(HK10*P(5,18) - HK11*P(6,18) + HK12*P(1,18) + HK13*P(0,18) - HK14*P(2,18) + HK15*P(3,18) + HK6*P(4,18)); -Kfusion(19) = HK23*(HK10*P(5,19) - HK11*P(6,19) + HK12*P(1,19) + HK13*P(0,19) - HK14*P(2,19) + HK15*P(3,19) + HK6*P(4,19)); -Kfusion(20) = HK23*(HK10*P(5,20) - HK11*P(6,20) + HK12*P(1,20) + HK13*P(0,20) - HK14*P(2,20) + HK15*P(3,20) + HK6*P(4,20)); -Kfusion(21) = HK23*(HK10*P(5,21) - HK11*P(6,21) + HK12*P(1,21) + HK13*P(0,21) - HK14*P(2,21) + HK15*P(3,21) + HK6*P(4,21)); -Kfusion(22) = HK23*(HK10*P(5,22) - HK11*P(6,22) + HK12*P(1,22) + HK13*P(0,22) - HK14*P(2,22) + HK15*P(3,22) + HK6*P(4,22)); -Kfusion(23) = HK23*(HK10*P(5,23) - HK11*P(6,23) + HK12*P(1,23) + HK13*P(0,23) - HK14*P(2,23) + HK15*P(3,23) + HK6*P(4,23)); +Kfusion(0) = -HK20*HK27; +Kfusion(1) = -HK22*HK27; +Kfusion(2) = -HK26*HK27; +Kfusion(3) = -HK25*HK27; +Kfusion(4) = -HK24*HK27; +Kfusion(5) = -HK21*HK27; +Kfusion(6) = -HK23*HK27; +Kfusion(7) = -HK27*(-HK13*P(1,7) - HK14*P(5,7) + HK15*P(0,7) + HK16*P(6,7) - HK17*P(3,7) + HK18*P(4,7) + HK19*P(2,7)); +Kfusion(8) = -HK27*(-HK13*P(1,8) - HK14*P(5,8) + HK15*P(0,8) + HK16*P(6,8) - HK17*P(3,8) + HK18*P(4,8) + HK19*P(2,8)); +Kfusion(9) = -HK27*(-HK13*P(1,9) - HK14*P(5,9) + HK15*P(0,9) + HK16*P(6,9) - HK17*P(3,9) + HK18*P(4,9) + HK19*P(2,9)); +Kfusion(10) = -HK27*(-HK13*P(1,10) - HK14*P(5,10) + HK15*P(0,10) + HK16*P(6,10) - HK17*P(3,10) + HK18*P(4,10) + HK19*P(2,10)); +Kfusion(11) = -HK27*(-HK13*P(1,11) - HK14*P(5,11) + HK15*P(0,11) + HK16*P(6,11) - HK17*P(3,11) + HK18*P(4,11) + HK19*P(2,11)); +Kfusion(12) = -HK27*(-HK13*P(1,12) - HK14*P(5,12) + HK15*P(0,12) + HK16*P(6,12) - HK17*P(3,12) + HK18*P(4,12) + HK19*P(2,12)); +Kfusion(13) = -HK27*(-HK13*P(1,13) - HK14*P(5,13) + HK15*P(0,13) + HK16*P(6,13) - HK17*P(3,13) + HK18*P(4,13) + HK19*P(2,13)); +Kfusion(14) = -HK27*(-HK13*P(1,14) - HK14*P(5,14) + HK15*P(0,14) + HK16*P(6,14) - HK17*P(3,14) + HK18*P(4,14) + HK19*P(2,14)); +Kfusion(15) = -HK27*(-HK13*P(1,15) - HK14*P(5,15) + HK15*P(0,15) + HK16*P(6,15) - HK17*P(3,15) + HK18*P(4,15) + HK19*P(2,15)); +Kfusion(16) = -HK27*(-HK13*P(1,16) - HK14*P(5,16) + HK15*P(0,16) + HK16*P(6,16) - HK17*P(3,16) + HK18*P(4,16) + HK19*P(2,16)); +Kfusion(17) = -HK27*(-HK13*P(1,17) - HK14*P(5,17) + HK15*P(0,17) + HK16*P(6,17) - HK17*P(3,17) + HK18*P(4,17) + HK19*P(2,17)); +Kfusion(18) = -HK27*(-HK13*P(1,18) - HK14*P(5,18) + HK15*P(0,18) + HK16*P(6,18) - HK17*P(3,18) + HK18*P(4,18) + HK19*P(2,18)); +Kfusion(19) = -HK27*(-HK13*P(1,19) - HK14*P(5,19) + HK15*P(0,19) + HK16*P(6,19) - HK17*P(3,19) + HK18*P(4,19) + HK19*P(2,19)); +Kfusion(20) = -HK27*(-HK13*P(1,20) - HK14*P(5,20) + HK15*P(0,20) + HK16*P(6,20) - HK17*P(3,20) + HK18*P(4,20) + HK19*P(2,20)); +Kfusion(21) = -HK27*(-HK13*P(1,21) - HK14*P(5,21) + HK15*P(0,21) + HK16*P(6,21) - HK17*P(3,21) + HK18*P(4,21) + HK19*P(2,21)); +Kfusion(22) = -HK27*(-HK13*P(1,22) - HK14*P(5,22) + HK15*P(0,22) + HK16*P(6,22) - HK17*P(3,22) + HK18*P(4,22) + HK19*P(2,22)); +Kfusion(23) = -HK27*(-HK13*P(1,23) - HK14*P(5,23) + HK15*P(0,23) + HK16*P(6,23) - HK17*P(3,23) + HK18*P(4,23) + HK19*P(2,23)); // axis 1 -const float HK0 = q0*ve + q1*vd - q3*vn; -const float HK1 = q0*vd - q1*ve + q2*vn; -const float HK2 = q1*vn + q2*ve + q3*vd; -const float HK3 = q2*vd; -const float HK4 = q0*vn; -const float HK5 = q3*ve; -const float HK6 = q1*q2; -const float HK7 = q0*q3; -const float HK8 = powf(q0, 2) - powf(q1, 2) + powf(q2, 2) - powf(q3, 2); -const float HK9 = q0*q1 + q2*q3; -const float HK10 = 2*HK9; -const float HK11 = -2*HK6 + 2*HK7; -const float HK12 = 2*HK2; -const float HK13 = 2*HK0; -const float HK14 = 2*HK1; -const float HK15 = -2*HK3 + 2*HK4 + 2*HK5; -const float HK16 = HK10*P(0,6) - HK11*P(0,4) + HK12*P(0,2) + HK13*P(0,0) + HK14*P(0,1) - HK15*P(0,3) + HK8*P(0,5); -const float HK17 = HK10*P(6,6) - HK11*P(4,6) + HK12*P(2,6) + HK13*P(0,6) + HK14*P(1,6) - HK15*P(3,6) + HK8*P(5,6); -const float HK18 = HK10*P(4,6) - HK11*P(4,4) + HK12*P(2,4) + HK13*P(0,4) + HK14*P(1,4) - HK15*P(3,4) + HK8*P(4,5); -const float HK19 = HK10*P(2,6) - HK11*P(2,4) + HK12*P(2,2) + HK13*P(0,2) + HK14*P(1,2) - HK15*P(2,3) + HK8*P(2,5); -const float HK20 = HK10*P(1,6) - HK11*P(1,4) + HK12*P(1,2) + HK13*P(0,1) + HK14*P(1,1) - HK15*P(1,3) + HK8*P(1,5); -const float HK21 = HK10*P(3,6) - HK11*P(3,4) + HK12*P(2,3) + HK13*P(0,3) + HK14*P(1,3) - HK15*P(3,3) + HK8*P(3,5); -const float HK22 = HK10*P(5,6) - HK11*P(4,5) + HK12*P(2,5) + HK13*P(0,5) + HK14*P(1,5) - HK15*P(3,5) + HK8*P(5,5); -const float HK23 = 1.0F/(HK10*HK17 - HK11*HK18 + HK12*HK19 + HK13*HK16 + HK14*HK20 - HK15*HK21 + HK22*HK8 + R_VEL); +const float HK0 = q1*vd - q3*vn; +const float HK1 = 2*ve; +const float HK2 = -HK1*q1 + q0*vd + q2*vn; +const float HK3 = q1*vn + q3*vd; +const float HK4 = q2*vd; +const float HK5 = q0*vn; +const float HK6 = HK1*q3; +const float HK7 = q1*q2; +const float HK8 = q0*q3; +const float HK9 = 2*powf(q1, 2); +const float HK10 = 2*powf(q3, 2); +const float HK11 = q0*q1 + q2*q3; +const float HK12 = 2*HK3; +const float HK13 = 2*HK11; +const float HK14 = 2*HK0; +const float HK15 = -2*HK7 + 2*HK8; +const float HK16 = 2*HK2; +const float HK17 = -2*HK4 + 2*HK5 + 2*HK6; +const float HK18 = HK10 + HK9 - 1; +const float HK19 = HK12*P(0,2) + HK13*P(0,6) + HK14*P(0,0) - HK15*P(0,4) + HK16*P(0,1) - HK17*P(0,3) - HK18*P(0,5); +const float HK20 = HK12*P(2,6) + HK13*P(6,6) + HK14*P(0,6) - HK15*P(4,6) + HK16*P(1,6) - HK17*P(3,6) - HK18*P(5,6); +const float HK21 = HK12*P(2,2) + HK13*P(2,6) + HK14*P(0,2) - HK15*P(2,4) + HK16*P(1,2) - HK17*P(2,3) - HK18*P(2,5); +const float HK22 = HK12*P(2,4) + HK13*P(4,6) + HK14*P(0,4) - HK15*P(4,4) + HK16*P(1,4) - HK17*P(3,4) - HK18*P(4,5); +const float HK23 = HK12*P(1,2) + HK13*P(1,6) + HK14*P(0,1) - HK15*P(1,4) + HK16*P(1,1) - HK17*P(1,3) - HK18*P(1,5); +const float HK24 = HK12*P(2,5) + HK13*P(5,6) + HK14*P(0,5) - HK15*P(4,5) + HK16*P(1,5) - HK17*P(3,5) - HK18*P(5,5); +const float HK25 = HK12*P(2,3) + HK13*P(3,6) + HK14*P(0,3) - HK15*P(3,4) + HK16*P(1,3) - HK17*P(3,3) - HK18*P(3,5); +const float HK26 = 1.0F/(HK12*HK21 + HK13*HK20 + HK14*HK19 - HK15*HK22 + HK16*HK23 - HK17*HK25 - HK18*HK24 + R_VEL); H_VEL(0) = 2*HK0; -H_VEL(1) = 2*HK1; -H_VEL(2) = 2*HK2; -H_VEL(3) = 2*HK3 - 2*HK4 - 2*HK5; -H_VEL(4) = 2*HK6 - 2*HK7; -H_VEL(5) = HK8; -H_VEL(6) = 2*HK9; +H_VEL(1) = 2*HK2; +H_VEL(2) = 2*HK3; +H_VEL(3) = 2*HK4 - 2*HK5 - 2*HK6; +H_VEL(4) = 2*HK7 - 2*HK8; +H_VEL(5) = -HK10 - HK9 + 1; +H_VEL(6) = 2*HK11; H_VEL(7) = 0; H_VEL(8) = 0; H_VEL(9) = 0; @@ -130,66 +137,70 @@ H_VEL(22) = 0; H_VEL(23) = 0; -Kfusion(0) = HK16*HK23; -Kfusion(1) = HK20*HK23; -Kfusion(2) = HK19*HK23; -Kfusion(3) = HK21*HK23; -Kfusion(4) = HK18*HK23; -Kfusion(5) = HK22*HK23; -Kfusion(6) = HK17*HK23; -Kfusion(7) = HK23*(HK10*P(6,7) - HK11*P(4,7) + HK12*P(2,7) + HK13*P(0,7) + HK14*P(1,7) - HK15*P(3,7) + HK8*P(5,7)); -Kfusion(8) = HK23*(HK10*P(6,8) - HK11*P(4,8) + HK12*P(2,8) + HK13*P(0,8) + HK14*P(1,8) - HK15*P(3,8) + HK8*P(5,8)); -Kfusion(9) = HK23*(HK10*P(6,9) - HK11*P(4,9) + HK12*P(2,9) + HK13*P(0,9) + HK14*P(1,9) - HK15*P(3,9) + HK8*P(5,9)); -Kfusion(10) = HK23*(HK10*P(6,10) - HK11*P(4,10) + HK12*P(2,10) + HK13*P(0,10) + HK14*P(1,10) - HK15*P(3,10) + HK8*P(5,10)); -Kfusion(11) = HK23*(HK10*P(6,11) - HK11*P(4,11) + HK12*P(2,11) + HK13*P(0,11) + HK14*P(1,11) - HK15*P(3,11) + HK8*P(5,11)); -Kfusion(12) = HK23*(HK10*P(6,12) - HK11*P(4,12) + HK12*P(2,12) + HK13*P(0,12) + HK14*P(1,12) - HK15*P(3,12) + HK8*P(5,12)); -Kfusion(13) = HK23*(HK10*P(6,13) - HK11*P(4,13) + HK12*P(2,13) + HK13*P(0,13) + HK14*P(1,13) - HK15*P(3,13) + HK8*P(5,13)); -Kfusion(14) = HK23*(HK10*P(6,14) - HK11*P(4,14) + HK12*P(2,14) + HK13*P(0,14) + HK14*P(1,14) - HK15*P(3,14) + HK8*P(5,14)); -Kfusion(15) = HK23*(HK10*P(6,15) - HK11*P(4,15) + HK12*P(2,15) + HK13*P(0,15) + HK14*P(1,15) - HK15*P(3,15) + HK8*P(5,15)); -Kfusion(16) = HK23*(HK10*P(6,16) - HK11*P(4,16) + HK12*P(2,16) + HK13*P(0,16) + HK14*P(1,16) - HK15*P(3,16) + HK8*P(5,16)); -Kfusion(17) = HK23*(HK10*P(6,17) - HK11*P(4,17) + HK12*P(2,17) + HK13*P(0,17) + HK14*P(1,17) - HK15*P(3,17) + HK8*P(5,17)); -Kfusion(18) = HK23*(HK10*P(6,18) - HK11*P(4,18) + HK12*P(2,18) + HK13*P(0,18) + HK14*P(1,18) - HK15*P(3,18) + HK8*P(5,18)); -Kfusion(19) = HK23*(HK10*P(6,19) - HK11*P(4,19) + HK12*P(2,19) + HK13*P(0,19) + HK14*P(1,19) - HK15*P(3,19) + HK8*P(5,19)); -Kfusion(20) = HK23*(HK10*P(6,20) - HK11*P(4,20) + HK12*P(2,20) + HK13*P(0,20) + HK14*P(1,20) - HK15*P(3,20) + HK8*P(5,20)); -Kfusion(21) = HK23*(HK10*P(6,21) - HK11*P(4,21) + HK12*P(2,21) + HK13*P(0,21) + HK14*P(1,21) - HK15*P(3,21) + HK8*P(5,21)); -Kfusion(22) = HK23*(HK10*P(6,22) - HK11*P(4,22) + HK12*P(2,22) + HK13*P(0,22) + HK14*P(1,22) - HK15*P(3,22) + HK8*P(5,22)); -Kfusion(23) = HK23*(HK10*P(6,23) - HK11*P(4,23) + HK12*P(2,23) + HK13*P(0,23) + HK14*P(1,23) - HK15*P(3,23) + HK8*P(5,23)); +Kfusion(0) = HK19*HK26; +Kfusion(1) = HK23*HK26; +Kfusion(2) = HK21*HK26; +Kfusion(3) = HK25*HK26; +Kfusion(4) = HK22*HK26; +Kfusion(5) = HK24*HK26; +Kfusion(6) = HK20*HK26; +Kfusion(7) = HK26*(HK12*P(2,7) + HK13*P(6,7) + HK14*P(0,7) - HK15*P(4,7) + HK16*P(1,7) - HK17*P(3,7) - HK18*P(5,7)); +Kfusion(8) = HK26*(HK12*P(2,8) + HK13*P(6,8) + HK14*P(0,8) - HK15*P(4,8) + HK16*P(1,8) - HK17*P(3,8) - HK18*P(5,8)); +Kfusion(9) = HK26*(HK12*P(2,9) + HK13*P(6,9) + HK14*P(0,9) - HK15*P(4,9) + HK16*P(1,9) - HK17*P(3,9) - HK18*P(5,9)); +Kfusion(10) = HK26*(HK12*P(2,10) + HK13*P(6,10) + HK14*P(0,10) - HK15*P(4,10) + HK16*P(1,10) - HK17*P(3,10) - HK18*P(5,10)); +Kfusion(11) = HK26*(HK12*P(2,11) + HK13*P(6,11) + HK14*P(0,11) - HK15*P(4,11) + HK16*P(1,11) - HK17*P(3,11) - HK18*P(5,11)); +Kfusion(12) = HK26*(HK12*P(2,12) + HK13*P(6,12) + HK14*P(0,12) - HK15*P(4,12) + HK16*P(1,12) - HK17*P(3,12) - HK18*P(5,12)); +Kfusion(13) = HK26*(HK12*P(2,13) + HK13*P(6,13) + HK14*P(0,13) - HK15*P(4,13) + HK16*P(1,13) - HK17*P(3,13) - HK18*P(5,13)); +Kfusion(14) = HK26*(HK12*P(2,14) + HK13*P(6,14) + HK14*P(0,14) - HK15*P(4,14) + HK16*P(1,14) - HK17*P(3,14) - HK18*P(5,14)); +Kfusion(15) = HK26*(HK12*P(2,15) + HK13*P(6,15) + HK14*P(0,15) - HK15*P(4,15) + HK16*P(1,15) - HK17*P(3,15) - HK18*P(5,15)); +Kfusion(16) = HK26*(HK12*P(2,16) + HK13*P(6,16) + HK14*P(0,16) - HK15*P(4,16) + HK16*P(1,16) - HK17*P(3,16) - HK18*P(5,16)); +Kfusion(17) = HK26*(HK12*P(2,17) + HK13*P(6,17) + HK14*P(0,17) - HK15*P(4,17) + HK16*P(1,17) - HK17*P(3,17) - HK18*P(5,17)); +Kfusion(18) = HK26*(HK12*P(2,18) + HK13*P(6,18) + HK14*P(0,18) - HK15*P(4,18) + HK16*P(1,18) - HK17*P(3,18) - HK18*P(5,18)); +Kfusion(19) = HK26*(HK12*P(2,19) + HK13*P(6,19) + HK14*P(0,19) - HK15*P(4,19) + HK16*P(1,19) - HK17*P(3,19) - HK18*P(5,19)); +Kfusion(20) = HK26*(HK12*P(2,20) + HK13*P(6,20) + HK14*P(0,20) - HK15*P(4,20) + HK16*P(1,20) - HK17*P(3,20) - HK18*P(5,20)); +Kfusion(21) = HK26*(HK12*P(2,21) + HK13*P(6,21) + HK14*P(0,21) - HK15*P(4,21) + HK16*P(1,21) - HK17*P(3,21) - HK18*P(5,21)); +Kfusion(22) = HK26*(HK12*P(2,22) + HK13*P(6,22) + HK14*P(0,22) - HK15*P(4,22) + HK16*P(1,22) - HK17*P(3,22) - HK18*P(5,22)); +Kfusion(23) = HK26*(HK12*P(2,23) + HK13*P(6,23) + HK14*P(0,23) - HK15*P(4,23) + HK16*P(1,23) - HK17*P(3,23) - HK18*P(5,23)); // axis 2 -const float HK0 = q0*vd - q1*ve + q2*vn; -const float HK1 = q3*vn; -const float HK2 = q0*ve; -const float HK3 = q1*vd; -const float HK4 = q0*vn - q2*vd + q3*ve; -const float HK5 = q1*vn + q2*ve + q3*vd; -const float HK6 = q0*q2 + q1*q3; -const float HK7 = q2*q3; -const float HK8 = q0*q1; -const float HK9 = powf(q0, 2) - powf(q1, 2) - powf(q2, 2) + powf(q3, 2); -const float HK10 = 2*HK6; -const float HK11 = -2*HK7 + 2*HK8; -const float HK12 = 2*HK5; -const float HK13 = 2*HK0; -const float HK14 = -2*HK1 + 2*HK2 + 2*HK3; -const float HK15 = 2*HK4; -const float HK16 = HK10*P(0,4) - HK11*P(0,5) + HK12*P(0,3) + HK13*P(0,0) - HK14*P(0,1) + HK15*P(0,2) + HK9*P(0,6); -const float HK17 = HK10*P(4,4) - HK11*P(4,5) + HK12*P(3,4) + HK13*P(0,4) - HK14*P(1,4) + HK15*P(2,4) + HK9*P(4,6); -const float HK18 = HK10*P(4,5) - HK11*P(5,5) + HK12*P(3,5) + HK13*P(0,5) - HK14*P(1,5) + HK15*P(2,5) + HK9*P(5,6); -const float HK19 = HK10*P(3,4) - HK11*P(3,5) + HK12*P(3,3) + HK13*P(0,3) - HK14*P(1,3) + HK15*P(2,3) + HK9*P(3,6); -const float HK20 = HK10*P(1,4) - HK11*P(1,5) + HK12*P(1,3) + HK13*P(0,1) - HK14*P(1,1) + HK15*P(1,2) + HK9*P(1,6); -const float HK21 = HK10*P(2,4) - HK11*P(2,5) + HK12*P(2,3) + HK13*P(0,2) - HK14*P(1,2) + HK15*P(2,2) + HK9*P(2,6); -const float HK22 = HK10*P(4,6) - HK11*P(5,6) + HK12*P(3,6) + HK13*P(0,6) - HK14*P(1,6) + HK15*P(2,6) + HK9*P(6,6); -const float HK23 = 1.0F/(HK10*HK17 - HK11*HK18 + HK12*HK19 + HK13*HK16 - HK14*HK20 + HK15*HK21 + HK22*HK9 + R_VEL); +const float HK0 = q2*vn; +const float HK1 = q1*ve; +const float HK2 = q3*vn; +const float HK3 = q0*ve; +const float HK4 = 2*vd; +const float HK5 = HK4*q1; +const float HK6 = -HK4*q2 + q0*vn + q3*ve; +const float HK7 = q1*vn + q2*ve; +const float HK8 = q0*q2 + q1*q3; +const float HK9 = q2*q3; +const float HK10 = q0*q1; +const float HK11 = 2*powf(q1, 2); +const float HK12 = 2*powf(q2, 2); +const float HK13 = 2*HK7; +const float HK14 = 2*HK8; +const float HK15 = -2*HK0 + 2*HK1; +const float HK16 = 2*HK10 - 2*HK9; +const float HK17 = 2*HK6; +const float HK18 = HK11 + HK12 - 1; +const float HK19 = -2*HK2 + 2*HK3 + 2*HK5; +const float HK20 = -HK13*P(0,3) - HK14*P(0,4) + HK15*P(0,0) + HK16*P(0,5) - HK17*P(0,2) + HK18*P(0,6) + HK19*P(0,1); +const float HK21 = -HK13*P(3,4) - HK14*P(4,4) + HK15*P(0,4) + HK16*P(4,5) - HK17*P(2,4) + HK18*P(4,6) + HK19*P(1,4); +const float HK22 = -HK13*P(3,3) - HK14*P(3,4) + HK15*P(0,3) + HK16*P(3,5) - HK17*P(2,3) + HK18*P(3,6) + HK19*P(1,3); +const float HK23 = -HK13*P(3,5) - HK14*P(4,5) + HK15*P(0,5) + HK16*P(5,5) - HK17*P(2,5) + HK18*P(5,6) + HK19*P(1,5); +const float HK24 = -HK13*P(3,6) - HK14*P(4,6) + HK15*P(0,6) + HK16*P(5,6) - HK17*P(2,6) + HK18*P(6,6) + HK19*P(1,6); +const float HK25 = -HK13*P(2,3) - HK14*P(2,4) + HK15*P(0,2) + HK16*P(2,5) - HK17*P(2,2) + HK18*P(2,6) + HK19*P(1,2); +const float HK26 = -HK13*P(1,3) - HK14*P(1,4) + HK15*P(0,1) + HK16*P(1,5) - HK17*P(1,2) + HK18*P(1,6) + HK19*P(1,1); +const float HK27 = 1.0F/(-HK13*HK22 - HK14*HK21 + HK15*HK20 + HK16*HK23 - HK17*HK25 + HK18*HK24 + HK19*HK26 + R_VEL); -H_VEL(0) = 2*HK0; -H_VEL(1) = 2*HK1 - 2*HK2 - 2*HK3; -H_VEL(2) = 2*HK4; -H_VEL(3) = 2*HK5; -H_VEL(4) = 2*HK6; -H_VEL(5) = 2*HK7 - 2*HK8; -H_VEL(6) = HK9; +H_VEL(0) = 2*HK0 - 2*HK1; +H_VEL(1) = 2*HK2 - 2*HK3 - 2*HK5; +H_VEL(2) = 2*HK6; +H_VEL(3) = 2*HK7; +H_VEL(4) = 2*HK8; +H_VEL(5) = -2*HK10 + 2*HK9; +H_VEL(6) = -HK11 - HK12 + 1; H_VEL(7) = 0; H_VEL(8) = 0; H_VEL(9) = 0; @@ -209,29 +220,29 @@ H_VEL(22) = 0; H_VEL(23) = 0; -Kfusion(0) = HK16*HK23; -Kfusion(1) = HK20*HK23; -Kfusion(2) = HK21*HK23; -Kfusion(3) = HK19*HK23; -Kfusion(4) = HK17*HK23; -Kfusion(5) = HK18*HK23; -Kfusion(6) = HK22*HK23; -Kfusion(7) = HK23*(HK10*P(4,7) - HK11*P(5,7) + HK12*P(3,7) + HK13*P(0,7) - HK14*P(1,7) + HK15*P(2,7) + HK9*P(6,7)); -Kfusion(8) = HK23*(HK10*P(4,8) - HK11*P(5,8) + HK12*P(3,8) + HK13*P(0,8) - HK14*P(1,8) + HK15*P(2,8) + HK9*P(6,8)); -Kfusion(9) = HK23*(HK10*P(4,9) - HK11*P(5,9) + HK12*P(3,9) + HK13*P(0,9) - HK14*P(1,9) + HK15*P(2,9) + HK9*P(6,9)); -Kfusion(10) = HK23*(HK10*P(4,10) - HK11*P(5,10) + HK12*P(3,10) + HK13*P(0,10) - HK14*P(1,10) + HK15*P(2,10) + HK9*P(6,10)); -Kfusion(11) = HK23*(HK10*P(4,11) - HK11*P(5,11) + HK12*P(3,11) + HK13*P(0,11) - HK14*P(1,11) + HK15*P(2,11) + HK9*P(6,11)); -Kfusion(12) = HK23*(HK10*P(4,12) - HK11*P(5,12) + HK12*P(3,12) + HK13*P(0,12) - HK14*P(1,12) + HK15*P(2,12) + HK9*P(6,12)); -Kfusion(13) = HK23*(HK10*P(4,13) - HK11*P(5,13) + HK12*P(3,13) + HK13*P(0,13) - HK14*P(1,13) + HK15*P(2,13) + HK9*P(6,13)); -Kfusion(14) = HK23*(HK10*P(4,14) - HK11*P(5,14) + HK12*P(3,14) + HK13*P(0,14) - HK14*P(1,14) + HK15*P(2,14) + HK9*P(6,14)); -Kfusion(15) = HK23*(HK10*P(4,15) - HK11*P(5,15) + HK12*P(3,15) + HK13*P(0,15) - HK14*P(1,15) + HK15*P(2,15) + HK9*P(6,15)); -Kfusion(16) = HK23*(HK10*P(4,16) - HK11*P(5,16) + HK12*P(3,16) + HK13*P(0,16) - HK14*P(1,16) + HK15*P(2,16) + HK9*P(6,16)); -Kfusion(17) = HK23*(HK10*P(4,17) - HK11*P(5,17) + HK12*P(3,17) + HK13*P(0,17) - HK14*P(1,17) + HK15*P(2,17) + HK9*P(6,17)); -Kfusion(18) = HK23*(HK10*P(4,18) - HK11*P(5,18) + HK12*P(3,18) + HK13*P(0,18) - HK14*P(1,18) + HK15*P(2,18) + HK9*P(6,18)); -Kfusion(19) = HK23*(HK10*P(4,19) - HK11*P(5,19) + HK12*P(3,19) + HK13*P(0,19) - HK14*P(1,19) + HK15*P(2,19) + HK9*P(6,19)); -Kfusion(20) = HK23*(HK10*P(4,20) - HK11*P(5,20) + HK12*P(3,20) + HK13*P(0,20) - HK14*P(1,20) + HK15*P(2,20) + HK9*P(6,20)); -Kfusion(21) = HK23*(HK10*P(4,21) - HK11*P(5,21) + HK12*P(3,21) + HK13*P(0,21) - HK14*P(1,21) + HK15*P(2,21) + HK9*P(6,21)); -Kfusion(22) = HK23*(HK10*P(4,22) - HK11*P(5,22) + HK12*P(3,22) + HK13*P(0,22) - HK14*P(1,22) + HK15*P(2,22) + HK9*P(6,22)); -Kfusion(23) = HK23*(HK10*P(4,23) - HK11*P(5,23) + HK12*P(3,23) + HK13*P(0,23) - HK14*P(1,23) + HK15*P(2,23) + HK9*P(6,23)); +Kfusion(0) = -HK20*HK27; +Kfusion(1) = -HK26*HK27; +Kfusion(2) = -HK25*HK27; +Kfusion(3) = -HK22*HK27; +Kfusion(4) = -HK21*HK27; +Kfusion(5) = -HK23*HK27; +Kfusion(6) = -HK24*HK27; +Kfusion(7) = -HK27*(-HK13*P(3,7) - HK14*P(4,7) + HK15*P(0,7) + HK16*P(5,7) - HK17*P(2,7) + HK18*P(6,7) + HK19*P(1,7)); +Kfusion(8) = -HK27*(-HK13*P(3,8) - HK14*P(4,8) + HK15*P(0,8) + HK16*P(5,8) - HK17*P(2,8) + HK18*P(6,8) + HK19*P(1,8)); +Kfusion(9) = -HK27*(-HK13*P(3,9) - HK14*P(4,9) + HK15*P(0,9) + HK16*P(5,9) - HK17*P(2,9) + HK18*P(6,9) + HK19*P(1,9)); +Kfusion(10) = -HK27*(-HK13*P(3,10) - HK14*P(4,10) + HK15*P(0,10) + HK16*P(5,10) - HK17*P(2,10) + HK18*P(6,10) + HK19*P(1,10)); +Kfusion(11) = -HK27*(-HK13*P(3,11) - HK14*P(4,11) + HK15*P(0,11) + HK16*P(5,11) - HK17*P(2,11) + HK18*P(6,11) + HK19*P(1,11)); +Kfusion(12) = -HK27*(-HK13*P(3,12) - HK14*P(4,12) + HK15*P(0,12) + HK16*P(5,12) - HK17*P(2,12) + HK18*P(6,12) + HK19*P(1,12)); +Kfusion(13) = -HK27*(-HK13*P(3,13) - HK14*P(4,13) + HK15*P(0,13) + HK16*P(5,13) - HK17*P(2,13) + HK18*P(6,13) + HK19*P(1,13)); +Kfusion(14) = -HK27*(-HK13*P(3,14) - HK14*P(4,14) + HK15*P(0,14) + HK16*P(5,14) - HK17*P(2,14) + HK18*P(6,14) + HK19*P(1,14)); +Kfusion(15) = -HK27*(-HK13*P(3,15) - HK14*P(4,15) + HK15*P(0,15) + HK16*P(5,15) - HK17*P(2,15) + HK18*P(6,15) + HK19*P(1,15)); +Kfusion(16) = -HK27*(-HK13*P(3,16) - HK14*P(4,16) + HK15*P(0,16) + HK16*P(5,16) - HK17*P(2,16) + HK18*P(6,16) + HK19*P(1,16)); +Kfusion(17) = -HK27*(-HK13*P(3,17) - HK14*P(4,17) + HK15*P(0,17) + HK16*P(5,17) - HK17*P(2,17) + HK18*P(6,17) + HK19*P(1,17)); +Kfusion(18) = -HK27*(-HK13*P(3,18) - HK14*P(4,18) + HK15*P(0,18) + HK16*P(5,18) - HK17*P(2,18) + HK18*P(6,18) + HK19*P(1,18)); +Kfusion(19) = -HK27*(-HK13*P(3,19) - HK14*P(4,19) + HK15*P(0,19) + HK16*P(5,19) - HK17*P(2,19) + HK18*P(6,19) + HK19*P(1,19)); +Kfusion(20) = -HK27*(-HK13*P(3,20) - HK14*P(4,20) + HK15*P(0,20) + HK16*P(5,20) - HK17*P(2,20) + HK18*P(6,20) + HK19*P(1,20)); +Kfusion(21) = -HK27*(-HK13*P(3,21) - HK14*P(4,21) + HK15*P(0,21) + HK16*P(5,21) - HK17*P(2,21) + HK18*P(6,21) + HK19*P(1,21)); +Kfusion(22) = -HK27*(-HK13*P(3,22) - HK14*P(4,22) + HK15*P(0,22) + HK16*P(5,22) - HK17*P(2,22) + HK18*P(6,22) + HK19*P(1,22)); +Kfusion(23) = -HK27*(-HK13*P(3,23) - HK14*P(4,23) + HK15*P(0,23) + HK16*P(5,23) - HK17*P(2,23) + HK18*P(6,23) + HK19*P(1,23)); diff --git a/EKF/python/ekf_derivation/generated/vel_bf_generated_alt.cpp b/EKF/python/ekf_derivation/generated/vel_bf_generated_alt.cpp index 4947b9ff50..7a66b237c5 100644 --- a/EKF/python/ekf_derivation/generated/vel_bf_generated_alt.cpp +++ b/EKF/python/ekf_derivation/generated/vel_bf_generated_alt.cpp @@ -1,96 +1,100 @@ // Sub Expressions -const float HK0 = q0*vn; -const float HK1 = q3*ve; -const float HK2 = q2*vd; -const float HK3 = HK0 + HK1 - HK2; -const float HK4 = 2*HK3; -const float HK5 = q1*vn + q2*ve + q3*vd; -const float HK6 = 2*HK5; -const float HK7 = q1*ve; -const float HK8 = q0*vd; -const float HK9 = q2*vn; +const float HK0 = q3*ve; +const float HK1 = q2*vd; +const float HK2 = -HK1; +const float HK3 = q2*ve; +const float HK4 = q3*vd; +const float HK5 = HK3 + HK4; +const float HK6 = q1*ve; +const float HK7 = q0*vd; +const float HK8 = q2*vn; +const float HK9 = 2*HK8; const float HK10 = q0*ve; const float HK11 = q1*vd; const float HK12 = q3*vn; -const float HK13 = HK10 + HK11 - HK12; -const float HK14 = 2*HK13; -const float HK15 = powf(q1, 2); -const float HK16 = powf(q2, 2); -const float HK17 = -HK16; -const float HK18 = powf(q0, 2); -const float HK19 = powf(q3, 2); -const float HK20 = HK18 - HK19; -const float HK21 = HK15 + HK17 + HK20; -const float HK22 = q0*q3; -const float HK23 = q1*q2; -const float HK24 = HK22 + HK23; -const float HK25 = q1*q3; -const float HK26 = q0*q2; -const float HK27 = 2*HK24; -const float HK28 = -2*HK25 + 2*HK26; -const float HK29 = 2*HK5; -const float HK30 = 2*HK3; -const float HK31 = -HK7 + HK8 + HK9; -const float HK32 = 2*HK31; -const float HK33 = HK32*P(0,2); -const float HK34 = 2*HK13; -const float HK35 = HK34*P(0,3); -const float HK36 = HK21*P(0,4) + HK27*P(0,5) - HK28*P(0,6) + HK29*P(0,1) + HK30*P(0,0) - HK33 + HK35; -const float HK37 = HK21*P(4,5) + HK27*P(5,5) - HK28*P(5,6) + HK29*P(1,5) + HK30*P(0,5) - HK32*P(2,5) + HK34*P(3,5); -const float HK38 = HK21*P(4,6) + HK27*P(5,6) - HK28*P(6,6) + HK29*P(1,6) + HK30*P(0,6) - HK32*P(2,6) + HK34*P(3,6); -const float HK39 = HK32*P(1,2); -const float HK40 = HK34*P(1,3); -const float HK41 = HK21*P(1,4) + HK27*P(1,5) - HK28*P(1,6) + HK29*P(1,1) + HK30*P(0,1) - HK39 + HK40; -const float HK42 = HK29*P(1,2); -const float HK43 = HK30*P(0,2); -const float HK44 = HK21*P(2,4) + HK27*P(2,5) - HK28*P(2,6) - HK32*P(2,2) + HK34*P(2,3) + HK42 + HK43; -const float HK45 = HK29*P(1,3); -const float HK46 = HK30*P(0,3); -const float HK47 = HK21*P(3,4) + HK27*P(3,5) - HK28*P(3,6) - HK32*P(2,3) + HK34*P(3,3) + HK45 + HK46; -const float HK48 = HK21*P(4,4) + HK27*P(4,5) - HK28*P(4,6) + HK29*P(1,4) + HK30*P(0,4) - HK32*P(2,4) + HK34*P(3,4); -const float HK49 = 1.0F/(HK21*HK48 + HK27*HK37 - HK28*HK38 + HK29*HK41 + HK30*HK36 - HK32*HK44 + HK34*HK47 + R_VEL); -const float HK50 = 2*HK31; -const float HK51 = -HK15; -const float HK52 = HK16 + HK20 + HK51; -const float HK53 = q0*q1; -const float HK54 = q2*q3; -const float HK55 = HK53 + HK54; -const float HK56 = 2*HK55; -const float HK57 = 2*HK22 - 2*HK23; -const float HK58 = HK32*P(0,1); -const float HK59 = HK29*P(0,2) + HK34*P(0,0) - HK46 + HK52*P(0,5) + HK56*P(0,6) - HK57*P(0,4) + HK58; -const float HK60 = HK29*P(2,6) - HK30*P(3,6) + HK32*P(1,6) + HK34*P(0,6) + HK52*P(5,6) + HK56*P(6,6) - HK57*P(4,6); -const float HK61 = HK29*P(2,4) - HK30*P(3,4) + HK32*P(1,4) + HK34*P(0,4) + HK52*P(4,5) + HK56*P(4,6) - HK57*P(4,4); -const float HK62 = HK30*P(2,3); -const float HK63 = HK29*P(2,2) + HK34*P(0,2) + HK39 + HK52*P(2,5) + HK56*P(2,6) - HK57*P(2,4) - HK62; -const float HK64 = HK34*P(0,1); -const float HK65 = -HK30*P(1,3) + HK32*P(1,1) + HK42 + HK52*P(1,5) + HK56*P(1,6) - HK57*P(1,4) + HK64; -const float HK66 = HK29*P(2,3); -const float HK67 = -HK30*P(3,3) + HK32*P(1,3) + HK35 + HK52*P(3,5) + HK56*P(3,6) - HK57*P(3,4) + HK66; -const float HK68 = HK29*P(2,5) - HK30*P(3,5) + HK32*P(1,5) + HK34*P(0,5) + HK52*P(5,5) + HK56*P(5,6) - HK57*P(4,5); -const float HK69 = 1.0F/(HK29*HK63 - HK30*HK67 + HK32*HK65 + HK34*HK59 + HK52*HK68 + HK56*HK60 - HK57*HK61 + R_VEL); -const float HK70 = HK25 + HK26; -const float HK71 = HK17 + HK18 + HK19 + HK51; +const float HK13 = HK10 + HK11 - 2*HK12; +const float HK14 = 2*powf(q2, 2); +const float HK15 = -HK14; +const float HK16 = 2*powf(q3, 2); +const float HK17 = 1 - HK16; +const float HK18 = q0*q3; +const float HK19 = q1*q2; +const float HK20 = HK18 + HK19; +const float HK21 = q1*q3; +const float HK22 = q0*q2; +const float HK23 = 2*HK5; +const float HK24 = 2*HK20; +const float HK25 = -2*HK0 + 2*HK1; +const float HK26 = -2*HK21 + 2*HK22; +const float HK27 = 2*HK13; +const float HK28 = HK16 - 1; +const float HK29 = HK14 + HK28; +const float HK30 = -HK6; +const float HK31 = 2*HK30 + 2*HK7 + 2*HK9; +const float HK32 = -HK23*P(0,1) - HK24*P(0,5) + HK25*P(0,0) + HK26*P(0,6) - HK27*P(0,3) + HK29*P(0,4) + HK31*P(0,2); +const float HK33 = -HK23*P(1,5) - HK24*P(5,5) + HK25*P(0,5) + HK26*P(5,6) - HK27*P(3,5) + HK29*P(4,5) + HK31*P(2,5); +const float HK34 = -HK23*P(1,1) - HK24*P(1,5) + HK25*P(0,1) + HK26*P(1,6) - HK27*P(1,3) + HK29*P(1,4) + HK31*P(1,2); +const float HK35 = -HK23*P(1,6) - HK24*P(5,6) + HK25*P(0,6) + HK26*P(6,6) - HK27*P(3,6) + HK29*P(4,6) + HK31*P(2,6); +const float HK36 = -HK23*P(1,4) - HK24*P(4,5) + HK25*P(0,4) + HK26*P(4,6) - HK27*P(3,4) + HK29*P(4,4) + HK31*P(2,4); +const float HK37 = -HK23*P(1,3) - HK24*P(3,5) + HK25*P(0,3) + HK26*P(3,6) - HK27*P(3,3) + HK29*P(3,4) + HK31*P(2,3); +const float HK38 = -HK23*P(1,2) - HK24*P(2,5) + HK25*P(0,2) + HK26*P(2,6) - HK27*P(2,3) + HK29*P(2,4) + HK31*P(2,2); +const float HK39 = 1.0F/(-HK23*HK34 - HK24*HK33 + HK25*HK32 + HK26*HK35 - HK27*HK37 + HK29*HK36 + HK31*HK38 + R_VEL); +const float HK40 = -HK12; +const float HK41 = HK11 + HK40; +const float HK42 = -2*HK6 + HK7 + HK8; +const float HK43 = q1*vn; +const float HK44 = HK4 + HK43; +const float HK45 = q0*vn; +const float HK46 = 2*HK0; +const float HK47 = 2*powf(q1, 2); +const float HK48 = -HK47; +const float HK49 = q0*q1; +const float HK50 = q2*q3; +const float HK51 = HK49 + HK50; +const float HK52 = 2*HK44; +const float HK53 = 2*HK51; +const float HK54 = 2*HK41; +const float HK55 = 2*HK18 - 2*HK19; +const float HK56 = 2*HK42; +const float HK57 = 2*HK2 + 2*HK45 + 2*HK46; +const float HK58 = HK28 + HK47; +const float HK59 = HK52*P(0,2) + HK53*P(0,6) + HK54*P(0,0) - HK55*P(0,4) + HK56*P(0,1) - HK57*P(0,3) - HK58*P(0,5); +const float HK60 = HK52*P(2,6) + HK53*P(6,6) + HK54*P(0,6) - HK55*P(4,6) + HK56*P(1,6) - HK57*P(3,6) - HK58*P(5,6); +const float HK61 = HK52*P(2,2) + HK53*P(2,6) + HK54*P(0,2) - HK55*P(2,4) + HK56*P(1,2) - HK57*P(2,3) - HK58*P(2,5); +const float HK62 = HK52*P(2,4) + HK53*P(4,6) + HK54*P(0,4) - HK55*P(4,4) + HK56*P(1,4) - HK57*P(3,4) - HK58*P(4,5); +const float HK63 = HK52*P(1,2) + HK53*P(1,6) + HK54*P(0,1) - HK55*P(1,4) + HK56*P(1,1) - HK57*P(1,3) - HK58*P(1,5); +const float HK64 = HK52*P(2,5) + HK53*P(5,6) + HK54*P(0,5) - HK55*P(4,5) + HK56*P(1,5) - HK57*P(3,5) - HK58*P(5,5); +const float HK65 = HK52*P(2,3) + HK53*P(3,6) + HK54*P(0,3) - HK55*P(3,4) + HK56*P(1,3) - HK57*P(3,3) - HK58*P(3,5); +const float HK66 = 1.0F/(HK52*HK61 + HK53*HK60 + HK54*HK59 - HK55*HK62 + HK56*HK63 - HK57*HK65 - HK58*HK64 + R_VEL); +const float HK67 = 2*HK11; +const float HK68 = HK0 - 2*HK1 + HK45; +const float HK69 = HK3 + HK43; +const float HK70 = HK21 + HK22; +const float HK71 = 2*HK69; const float HK72 = 2*HK70; -const float HK73 = 2*HK53 - 2*HK54; -const float HK74 = HK29*P(0,3) + HK32*P(0,0) + HK43 - HK64 + HK71*P(0,6) + HK72*P(0,4) - HK73*P(0,5); -const float HK75 = HK29*P(3,4) + HK30*P(2,4) + HK32*P(0,4) - HK34*P(1,4) + HK71*P(4,6) + HK72*P(4,4) - HK73*P(4,5); -const float HK76 = HK29*P(3,5) + HK30*P(2,5) + HK32*P(0,5) - HK34*P(1,5) + HK71*P(5,6) + HK72*P(4,5) - HK73*P(5,5); -const float HK77 = HK29*P(3,3) + HK32*P(0,3) - HK40 + HK62 + HK71*P(3,6) + HK72*P(3,4) - HK73*P(3,5); -const float HK78 = HK30*P(1,2) - HK34*P(1,1) + HK45 + HK58 + HK71*P(1,6) + HK72*P(1,4) - HK73*P(1,5); -const float HK79 = HK30*P(2,2) + HK33 - HK34*P(1,2) + HK66 + HK71*P(2,6) + HK72*P(2,4) - HK73*P(2,5); -const float HK80 = HK29*P(3,6) + HK30*P(2,6) + HK32*P(0,6) - HK34*P(1,6) + HK71*P(6,6) + HK72*P(4,6) - HK73*P(5,6); -const float HK81 = 1.0F/(HK29*HK77 + HK30*HK79 + HK32*HK74 - HK34*HK78 + HK71*HK80 + HK72*HK75 - HK73*HK76 + R_VEL); +const float HK73 = 2*HK6 - 2*HK8; +const float HK74 = 2*HK49 - 2*HK50; +const float HK75 = 2*HK68; +const float HK76 = HK14 + HK47 - 1; +const float HK77 = 2*HK10 + 2*HK40 + 2*HK67; +const float HK78 = -HK71*P(0,3) - HK72*P(0,4) + HK73*P(0,0) + HK74*P(0,5) - HK75*P(0,2) + HK76*P(0,6) + HK77*P(0,1); +const float HK79 = -HK71*P(3,4) - HK72*P(4,4) + HK73*P(0,4) + HK74*P(4,5) - HK75*P(2,4) + HK76*P(4,6) + HK77*P(1,4); +const float HK80 = -HK71*P(3,3) - HK72*P(3,4) + HK73*P(0,3) + HK74*P(3,5) - HK75*P(2,3) + HK76*P(3,6) + HK77*P(1,3); +const float HK81 = -HK71*P(3,5) - HK72*P(4,5) + HK73*P(0,5) + HK74*P(5,5) - HK75*P(2,5) + HK76*P(5,6) + HK77*P(1,5); +const float HK82 = -HK71*P(3,6) - HK72*P(4,6) + HK73*P(0,6) + HK74*P(5,6) - HK75*P(2,6) + HK76*P(6,6) + HK77*P(1,6); +const float HK83 = -HK71*P(2,3) - HK72*P(2,4) + HK73*P(0,2) + HK74*P(2,5) - HK75*P(2,2) + HK76*P(2,6) + HK77*P(1,2); +const float HK84 = -HK71*P(1,3) - HK72*P(1,4) + HK73*P(0,1) + HK74*P(1,5) - HK75*P(1,2) + HK76*P(1,6) + HK77*P(1,1); +const float HK85 = 1.0F/(-HK71*HK80 - HK72*HK79 + HK73*HK78 + HK74*HK81 - HK75*HK83 + HK76*HK82 + HK77*HK84 + R_VEL); // Observation Jacobians - axis 0 -Hfusion.at<0>() = HK4; -Hfusion.at<1>() = HK6; -Hfusion.at<2>() = 2*HK7 - 2*HK8 - 2*HK9; -Hfusion.at<3>() = HK14; -Hfusion.at<4>() = HK21; -Hfusion.at<5>() = 2*HK24; -Hfusion.at<6>() = 2*HK25 - 2*HK26; +Hfusion.at<0>() = 2*HK0 + 2*HK2; +Hfusion.at<1>() = 2*HK5; +Hfusion.at<2>() = 2*HK6 - 2*HK7 - 2*HK9; +Hfusion.at<3>() = 2*HK13; +Hfusion.at<4>() = HK15 + HK17; +Hfusion.at<5>() = 2*HK20; +Hfusion.at<6>() = 2*HK21 - 2*HK22; Hfusion.at<7>() = 0; Hfusion.at<8>() = 0; Hfusion.at<9>() = 0; @@ -111,40 +115,40 @@ Hfusion.at<23>() = 0; // Kalman gains - axis 0 -Kfusion(0) = HK36*HK49; -Kfusion(1) = HK41*HK49; -Kfusion(2) = HK44*HK49; -Kfusion(3) = HK47*HK49; -Kfusion(4) = HK48*HK49; -Kfusion(5) = HK37*HK49; -Kfusion(6) = HK38*HK49; -Kfusion(7) = HK49*(HK21*P(4,7) + HK27*P(5,7) - HK28*P(6,7) + HK29*P(1,7) + HK30*P(0,7) - HK32*P(2,7) + HK34*P(3,7)); -Kfusion(8) = HK49*(HK21*P(4,8) + HK27*P(5,8) - HK28*P(6,8) + HK29*P(1,8) + HK30*P(0,8) - HK32*P(2,8) + HK34*P(3,8)); -Kfusion(9) = HK49*(HK21*P(4,9) + HK27*P(5,9) - HK28*P(6,9) + HK29*P(1,9) + HK30*P(0,9) - HK32*P(2,9) + HK34*P(3,9)); -Kfusion(10) = HK49*(HK21*P(4,10) + HK27*P(5,10) - HK28*P(6,10) + HK29*P(1,10) + HK30*P(0,10) - HK32*P(2,10) + HK34*P(3,10)); -Kfusion(11) = HK49*(HK21*P(4,11) + HK27*P(5,11) - HK28*P(6,11) + HK29*P(1,11) + HK30*P(0,11) - HK32*P(2,11) + HK34*P(3,11)); -Kfusion(12) = HK49*(HK21*P(4,12) + HK27*P(5,12) - HK28*P(6,12) + HK29*P(1,12) + HK30*P(0,12) - HK32*P(2,12) + HK34*P(3,12)); -Kfusion(13) = HK49*(HK21*P(4,13) + HK27*P(5,13) - HK28*P(6,13) + HK29*P(1,13) + HK30*P(0,13) - HK32*P(2,13) + HK34*P(3,13)); -Kfusion(14) = HK49*(HK21*P(4,14) + HK27*P(5,14) - HK28*P(6,14) + HK29*P(1,14) + HK30*P(0,14) - HK32*P(2,14) + HK34*P(3,14)); -Kfusion(15) = HK49*(HK21*P(4,15) + HK27*P(5,15) - HK28*P(6,15) + HK29*P(1,15) + HK30*P(0,15) - HK32*P(2,15) + HK34*P(3,15)); -Kfusion(16) = HK49*(HK21*P(4,16) + HK27*P(5,16) - HK28*P(6,16) + HK29*P(1,16) + HK30*P(0,16) - HK32*P(2,16) + HK34*P(3,16)); -Kfusion(17) = HK49*(HK21*P(4,17) + HK27*P(5,17) - HK28*P(6,17) + HK29*P(1,17) + HK30*P(0,17) - HK32*P(2,17) + HK34*P(3,17)); -Kfusion(18) = HK49*(HK21*P(4,18) + HK27*P(5,18) - HK28*P(6,18) + HK29*P(1,18) + HK30*P(0,18) - HK32*P(2,18) + HK34*P(3,18)); -Kfusion(19) = HK49*(HK21*P(4,19) + HK27*P(5,19) - HK28*P(6,19) + HK29*P(1,19) + HK30*P(0,19) - HK32*P(2,19) + HK34*P(3,19)); -Kfusion(20) = HK49*(HK21*P(4,20) + HK27*P(5,20) - HK28*P(6,20) + HK29*P(1,20) + HK30*P(0,20) - HK32*P(2,20) + HK34*P(3,20)); -Kfusion(21) = HK49*(HK21*P(4,21) + HK27*P(5,21) - HK28*P(6,21) + HK29*P(1,21) + HK30*P(0,21) - HK32*P(2,21) + HK34*P(3,21)); -Kfusion(22) = HK49*(HK21*P(4,22) + HK27*P(5,22) - HK28*P(6,22) + HK29*P(1,22) + HK30*P(0,22) - HK32*P(2,22) + HK34*P(3,22)); -Kfusion(23) = HK49*(HK21*P(4,23) + HK27*P(5,23) - HK28*P(6,23) + HK29*P(1,23) + HK30*P(0,23) - HK32*P(2,23) + HK34*P(3,23)); +Kfusion(0) = -HK32*HK39; +Kfusion(1) = -HK34*HK39; +Kfusion(2) = -HK38*HK39; +Kfusion(3) = -HK37*HK39; +Kfusion(4) = -HK36*HK39; +Kfusion(5) = -HK33*HK39; +Kfusion(6) = -HK35*HK39; +Kfusion(7) = -HK39*(-HK23*P(1,7) - HK24*P(5,7) + HK25*P(0,7) + HK26*P(6,7) - HK27*P(3,7) + HK29*P(4,7) + HK31*P(2,7)); +Kfusion(8) = -HK39*(-HK23*P(1,8) - HK24*P(5,8) + HK25*P(0,8) + HK26*P(6,8) - HK27*P(3,8) + HK29*P(4,8) + HK31*P(2,8)); +Kfusion(9) = -HK39*(-HK23*P(1,9) - HK24*P(5,9) + HK25*P(0,9) + HK26*P(6,9) - HK27*P(3,9) + HK29*P(4,9) + HK31*P(2,9)); +Kfusion(10) = -HK39*(-HK23*P(1,10) - HK24*P(5,10) + HK25*P(0,10) + HK26*P(6,10) - HK27*P(3,10) + HK29*P(4,10) + HK31*P(2,10)); +Kfusion(11) = -HK39*(-HK23*P(1,11) - HK24*P(5,11) + HK25*P(0,11) + HK26*P(6,11) - HK27*P(3,11) + HK29*P(4,11) + HK31*P(2,11)); +Kfusion(12) = -HK39*(-HK23*P(1,12) - HK24*P(5,12) + HK25*P(0,12) + HK26*P(6,12) - HK27*P(3,12) + HK29*P(4,12) + HK31*P(2,12)); +Kfusion(13) = -HK39*(-HK23*P(1,13) - HK24*P(5,13) + HK25*P(0,13) + HK26*P(6,13) - HK27*P(3,13) + HK29*P(4,13) + HK31*P(2,13)); +Kfusion(14) = -HK39*(-HK23*P(1,14) - HK24*P(5,14) + HK25*P(0,14) + HK26*P(6,14) - HK27*P(3,14) + HK29*P(4,14) + HK31*P(2,14)); +Kfusion(15) = -HK39*(-HK23*P(1,15) - HK24*P(5,15) + HK25*P(0,15) + HK26*P(6,15) - HK27*P(3,15) + HK29*P(4,15) + HK31*P(2,15)); +Kfusion(16) = -HK39*(-HK23*P(1,16) - HK24*P(5,16) + HK25*P(0,16) + HK26*P(6,16) - HK27*P(3,16) + HK29*P(4,16) + HK31*P(2,16)); +Kfusion(17) = -HK39*(-HK23*P(1,17) - HK24*P(5,17) + HK25*P(0,17) + HK26*P(6,17) - HK27*P(3,17) + HK29*P(4,17) + HK31*P(2,17)); +Kfusion(18) = -HK39*(-HK23*P(1,18) - HK24*P(5,18) + HK25*P(0,18) + HK26*P(6,18) - HK27*P(3,18) + HK29*P(4,18) + HK31*P(2,18)); +Kfusion(19) = -HK39*(-HK23*P(1,19) - HK24*P(5,19) + HK25*P(0,19) + HK26*P(6,19) - HK27*P(3,19) + HK29*P(4,19) + HK31*P(2,19)); +Kfusion(20) = -HK39*(-HK23*P(1,20) - HK24*P(5,20) + HK25*P(0,20) + HK26*P(6,20) - HK27*P(3,20) + HK29*P(4,20) + HK31*P(2,20)); +Kfusion(21) = -HK39*(-HK23*P(1,21) - HK24*P(5,21) + HK25*P(0,21) + HK26*P(6,21) - HK27*P(3,21) + HK29*P(4,21) + HK31*P(2,21)); +Kfusion(22) = -HK39*(-HK23*P(1,22) - HK24*P(5,22) + HK25*P(0,22) + HK26*P(6,22) - HK27*P(3,22) + HK29*P(4,22) + HK31*P(2,22)); +Kfusion(23) = -HK39*(-HK23*P(1,23) - HK24*P(5,23) + HK25*P(0,23) + HK26*P(6,23) - HK27*P(3,23) + HK29*P(4,23) + HK31*P(2,23)); // Observation Jacobians - axis 1 -Hfusion.at<0>() = HK14; -Hfusion.at<1>() = HK50; -Hfusion.at<2>() = HK6; -Hfusion.at<3>() = -2*HK0 - 2*HK1 + 2*HK2; -Hfusion.at<4>() = -2*HK22 + 2*HK23; -Hfusion.at<5>() = HK52; -Hfusion.at<6>() = 2*HK55; +Hfusion.at<0>() = 2*HK41; +Hfusion.at<1>() = 2*HK42; +Hfusion.at<2>() = 2*HK44; +Hfusion.at<3>() = 2*HK1 - 2*HK45 - 2*HK46; +Hfusion.at<4>() = -2*HK18 + 2*HK19; +Hfusion.at<5>() = HK17 + HK48; +Hfusion.at<6>() = 2*HK51; Hfusion.at<7>() = 0; Hfusion.at<8>() = 0; Hfusion.at<9>() = 0; @@ -165,40 +169,40 @@ Hfusion.at<23>() = 0; // Kalman gains - axis 1 -Kfusion(0) = HK59*HK69; -Kfusion(1) = HK65*HK69; -Kfusion(2) = HK63*HK69; -Kfusion(3) = HK67*HK69; -Kfusion(4) = HK61*HK69; -Kfusion(5) = HK68*HK69; -Kfusion(6) = HK60*HK69; -Kfusion(7) = HK69*(HK29*P(2,7) - HK30*P(3,7) + HK32*P(1,7) + HK34*P(0,7) + HK52*P(5,7) + HK56*P(6,7) - HK57*P(4,7)); -Kfusion(8) = HK69*(HK29*P(2,8) - HK30*P(3,8) + HK32*P(1,8) + HK34*P(0,8) + HK52*P(5,8) + HK56*P(6,8) - HK57*P(4,8)); -Kfusion(9) = HK69*(HK29*P(2,9) - HK30*P(3,9) + HK32*P(1,9) + HK34*P(0,9) + HK52*P(5,9) + HK56*P(6,9) - HK57*P(4,9)); -Kfusion(10) = HK69*(HK29*P(2,10) - HK30*P(3,10) + HK32*P(1,10) + HK34*P(0,10) + HK52*P(5,10) + HK56*P(6,10) - HK57*P(4,10)); -Kfusion(11) = HK69*(HK29*P(2,11) - HK30*P(3,11) + HK32*P(1,11) + HK34*P(0,11) + HK52*P(5,11) + HK56*P(6,11) - HK57*P(4,11)); -Kfusion(12) = HK69*(HK29*P(2,12) - HK30*P(3,12) + HK32*P(1,12) + HK34*P(0,12) + HK52*P(5,12) + HK56*P(6,12) - HK57*P(4,12)); -Kfusion(13) = HK69*(HK29*P(2,13) - HK30*P(3,13) + HK32*P(1,13) + HK34*P(0,13) + HK52*P(5,13) + HK56*P(6,13) - HK57*P(4,13)); -Kfusion(14) = HK69*(HK29*P(2,14) - HK30*P(3,14) + HK32*P(1,14) + HK34*P(0,14) + HK52*P(5,14) + HK56*P(6,14) - HK57*P(4,14)); -Kfusion(15) = HK69*(HK29*P(2,15) - HK30*P(3,15) + HK32*P(1,15) + HK34*P(0,15) + HK52*P(5,15) + HK56*P(6,15) - HK57*P(4,15)); -Kfusion(16) = HK69*(HK29*P(2,16) - HK30*P(3,16) + HK32*P(1,16) + HK34*P(0,16) + HK52*P(5,16) + HK56*P(6,16) - HK57*P(4,16)); -Kfusion(17) = HK69*(HK29*P(2,17) - HK30*P(3,17) + HK32*P(1,17) + HK34*P(0,17) + HK52*P(5,17) + HK56*P(6,17) - HK57*P(4,17)); -Kfusion(18) = HK69*(HK29*P(2,18) - HK30*P(3,18) + HK32*P(1,18) + HK34*P(0,18) + HK52*P(5,18) + HK56*P(6,18) - HK57*P(4,18)); -Kfusion(19) = HK69*(HK29*P(2,19) - HK30*P(3,19) + HK32*P(1,19) + HK34*P(0,19) + HK52*P(5,19) + HK56*P(6,19) - HK57*P(4,19)); -Kfusion(20) = HK69*(HK29*P(2,20) - HK30*P(3,20) + HK32*P(1,20) + HK34*P(0,20) + HK52*P(5,20) + HK56*P(6,20) - HK57*P(4,20)); -Kfusion(21) = HK69*(HK29*P(2,21) - HK30*P(3,21) + HK32*P(1,21) + HK34*P(0,21) + HK52*P(5,21) + HK56*P(6,21) - HK57*P(4,21)); -Kfusion(22) = HK69*(HK29*P(2,22) - HK30*P(3,22) + HK32*P(1,22) + HK34*P(0,22) + HK52*P(5,22) + HK56*P(6,22) - HK57*P(4,22)); -Kfusion(23) = HK69*(HK29*P(2,23) - HK30*P(3,23) + HK32*P(1,23) + HK34*P(0,23) + HK52*P(5,23) + HK56*P(6,23) - HK57*P(4,23)); +Kfusion(0) = HK59*HK66; +Kfusion(1) = HK63*HK66; +Kfusion(2) = HK61*HK66; +Kfusion(3) = HK65*HK66; +Kfusion(4) = HK62*HK66; +Kfusion(5) = HK64*HK66; +Kfusion(6) = HK60*HK66; +Kfusion(7) = HK66*(HK52*P(2,7) + HK53*P(6,7) + HK54*P(0,7) - HK55*P(4,7) + HK56*P(1,7) - HK57*P(3,7) - HK58*P(5,7)); +Kfusion(8) = HK66*(HK52*P(2,8) + HK53*P(6,8) + HK54*P(0,8) - HK55*P(4,8) + HK56*P(1,8) - HK57*P(3,8) - HK58*P(5,8)); +Kfusion(9) = HK66*(HK52*P(2,9) + HK53*P(6,9) + HK54*P(0,9) - HK55*P(4,9) + HK56*P(1,9) - HK57*P(3,9) - HK58*P(5,9)); +Kfusion(10) = HK66*(HK52*P(2,10) + HK53*P(6,10) + HK54*P(0,10) - HK55*P(4,10) + HK56*P(1,10) - HK57*P(3,10) - HK58*P(5,10)); +Kfusion(11) = HK66*(HK52*P(2,11) + HK53*P(6,11) + HK54*P(0,11) - HK55*P(4,11) + HK56*P(1,11) - HK57*P(3,11) - HK58*P(5,11)); +Kfusion(12) = HK66*(HK52*P(2,12) + HK53*P(6,12) + HK54*P(0,12) - HK55*P(4,12) + HK56*P(1,12) - HK57*P(3,12) - HK58*P(5,12)); +Kfusion(13) = HK66*(HK52*P(2,13) + HK53*P(6,13) + HK54*P(0,13) - HK55*P(4,13) + HK56*P(1,13) - HK57*P(3,13) - HK58*P(5,13)); +Kfusion(14) = HK66*(HK52*P(2,14) + HK53*P(6,14) + HK54*P(0,14) - HK55*P(4,14) + HK56*P(1,14) - HK57*P(3,14) - HK58*P(5,14)); +Kfusion(15) = HK66*(HK52*P(2,15) + HK53*P(6,15) + HK54*P(0,15) - HK55*P(4,15) + HK56*P(1,15) - HK57*P(3,15) - HK58*P(5,15)); +Kfusion(16) = HK66*(HK52*P(2,16) + HK53*P(6,16) + HK54*P(0,16) - HK55*P(4,16) + HK56*P(1,16) - HK57*P(3,16) - HK58*P(5,16)); +Kfusion(17) = HK66*(HK52*P(2,17) + HK53*P(6,17) + HK54*P(0,17) - HK55*P(4,17) + HK56*P(1,17) - HK57*P(3,17) - HK58*P(5,17)); +Kfusion(18) = HK66*(HK52*P(2,18) + HK53*P(6,18) + HK54*P(0,18) - HK55*P(4,18) + HK56*P(1,18) - HK57*P(3,18) - HK58*P(5,18)); +Kfusion(19) = HK66*(HK52*P(2,19) + HK53*P(6,19) + HK54*P(0,19) - HK55*P(4,19) + HK56*P(1,19) - HK57*P(3,19) - HK58*P(5,19)); +Kfusion(20) = HK66*(HK52*P(2,20) + HK53*P(6,20) + HK54*P(0,20) - HK55*P(4,20) + HK56*P(1,20) - HK57*P(3,20) - HK58*P(5,20)); +Kfusion(21) = HK66*(HK52*P(2,21) + HK53*P(6,21) + HK54*P(0,21) - HK55*P(4,21) + HK56*P(1,21) - HK57*P(3,21) - HK58*P(5,21)); +Kfusion(22) = HK66*(HK52*P(2,22) + HK53*P(6,22) + HK54*P(0,22) - HK55*P(4,22) + HK56*P(1,22) - HK57*P(3,22) - HK58*P(5,22)); +Kfusion(23) = HK66*(HK52*P(2,23) + HK53*P(6,23) + HK54*P(0,23) - HK55*P(4,23) + HK56*P(1,23) - HK57*P(3,23) - HK58*P(5,23)); // Observation Jacobians - axis 2 -Hfusion.at<0>() = HK50; -Hfusion.at<1>() = -2*HK10 - 2*HK11 + 2*HK12; -Hfusion.at<2>() = HK4; -Hfusion.at<3>() = HK6; +Hfusion.at<0>() = 2*HK30 + 2*HK8; +Hfusion.at<1>() = -2*HK10 + 2*HK12 - 2*HK67; +Hfusion.at<2>() = 2*HK68; +Hfusion.at<3>() = 2*HK69; Hfusion.at<4>() = 2*HK70; -Hfusion.at<5>() = -2*HK53 + 2*HK54; -Hfusion.at<6>() = HK71; +Hfusion.at<5>() = -2*HK49 + 2*HK50; +Hfusion.at<6>() = HK15 + HK48 + 1; Hfusion.at<7>() = 0; Hfusion.at<8>() = 0; Hfusion.at<9>() = 0; @@ -219,29 +223,29 @@ Hfusion.at<23>() = 0; // Kalman gains - axis 2 -Kfusion(0) = HK74*HK81; -Kfusion(1) = HK78*HK81; -Kfusion(2) = HK79*HK81; -Kfusion(3) = HK77*HK81; -Kfusion(4) = HK75*HK81; -Kfusion(5) = HK76*HK81; -Kfusion(6) = HK80*HK81; -Kfusion(7) = HK81*(HK29*P(3,7) + HK30*P(2,7) + HK32*P(0,7) - HK34*P(1,7) + HK71*P(6,7) + HK72*P(4,7) - HK73*P(5,7)); -Kfusion(8) = HK81*(HK29*P(3,8) + HK30*P(2,8) + HK32*P(0,8) - HK34*P(1,8) + HK71*P(6,8) + HK72*P(4,8) - HK73*P(5,8)); -Kfusion(9) = HK81*(HK29*P(3,9) + HK30*P(2,9) + HK32*P(0,9) - HK34*P(1,9) + HK71*P(6,9) + HK72*P(4,9) - HK73*P(5,9)); -Kfusion(10) = HK81*(HK29*P(3,10) + HK30*P(2,10) + HK32*P(0,10) - HK34*P(1,10) + HK71*P(6,10) + HK72*P(4,10) - HK73*P(5,10)); -Kfusion(11) = HK81*(HK29*P(3,11) + HK30*P(2,11) + HK32*P(0,11) - HK34*P(1,11) + HK71*P(6,11) + HK72*P(4,11) - HK73*P(5,11)); -Kfusion(12) = HK81*(HK29*P(3,12) + HK30*P(2,12) + HK32*P(0,12) - HK34*P(1,12) + HK71*P(6,12) + HK72*P(4,12) - HK73*P(5,12)); -Kfusion(13) = HK81*(HK29*P(3,13) + HK30*P(2,13) + HK32*P(0,13) - HK34*P(1,13) + HK71*P(6,13) + HK72*P(4,13) - HK73*P(5,13)); -Kfusion(14) = HK81*(HK29*P(3,14) + HK30*P(2,14) + HK32*P(0,14) - HK34*P(1,14) + HK71*P(6,14) + HK72*P(4,14) - HK73*P(5,14)); -Kfusion(15) = HK81*(HK29*P(3,15) + HK30*P(2,15) + HK32*P(0,15) - HK34*P(1,15) + HK71*P(6,15) + HK72*P(4,15) - HK73*P(5,15)); -Kfusion(16) = HK81*(HK29*P(3,16) + HK30*P(2,16) + HK32*P(0,16) - HK34*P(1,16) + HK71*P(6,16) + HK72*P(4,16) - HK73*P(5,16)); -Kfusion(17) = HK81*(HK29*P(3,17) + HK30*P(2,17) + HK32*P(0,17) - HK34*P(1,17) + HK71*P(6,17) + HK72*P(4,17) - HK73*P(5,17)); -Kfusion(18) = HK81*(HK29*P(3,18) + HK30*P(2,18) + HK32*P(0,18) - HK34*P(1,18) + HK71*P(6,18) + HK72*P(4,18) - HK73*P(5,18)); -Kfusion(19) = HK81*(HK29*P(3,19) + HK30*P(2,19) + HK32*P(0,19) - HK34*P(1,19) + HK71*P(6,19) + HK72*P(4,19) - HK73*P(5,19)); -Kfusion(20) = HK81*(HK29*P(3,20) + HK30*P(2,20) + HK32*P(0,20) - HK34*P(1,20) + HK71*P(6,20) + HK72*P(4,20) - HK73*P(5,20)); -Kfusion(21) = HK81*(HK29*P(3,21) + HK30*P(2,21) + HK32*P(0,21) - HK34*P(1,21) + HK71*P(6,21) + HK72*P(4,21) - HK73*P(5,21)); -Kfusion(22) = HK81*(HK29*P(3,22) + HK30*P(2,22) + HK32*P(0,22) - HK34*P(1,22) + HK71*P(6,22) + HK72*P(4,22) - HK73*P(5,22)); -Kfusion(23) = HK81*(HK29*P(3,23) + HK30*P(2,23) + HK32*P(0,23) - HK34*P(1,23) + HK71*P(6,23) + HK72*P(4,23) - HK73*P(5,23)); +Kfusion(0) = -HK78*HK85; +Kfusion(1) = -HK84*HK85; +Kfusion(2) = -HK83*HK85; +Kfusion(3) = -HK80*HK85; +Kfusion(4) = -HK79*HK85; +Kfusion(5) = -HK81*HK85; +Kfusion(6) = -HK82*HK85; +Kfusion(7) = -HK85*(-HK71*P(3,7) - HK72*P(4,7) + HK73*P(0,7) + HK74*P(5,7) - HK75*P(2,7) + HK76*P(6,7) + HK77*P(1,7)); +Kfusion(8) = -HK85*(-HK71*P(3,8) - HK72*P(4,8) + HK73*P(0,8) + HK74*P(5,8) - HK75*P(2,8) + HK76*P(6,8) + HK77*P(1,8)); +Kfusion(9) = -HK85*(-HK71*P(3,9) - HK72*P(4,9) + HK73*P(0,9) + HK74*P(5,9) - HK75*P(2,9) + HK76*P(6,9) + HK77*P(1,9)); +Kfusion(10) = -HK85*(-HK71*P(3,10) - HK72*P(4,10) + HK73*P(0,10) + HK74*P(5,10) - HK75*P(2,10) + HK76*P(6,10) + HK77*P(1,10)); +Kfusion(11) = -HK85*(-HK71*P(3,11) - HK72*P(4,11) + HK73*P(0,11) + HK74*P(5,11) - HK75*P(2,11) + HK76*P(6,11) + HK77*P(1,11)); +Kfusion(12) = -HK85*(-HK71*P(3,12) - HK72*P(4,12) + HK73*P(0,12) + HK74*P(5,12) - HK75*P(2,12) + HK76*P(6,12) + HK77*P(1,12)); +Kfusion(13) = -HK85*(-HK71*P(3,13) - HK72*P(4,13) + HK73*P(0,13) + HK74*P(5,13) - HK75*P(2,13) + HK76*P(6,13) + HK77*P(1,13)); +Kfusion(14) = -HK85*(-HK71*P(3,14) - HK72*P(4,14) + HK73*P(0,14) + HK74*P(5,14) - HK75*P(2,14) + HK76*P(6,14) + HK77*P(1,14)); +Kfusion(15) = -HK85*(-HK71*P(3,15) - HK72*P(4,15) + HK73*P(0,15) + HK74*P(5,15) - HK75*P(2,15) + HK76*P(6,15) + HK77*P(1,15)); +Kfusion(16) = -HK85*(-HK71*P(3,16) - HK72*P(4,16) + HK73*P(0,16) + HK74*P(5,16) - HK75*P(2,16) + HK76*P(6,16) + HK77*P(1,16)); +Kfusion(17) = -HK85*(-HK71*P(3,17) - HK72*P(4,17) + HK73*P(0,17) + HK74*P(5,17) - HK75*P(2,17) + HK76*P(6,17) + HK77*P(1,17)); +Kfusion(18) = -HK85*(-HK71*P(3,18) - HK72*P(4,18) + HK73*P(0,18) + HK74*P(5,18) - HK75*P(2,18) + HK76*P(6,18) + HK77*P(1,18)); +Kfusion(19) = -HK85*(-HK71*P(3,19) - HK72*P(4,19) + HK73*P(0,19) + HK74*P(5,19) - HK75*P(2,19) + HK76*P(6,19) + HK77*P(1,19)); +Kfusion(20) = -HK85*(-HK71*P(3,20) - HK72*P(4,20) + HK73*P(0,20) + HK74*P(5,20) - HK75*P(2,20) + HK76*P(6,20) + HK77*P(1,20)); +Kfusion(21) = -HK85*(-HK71*P(3,21) - HK72*P(4,21) + HK73*P(0,21) + HK74*P(5,21) - HK75*P(2,21) + HK76*P(6,21) + HK77*P(1,21)); +Kfusion(22) = -HK85*(-HK71*P(3,22) - HK72*P(4,22) + HK73*P(0,22) + HK74*P(5,22) - HK75*P(2,22) + HK76*P(6,22) + HK77*P(1,22)); +Kfusion(23) = -HK85*(-HK71*P(3,23) - HK72*P(4,23) + HK73*P(0,23) + HK74*P(5,23) - HK75*P(2,23) + HK76*P(6,23) + HK77*P(1,23)); diff --git a/EKF/python/ekf_derivation/generated/yaw_generated.cpp b/EKF/python/ekf_derivation/generated/yaw_generated.cpp index b2f8a50985..cbed2c8658 100644 --- a/EKF/python/ekf_derivation/generated/yaw_generated.cpp +++ b/EKF/python/ekf_derivation/generated/yaw_generated.cpp @@ -1,20 +1,19 @@ // calculate 321 yaw observation matrix - option A -const float SA0 = 2*q3; -const float SA1 = 2*q2; -const float SA2 = SA0*q0 + SA1*q1; -const float SA3 = powf(q0, 2) + powf(q1, 2) - powf(q2, 2) - powf(q3, 2); +const float SA0 = 2*q0; +const float SA1 = 2*q1; +const float SA2 = SA0*q3 + SA1*q2; +const float SA3 = -2*powf(q2, 2) - 2*powf(q3, 2) + 1; const float SA4 = powf(SA3, -2); const float SA5 = 1.0F/(powf(SA2, 2)*SA4 + 1); const float SA6 = 1.0F/SA3; -const float SA7 = SA2*SA4; -const float SA8 = 2*SA7; -const float SA9 = 2*SA6; +const float SA7 = 2*SA5*SA6; +const float SA8 = 4*SA2*SA4; -H_YAW.at<0>() = SA5*(SA0*SA6 - SA8*q0); -H_YAW.at<1>() = SA5*(SA1*SA6 - SA8*q1); -H_YAW.at<2>() = SA5*(SA1*SA7 + SA9*q1); -H_YAW.at<3>() = SA5*(SA0*SA7 + SA9*q0); +H_YAW.at<0>() = SA7*q3; +H_YAW.at<1>() = SA7*q2; +H_YAW.at<2>() = SA5*(SA1*SA6 + SA8*q2); +H_YAW.at<3>() = SA5*(SA0*SA6 + SA8*q3); H_YAW.at<4>() = 0; H_YAW.at<5>() = 0; H_YAW.at<6>() = 0; @@ -42,18 +41,17 @@ const float SB0 = 2*q0; const float SB1 = 2*q1; const float SB2 = SB0*q3 + SB1*q2; const float SB3 = powf(SB2, -2); -const float SB4 = powf(q0, 2) + powf(q1, 2) - powf(q2, 2) - powf(q3, 2); +const float SB4 = -2*powf(q2, 2) - 2*powf(q3, 2) + 1; const float SB5 = 1.0F/(SB3*powf(SB4, 2) + 1); -const float SB6 = 1.0F/SB2; -const float SB7 = SB3*SB4; -const float SB8 = 2*SB7; -const float SB9 = 2*SB6; +const float SB6 = SB3*SB4; +const float SB7 = 2*SB5*SB6; +const float SB8 = 4/SB2; -H_YAW.at<0>() = -SB5*(SB0*SB6 - SB8*q3); -H_YAW.at<1>() = -SB5*(SB1*SB6 - SB8*q2); -H_YAW.at<2>() = -SB5*(-SB1*SB7 - SB9*q2); -H_YAW.at<3>() = -SB5*(-SB0*SB7 - SB9*q3); +H_YAW.at<0>() = SB7*q3; +H_YAW.at<1>() = SB7*q2; +H_YAW.at<2>() = -SB5*(-SB1*SB6 - SB8*q2); +H_YAW.at<3>() = -SB5*(-SB0*SB6 - SB8*q3); H_YAW.at<4>() = 0; H_YAW.at<5>() = 0; H_YAW.at<6>() = 0; @@ -77,22 +75,21 @@ H_YAW.at<23>() = 0; // calculate 312 yaw observation matrix - option A -const float SA0 = 2*q3; +const float SA0 = 2*q0; const float SA1 = 2*q2; -const float SA2 = SA0*q0 - SA1*q1; -const float SA3 = powf(q0, 2) - powf(q1, 2) + powf(q2, 2) - powf(q3, 2); +const float SA2 = SA0*q3 - SA1*q1; +const float SA3 = -2*powf(q1, 2) - 2*powf(q3, 2) + 1; const float SA4 = powf(SA3, -2); const float SA5 = 1.0F/(powf(SA2, 2)*SA4 + 1); const float SA6 = 1.0F/SA3; -const float SA7 = SA2*SA4; -const float SA8 = 2*SA7; -const float SA9 = 2*SA6; +const float SA7 = 2*SA5*SA6; +const float SA8 = 4*SA2*SA4; -H_YAW.at<0>() = SA5*(SA0*SA6 - SA8*q0); +H_YAW.at<0>() = SA7*q3; H_YAW.at<1>() = SA5*(-SA1*SA6 + SA8*q1); -H_YAW.at<2>() = SA5*(-SA1*SA7 - SA9*q1); -H_YAW.at<3>() = SA5*(SA0*SA7 + SA9*q0); +H_YAW.at<2>() = -SA7*q1; +H_YAW.at<3>() = SA5*(SA0*SA6 + SA8*q3); H_YAW.at<4>() = 0; H_YAW.at<5>() = 0; H_YAW.at<6>() = 0; @@ -117,21 +114,20 @@ H_YAW.at<23>() = 0; // calculate 312 yaw observation matrix - option B const float SB0 = 2*q0; -const float SB1 = 2*q1; -const float SB2 = -SB0*q3 + SB1*q2; +const float SB1 = 2*q2; +const float SB2 = -SB0*q3 + SB1*q1; const float SB3 = powf(SB2, -2); -const float SB4 = -powf(q0, 2) + powf(q1, 2) - powf(q2, 2) + powf(q3, 2); +const float SB4 = 2*powf(q1, 2) + 2*powf(q3, 2) - 1; const float SB5 = 1.0F/(SB3*powf(SB4, 2) + 1); -const float SB6 = 1.0F/SB2; -const float SB7 = SB3*SB4; -const float SB8 = 2*SB7; -const float SB9 = 2*SB6; +const float SB6 = SB3*SB4; +const float SB7 = 2*SB5*SB6; +const float SB8 = 4/SB2; -H_YAW.at<0>() = -SB5*(-SB0*SB6 + SB8*q3); -H_YAW.at<1>() = -SB5*(SB1*SB6 - SB8*q2); -H_YAW.at<2>() = -SB5*(-SB1*SB7 - SB9*q2); -H_YAW.at<3>() = -SB5*(SB0*SB7 + SB9*q3); +H_YAW.at<0>() = -SB7*q3; +H_YAW.at<1>() = -SB5*(-SB1*SB6 + SB8*q1); +H_YAW.at<2>() = SB7*q1; +H_YAW.at<3>() = -SB5*(SB0*SB6 + SB8*q3); H_YAW.at<4>() = 0; H_YAW.at<5>() = 0; H_YAW.at<6>() = 0;