fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 15:00:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2_derivation: update generated equations with SymPy 1.10.1

Browse Source
This commit is contained in:
bresch
2022-07-01 17:26:15 +02:00
committed by Mathieu Bresciani
parent dff946c39a
commit 11f78a3686
17 changed files with 1354 additions and 1587 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated.cpp
+157 -256
View File
@@ -1,83 +1,35 @@
// Axis 0 equations
// Sub Expressions
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);
const float HKX0 = magD*q2 - magE*q3;
const float HKX1 = magD*q3 + magE*q2;
const float HKX2 = 2*magN;
const float HKX3 = HKX2*q2 + magD*q0 - magE*q1;
const float HKX4 = -HKX2*q3 + magD*q1 + magE*q0;
const float HKX5 = 2*powf(q2, 2) + 2*powf(q3, 2) - 1;
const float HKX6 = q0*q3 + q1*q2;
const float HKX7 = q0*q2 - q1*q3;
const float HKX8 = 2*HKX0;
const float HKX9 = 2*HKX7;
const float HKX10 = 2*HKX3;
const float HKX11 = -2*HKX1*P(0,1) + HKX10*P(0,2) - 2*HKX4*P(0,3) + HKX5*P(0,16) - 2*HKX6*P(0,17) + HKX8*P(0,0) + HKX9*P(0,18) - P(0,19);
const float HKX12 = -2*HKX1*P(1,16) + HKX10*P(2,16) - 2*HKX4*P(3,16) + HKX5*P(16,16) - 2*HKX6*P(16,17) + HKX8*P(0,16) + HKX9*P(16,18) - P(16,19);
const float HKX13 = -2*HKX1*P(1,1) + HKX10*P(1,2) - 2*HKX4*P(1,3) + HKX5*P(1,16) - 2*HKX6*P(1,17) + HKX8*P(0,1) + HKX9*P(1,18) - P(1,19);
const float HKX14 = 2*HKX1;
const float HKX15 = -2*HKX1*P(1,17) + HKX10*P(2,17) - 2*HKX4*P(3,17) + HKX5*P(16,17) - 2*HKX6*P(17,17) + HKX8*P(0,17) + HKX9*P(17,18) - P(17,19);
const float HKX16 = 2*HKX6;
const float HKX17 = -2*HKX1*P(1,3) + HKX10*P(2,3) - 2*HKX4*P(3,3) + HKX5*P(3,16) - 2*HKX6*P(3,17) + HKX8*P(0,3) + HKX9*P(3,18) - P(3,19);
const float HKX18 = 2*HKX4;
const float HKX19 = -2*HKX1*P(1,18) + HKX10*P(2,18) - 2*HKX4*P(3,18) + HKX5*P(16,18) - 2*HKX6*P(17,18) + HKX8*P(0,18) + HKX9*P(18,18) - P(18,19);
const float HKX20 = -2*HKX1*P(1,2) + HKX10*P(2,2) - 2*HKX4*P(2,3) + HKX5*P(2,16) - 2*HKX6*P(2,17) + HKX8*P(0,2) + HKX9*P(2,18) - P(2,19);
const float HKX21 = HKX10*P(2,19) - HKX14*P(1,19) - HKX16*P(17,19) - HKX18*P(3,19) + HKX5*P(16,19) + HKX8*P(0,19) + HKX9*P(18,19) - P(19,19);
const float HKX22 = 1.0F/(-HKX10*HKX20 - HKX11*HKX8 - HKX12*HKX5 + HKX13*HKX14 + HKX15*HKX16 + HKX17*HKX18 - HKX19*HKX9 + HKX21 - R_MAG);
// Observation Jacobians
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<0>() = -2*HKX0;
Hfusion.at<1>() = 2*HKX1;
Hfusion.at<2>() = -2*HKX3;
Hfusion.at<3>() = 2*HKX4;
Hfusion.at<4>() = 0;
Hfusion.at<5>() = 0;
Hfusion.at<6>() = 0;
@@ -90,9 +42,9 @@ Hfusion.at<12>() = 0;
Hfusion.at<13>() = 0;
Hfusion.at<14>() = 0;
Hfusion.at<15>() = 0;
Hfusion.at<16>() = -HKX8 - HKX9 + 1;
Hfusion.at<17>() = 2*HKX10;
Hfusion.at<18>() = 2*HKX11 - 2*HKX12;
Hfusion.at<16>() = -HKX5;
Hfusion.at<17>() = 2*HKX6;
Hfusion.at<18>() = -2*HKX7;
Hfusion.at<19>() = 1;
Hfusion.at<20>() = 0;
Hfusion.at<21>() = 0;
@@ -101,30 +53,30 @@ Hfusion.at<23>() = 0;
// Kalman gains
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));
Kfusion(0) = HKX11*HKX22;
Kfusion(1) = HKX13*HKX22;
Kfusion(2) = HKX20*HKX22;
Kfusion(3) = HKX17*HKX22;
Kfusion(4) = HKX22*(HKX10*P(2,4) - HKX14*P(1,4) - HKX16*P(4,17) - HKX18*P(3,4) + HKX5*P(4,16) + HKX8*P(0,4) + HKX9*P(4,18) - P(4,19));
Kfusion(5) = HKX22*(HKX10*P(2,5) - HKX14*P(1,5) - HKX16*P(5,17) - HKX18*P(3,5) + HKX5*P(5,16) + HKX8*P(0,5) + HKX9*P(5,18) - P(5,19));
Kfusion(6) = HKX22*(HKX10*P(2,6) - HKX14*P(1,6) - HKX16*P(6,17) - HKX18*P(3,6) + HKX5*P(6,16) + HKX8*P(0,6) + HKX9*P(6,18) - P(6,19));
Kfusion(7) = HKX22*(HKX10*P(2,7) - HKX14*P(1,7) - HKX16*P(7,17) - HKX18*P(3,7) + HKX5*P(7,16) + HKX8*P(0,7) + HKX9*P(7,18) - P(7,19));
Kfusion(8) = HKX22*(HKX10*P(2,8) - HKX14*P(1,8) - HKX16*P(8,17) - HKX18*P(3,8) + HKX5*P(8,16) + HKX8*P(0,8) + HKX9*P(8,18) - P(8,19));
Kfusion(9) = HKX22*(HKX10*P(2,9) - HKX14*P(1,9) - HKX16*P(9,17) - HKX18*P(3,9) + HKX5*P(9,16) + HKX8*P(0,9) + HKX9*P(9,18) - P(9,19));
Kfusion(10) = HKX22*(HKX10*P(2,10) - HKX14*P(1,10) - HKX16*P(10,17) - HKX18*P(3,10) + HKX5*P(10,16) + HKX8*P(0,10) + HKX9*P(10,18) - P(10,19));
Kfusion(11) = HKX22*(HKX10*P(2,11) - HKX14*P(1,11) - HKX16*P(11,17) - HKX18*P(3,11) + HKX5*P(11,16) + HKX8*P(0,11) + HKX9*P(11,18) - P(11,19));
Kfusion(12) = HKX22*(HKX10*P(2,12) - HKX14*P(1,12) - HKX16*P(12,17) - HKX18*P(3,12) + HKX5*P(12,16) + HKX8*P(0,12) + HKX9*P(12,18) - P(12,19));
Kfusion(13) = HKX22*(HKX10*P(2,13) - HKX14*P(1,13) - HKX16*P(13,17) - HKX18*P(3,13) + HKX5*P(13,16) + HKX8*P(0,13) + HKX9*P(13,18) - P(13,19));
Kfusion(14) = HKX22*(HKX10*P(2,14) - HKX14*P(1,14) - HKX16*P(14,17) - HKX18*P(3,14) + HKX5*P(14,16) + HKX8*P(0,14) + HKX9*P(14,18) - P(14,19));
Kfusion(15) = HKX22*(HKX10*P(2,15) - HKX14*P(1,15) - HKX16*P(15,17) - HKX18*P(3,15) + HKX5*P(15,16) + HKX8*P(0,15) + HKX9*P(15,18) - P(15,19));
Kfusion(16) = HKX12*HKX22;
Kfusion(17) = HKX15*HKX22;
Kfusion(18) = HKX19*HKX22;
Kfusion(19) = HKX21*HKX22;
Kfusion(20) = HKX22*(HKX10*P(2,20) - HKX14*P(1,20) - HKX16*P(17,20) - HKX18*P(3,20) + HKX5*P(16,20) + HKX8*P(0,20) + HKX9*P(18,20) - P(19,20));
Kfusion(21) = HKX22*(HKX10*P(2,21) - HKX14*P(1,21) - HKX16*P(17,21) - HKX18*P(3,21) + HKX5*P(16,21) + HKX8*P(0,21) + HKX9*P(18,21) - P(19,21));
Kfusion(22) = HKX22*(HKX10*P(2,22) - HKX14*P(1,22) - HKX16*P(17,22) - HKX18*P(3,22) + HKX5*P(16,22) + HKX8*P(0,22) + HKX9*P(18,22) - P(19,22));
Kfusion(23) = HKX22*(HKX10*P(2,23) - HKX14*P(1,23) - HKX16*P(17,23) - HKX18*P(3,23) + HKX5*P(16,23) + HKX8*P(0,23) + HKX9*P(18,23) - P(19,23));
// Axis 1 equations
@@ -133,37 +85,32 @@ 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);
const float HKY4 = HKY1*q3 - magD*q2 + magN*q0;
const float HKY5 = q0*q3 - q1*q2;
const float HKY6 = 2*powf(q1, 2) + 2*powf(q3, 2) - 1;
const float HKY7 = q0*q1 + q2*q3;
const float HKY8 = 2*HKY7;
const float HKY9 = 2*HKY3;
const float HKY10 = 2*HKY0;
const float HKY11 = 2*HKY5;
const float HKY12 = 2*HKY2;
const float HKY13 = 2*HKY4;
const float HKY14 = HKY10*P(0,0) - HKY11*P(0,16) + HKY12*P(0,1) - HKY13*P(0,3) - HKY6*P(0,17) + HKY8*P(0,18) + HKY9*P(0,2) + P(0,20);
const float HKY15 = HKY10*P(0,17) - HKY11*P(16,17) + HKY12*P(1,17) - HKY13*P(3,17) - HKY6*P(17,17) + HKY8*P(17,18) + HKY9*P(2,17) + P(17,20);
const float HKY16 = HKY10*P(0,16) - HKY11*P(16,16) + HKY12*P(1,16) - HKY13*P(3,16) - HKY6*P(16,17) + HKY8*P(16,18) + HKY9*P(2,16) + P(16,20);
const float HKY17 = HKY10*P(0,3) - HKY11*P(3,16) + HKY12*P(1,3) - HKY13*P(3,3) - HKY6*P(3,17) + HKY8*P(3,18) + HKY9*P(2,3) + P(3,20);
const float HKY18 = HKY10*P(0,2) - HKY11*P(2,16) + HKY12*P(1,2) - HKY13*P(2,3) - HKY6*P(2,17) + HKY8*P(2,18) + HKY9*P(2,2) + P(2,20);
const float HKY19 = HKY10*P(0,18) - HKY11*P(16,18) + HKY12*P(1,18) - HKY13*P(3,18) - HKY6*P(17,18) + HKY8*P(18,18) + HKY9*P(2,18) + P(18,20);
const float HKY20 = HKY10*P(0,1) - HKY11*P(1,16) + HKY12*P(1,1) - HKY13*P(1,3) - HKY6*P(1,17) + HKY8*P(1,18) + HKY9*P(1,2) + P(1,20);
const float HKY21 = HKY10*P(0,20) - HKY11*P(16,20) + HKY12*P(1,20) - HKY13*P(3,20) - HKY6*P(17,20) + HKY8*P(18,20) + HKY9*P(2,20) + P(20,20);
const float HKY22 = 1.0F/(HKY10*HKY14 - HKY11*HKY16 + HKY12*HKY20 - HKY13*HKY17 - HKY15*HKY6 + HKY18*HKY9 + HKY19*HKY8 + HKY21 + R_MAG);
// Observation Jacobians
Hfusion.at<0>() = 2*HKY0;
Hfusion.at<1>() = 2*HKY2;
Hfusion.at<2>() = 2*HKY3;
Hfusion.at<3>() = 2*HKY4 - 2*HKY5 - 2*HKY6;
Hfusion.at<3>() = -2*HKY4;
Hfusion.at<4>() = 0;
Hfusion.at<5>() = 0;
Hfusion.at<6>() = 0;
@@ -176,9 +123,9 @@ Hfusion.at<12>() = 0;
Hfusion.at<13>() = 0;
Hfusion.at<14>() = 0;
Hfusion.at<15>() = 0;
Hfusion.at<16>() = 2*HKY7 - 2*HKY8;
Hfusion.at<17>() = -HKY10 - HKY9 + 1;
Hfusion.at<18>() = 2*HKY11;
Hfusion.at<16>() = -2*HKY5;
Hfusion.at<17>() = -HKY6;
Hfusion.at<18>() = 2*HKY7;
Hfusion.at<19>() = 0;
Hfusion.at<20>() = 1;
Hfusion.at<21>() = 0;
@@ -187,112 +134,64 @@ Hfusion.at<23>() = 0;
// Kalman gains
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));
Kfusion(0) = HKY14*HKY22;
Kfusion(1) = HKY20*HKY22;
Kfusion(2) = HKY18*HKY22;
Kfusion(3) = HKY17*HKY22;
Kfusion(4) = HKY22*(HKY10*P(0,4) - HKY11*P(4,16) + HKY12*P(1,4) - HKY13*P(3,4) - HKY6*P(4,17) + HKY8*P(4,18) + HKY9*P(2,4) + P(4,20));
Kfusion(5) = HKY22*(HKY10*P(0,5) - HKY11*P(5,16) + HKY12*P(1,5) - HKY13*P(3,5) - HKY6*P(5,17) + HKY8*P(5,18) + HKY9*P(2,5) + P(5,20));
Kfusion(6) = HKY22*(HKY10*P(0,6) - HKY11*P(6,16) + HKY12*P(1,6) - HKY13*P(3,6) - HKY6*P(6,17) + HKY8*P(6,18) + HKY9*P(2,6) + P(6,20));
Kfusion(7) = HKY22*(HKY10*P(0,7) - HKY11*P(7,16) + HKY12*P(1,7) - HKY13*P(3,7) - HKY6*P(7,17) + HKY8*P(7,18) + HKY9*P(2,7) + P(7,20));
Kfusion(8) = HKY22*(HKY10*P(0,8) - HKY11*P(8,16) + HKY12*P(1,8) - HKY13*P(3,8) - HKY6*P(8,17) + HKY8*P(8,18) + HKY9*P(2,8) + P(8,20));
Kfusion(9) = HKY22*(HKY10*P(0,9) - HKY11*P(9,16) + HKY12*P(1,9) - HKY13*P(3,9) - HKY6*P(9,17) + HKY8*P(9,18) + HKY9*P(2,9) + P(9,20));
Kfusion(10) = HKY22*(HKY10*P(0,10) - HKY11*P(10,16) + HKY12*P(1,10) - HKY13*P(3,10) - HKY6*P(10,17) + HKY8*P(10,18) + HKY9*P(2,10) + P(10,20));
Kfusion(11) = HKY22*(HKY10*P(0,11) - HKY11*P(11,16) + HKY12*P(1,11) - HKY13*P(3,11) - HKY6*P(11,17) + HKY8*P(11,18) + HKY9*P(2,11) + P(11,20));
Kfusion(12) = HKY22*(HKY10*P(0,12) - HKY11*P(12,16) + HKY12*P(1,12) - HKY13*P(3,12) - HKY6*P(12,17) + HKY8*P(12,18) + HKY9*P(2,12) + P(12,20));
Kfusion(13) = HKY22*(HKY10*P(0,13) - HKY11*P(13,16) + HKY12*P(1,13) - HKY13*P(3,13) - HKY6*P(13,17) + HKY8*P(13,18) + HKY9*P(2,13) + P(13,20));
Kfusion(14) = HKY22*(HKY10*P(0,14) - HKY11*P(14,16) + HKY12*P(1,14) - HKY13*P(3,14) - HKY6*P(14,17) + HKY8*P(14,18) + HKY9*P(2,14) + P(14,20));
Kfusion(15) = HKY22*(HKY10*P(0,15) - HKY11*P(15,16) + HKY12*P(1,15) - HKY13*P(3,15) - HKY6*P(15,17) + HKY8*P(15,18) + HKY9*P(2,15) + P(15,20));
Kfusion(16) = HKY16*HKY22;
Kfusion(17) = HKY15*HKY22;
Kfusion(18) = HKY19*HKY22;
Kfusion(19) = HKY22*(HKY10*P(0,19) - HKY11*P(16,19) + HKY12*P(1,19) - HKY13*P(3,19) - HKY6*P(17,19) + HKY8*P(18,19) + HKY9*P(2,19) + P(19,20));
Kfusion(20) = HKY21*HKY22;
Kfusion(21) = HKY22*(HKY10*P(0,21) - HKY11*P(16,21) + HKY12*P(1,21) - HKY13*P(3,21) - HKY6*P(17,21) + HKY8*P(18,21) + HKY9*P(2,21) + P(20,21));
Kfusion(22) = HKY22*(HKY10*P(0,22) - HKY11*P(16,22) + HKY12*P(1,22) - HKY13*P(3,22) - HKY6*P(17,22) + HKY8*P(18,22) + HKY9*P(2,22) + P(20,22));
Kfusion(23) = HKY22*(HKY10*P(0,23) - HKY11*P(16,23) + HKY12*P(1,23) - HKY13*P(3,23) - HKY6*P(17,23) + HKY8*P(18,23) + HKY9*P(2,23) + P(20,23));
// Axis 2 equations
// Sub Expressions
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);
const float HKZ0 = magE*q1 - magN*q2;
const float HKZ1 = 2*magD;
const float HKZ2 = HKZ1*q1 + magE*q0 - magN*q3;
const float HKZ3 = -HKZ1*q2 + magE*q3 + magN*q0;
const float HKZ4 = magE*q2 + magN*q1;
const float HKZ5 = q0*q2 + q1*q3;
const float HKZ6 = q0*q1 - q2*q3;
const float HKZ7 = 2*powf(q1, 2) + 2*powf(q2, 2) - 1;
const float HKZ8 = 2*HKZ0;
const float HKZ9 = 2*HKZ6;
const float HKZ10 = 2*HKZ2;
const float HKZ11 = HKZ10*P(0,1) - 2*HKZ3*P(0,2) - 2*HKZ4*P(0,3) - 2*HKZ5*P(0,16) + HKZ7*P(0,18) + HKZ8*P(0,0) + HKZ9*P(0,17) - P(0,21);
const float HKZ12 = HKZ10*P(1,18) - 2*HKZ3*P(2,18) - 2*HKZ4*P(3,18) - 2*HKZ5*P(16,18) + HKZ7*P(18,18) + HKZ8*P(0,18) + HKZ9*P(17,18) - P(18,21);
const float HKZ13 = HKZ10*P(1,3) - 2*HKZ3*P(2,3) - 2*HKZ4*P(3,3) - 2*HKZ5*P(3,16) + HKZ7*P(3,18) + HKZ8*P(0,3) + HKZ9*P(3,17) - P(3,21);
const float HKZ14 = 2*HKZ4;
const float HKZ15 = HKZ10*P(1,16) - 2*HKZ3*P(2,16) - 2*HKZ4*P(3,16) - 2*HKZ5*P(16,16) + HKZ7*P(16,18) + HKZ8*P(0,16) + HKZ9*P(16,17) - P(16,21);
const float HKZ16 = 2*HKZ5;
const float HKZ17 = HKZ10*P(1,2) - 2*HKZ3*P(2,2) - 2*HKZ4*P(2,3) - 2*HKZ5*P(2,16) + HKZ7*P(2,18) + HKZ8*P(0,2) + HKZ9*P(2,17) - P(2,21);
const float HKZ18 = 2*HKZ3;
const float HKZ19 = HKZ10*P(1,17) - 2*HKZ3*P(2,17) - 2*HKZ4*P(3,17) - 2*HKZ5*P(16,17) + HKZ7*P(17,18) + HKZ8*P(0,17) + HKZ9*P(17,17) - P(17,21);
const float HKZ20 = HKZ10*P(1,1) - 2*HKZ3*P(1,2) - 2*HKZ4*P(1,3) - 2*HKZ5*P(1,16) + HKZ7*P(1,18) + HKZ8*P(0,1) + HKZ9*P(1,17) - P(1,21);
const float HKZ21 = HKZ10*P(1,21) - HKZ14*P(3,21) - HKZ16*P(16,21) - HKZ18*P(2,21) + HKZ7*P(18,21) + HKZ8*P(0,21) + HKZ9*P(17,21) - P(21,21);
const float HKZ22 = 1.0F/(-HKZ10*HKZ20 - HKZ11*HKZ8 - HKZ12*HKZ7 + HKZ13*HKZ14 + HKZ15*HKZ16 + HKZ17*HKZ18 - HKZ19*HKZ9 + HKZ21 - R_MAG);
// Observation Jacobians
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<0>() = -2*HKZ0;
Hfusion.at<1>() = -2*HKZ2;
Hfusion.at<2>() = 2*HKZ3;
Hfusion.at<3>() = 2*HKZ4;
Hfusion.at<4>() = 0;
Hfusion.at<5>() = 0;
Hfusion.at<6>() = 0;
@@ -305,9 +204,9 @@ Hfusion.at<12>() = 0;
Hfusion.at<13>() = 0;
Hfusion.at<14>() = 0;
Hfusion.at<15>() = 0;
Hfusion.at<16>() = 2*HKZ8;
Hfusion.at<17>() = -2*HKZ10 + 2*HKZ9;
Hfusion.at<18>() = -HKZ11 - HKZ12 + 1;
Hfusion.at<16>() = 2*HKZ5;
Hfusion.at<17>() = -2*HKZ6;
Hfusion.at<18>() = -HKZ7;
Hfusion.at<19>() = 0;
Hfusion.at<20>() = 0;
Hfusion.at<21>() = 1;
@@ -316,27 +215,29 @@ Hfusion.at<23>() = 0;
// Kalman gains
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));
Kfusion(0) = HKZ11*HKZ22;
Kfusion(1) = HKZ20*HKZ22;
Kfusion(2) = HKZ17*HKZ22;
Kfusion(3) = HKZ13*HKZ22;
Kfusion(4) = HKZ22*(HKZ10*P(1,4) - HKZ14*P(3,4) - HKZ16*P(4,16) - HKZ18*P(2,4) + HKZ7*P(4,18) + HKZ8*P(0,4) + HKZ9*P(4,17) - P(4,21));
Kfusion(5) = HKZ22*(HKZ10*P(1,5) - HKZ14*P(3,5) - HKZ16*P(5,16) - HKZ18*P(2,5) + HKZ7*P(5,18) + HKZ8*P(0,5) + HKZ9*P(5,17) - P(5,21));
Kfusion(6) = HKZ22*(HKZ10*P(1,6) - HKZ14*P(3,6) - HKZ16*P(6,16) - HKZ18*P(2,6) + HKZ7*P(6,18) + HKZ8*P(0,6) + HKZ9*P(6,17) - P(6,21));
Kfusion(7) = HKZ22*(HKZ10*P(1,7) - HKZ14*P(3,7) - HKZ16*P(7,16) - HKZ18*P(2,7) + HKZ7*P(7,18) + HKZ8*P(0,7) + HKZ9*P(7,17) - P(7,21));
Kfusion(8) = HKZ22*(HKZ10*P(1,8) - HKZ14*P(3,8) - HKZ16*P(8,16) - HKZ18*P(2,8) + HKZ7*P(8,18) + HKZ8*P(0,8) + HKZ9*P(8,17) - P(8,21));
Kfusion(9) = HKZ22*(HKZ10*P(1,9) - HKZ14*P(3,9) - HKZ16*P(9,16) - HKZ18*P(2,9) + HKZ7*P(9,18) + HKZ8*P(0,9) + HKZ9*P(9,17) - P(9,21));
Kfusion(10) = HKZ22*(HKZ10*P(1,10) - HKZ14*P(3,10) - HKZ16*P(10,16) - HKZ18*P(2,10) + HKZ7*P(10,18) + HKZ8*P(0,10) + HKZ9*P(10,17) - P(10,21));
Kfusion(11) = HKZ22*(HKZ10*P(1,11) - HKZ14*P(3,11) - HKZ16*P(11,16) - HKZ18*P(2,11) + HKZ7*P(11,18) + HKZ8*P(0,11) + HKZ9*P(11,17) - P(11,21));
Kfusion(12) = HKZ22*(HKZ10*P(1,12) - HKZ14*P(3,12) - HKZ16*P(12,16) - HKZ18*P(2,12) + HKZ7*P(12,18) + HKZ8*P(0,12) + HKZ9*P(12,17) - P(12,21));
Kfusion(13) = HKZ22*(HKZ10*P(1,13) - HKZ14*P(3,13) - HKZ16*P(13,16) - HKZ18*P(2,13) + HKZ7*P(13,18) + HKZ8*P(0,13) + HKZ9*P(13,17) - P(13,21));
Kfusion(14) = HKZ22*(HKZ10*P(1,14) - HKZ14*P(3,14) - HKZ16*P(14,16) - HKZ18*P(2,14) + HKZ7*P(14,18) + HKZ8*P(0,14) + HKZ9*P(14,17) - P(14,21));
Kfusion(15) = HKZ22*(HKZ10*P(1,15) - HKZ14*P(3,15) - HKZ16*P(15,16) - HKZ18*P(2,15) + HKZ7*P(15,18) + HKZ8*P(0,15) + HKZ9*P(15,17) - P(15,21));
Kfusion(16) = HKZ15*HKZ22;
Kfusion(17) = HKZ19*HKZ22;
Kfusion(18) = HKZ12*HKZ22;
Kfusion(19) = HKZ22*(HKZ10*P(1,19) - HKZ14*P(3,19) - HKZ16*P(16,19) - HKZ18*P(2,19) + HKZ7*P(18,19) + HKZ8*P(0,19) + HKZ9*P(17,19) - P(19,21));
Kfusion(20) = HKZ22*(HKZ10*P(1,20) - HKZ14*P(3,20) - HKZ16*P(16,20) - HKZ18*P(2,20) + HKZ7*P(18,20) + HKZ8*P(0,20) + HKZ9*P(17,20) - P(20,21));
Kfusion(21) = HKZ21*HKZ22;
Kfusion(22) = HKZ22*(HKZ10*P(1,22) - HKZ14*P(3,22) - HKZ16*P(16,22) - HKZ18*P(2,22) + HKZ7*P(18,22) + HKZ8*P(0,22) + HKZ9*P(17,22) - P(21,22));
Kfusion(23) = HKZ22*(HKZ10*P(1,23) - HKZ14*P(3,23) - HKZ16*P(16,23) - HKZ18*P(2,23) + HKZ7*P(18,23) + HKZ8*P(0,23) + HKZ9*P(17,23) - P(21,23));
src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated_alt.cpp
+179 -267
View File
@@ -1,186 +1,98 @@
// Sub Expressions
const float HK0 = magE*q3;
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 HK0 = magD*q2;
const float HK1 = HK0 - magE*q3;
const float HK2 = magD*q3;
const float HK3 = magE*q2;
const float HK4 = HK2 + HK3;
const float HK5 = magD*q0;
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 - 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 = -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);
const float HK7 = magN*q2;
const float HK8 = HK5 - HK6 + 2*HK7;
const float HK9 = magD*q1;
const float HK10 = magE*q0;
const float HK11 = magN*q3;
const float HK12 = HK10 - 2*HK11 + HK9;
const float HK13 = 2*powf(q2, 2);
const float HK14 = 2*powf(q3, 2) - 1;
const float HK15 = HK13 + HK14;
const float HK16 = q0*q3;
const float HK17 = HK16 + q1*q2;
const float HK18 = q0*q2;
const float HK19 = HK18 - q1*q3;
const float HK20 = 2*HK1;
const float HK21 = 2*HK19;
const float HK22 = 2*HK8;
const float HK23 = -2*HK12*P(0,3) + HK15*P(0,16) - 2*HK17*P(0,17) + HK20*P(0,0) + HK21*P(0,18) + HK22*P(0,2) - 2*HK4*P(0,1) - P(0,19);
const float HK24 = -R_MAG;
const float HK25 = -2*HK12*P(3,16) + HK15*P(16,16) - 2*HK17*P(16,17) + HK20*P(0,16) + HK21*P(16,18) + HK22*P(2,16) - 2*HK4*P(1,16) - P(16,19);
const float HK26 = -2*HK12*P(1,3) + HK15*P(1,16) - 2*HK17*P(1,17) + HK20*P(0,1) + HK21*P(1,18) + HK22*P(1,2) - 2*HK4*P(1,1) - P(1,19);
const float HK27 = 2*HK4;
const float HK28 = -2*HK12*P(3,17) + HK15*P(16,17) - 2*HK17*P(17,17) + HK20*P(0,17) + HK21*P(17,18) + HK22*P(2,17) - 2*HK4*P(1,17) - P(17,19);
const float HK29 = 2*HK17;
const float HK30 = -2*HK12*P(3,3) + HK15*P(3,16) - 2*HK17*P(3,17) + HK20*P(0,3) + HK21*P(3,18) + HK22*P(2,3) - 2*HK4*P(1,3) - P(3,19);
const float HK31 = 2*HK12;
const float HK32 = -2*HK12*P(3,18) + HK15*P(16,18) - 2*HK17*P(17,18) + HK20*P(0,18) + HK21*P(18,18) + HK22*P(2,18) - 2*HK4*P(1,18) - P(18,19);
const float HK33 = -2*HK12*P(2,3) + HK15*P(2,16) - 2*HK17*P(2,17) + HK20*P(0,2) + HK21*P(2,18) + HK22*P(2,2) - 2*HK4*P(1,2) - P(2,19);
const float HK34 = HK15*P(16,19) + HK20*P(0,19) + HK21*P(18,19) + HK22*P(2,19) - HK27*P(1,19) - HK29*P(17,19) - HK31*P(3,19) - P(19,19);
const float HK35 = 1.0F/(-HK15*HK25 - HK20*HK23 - HK21*HK32 - HK22*HK33 + HK24 + HK26*HK27 + HK28*HK29 + HK30*HK31 + HK34);
const float HK36 = -P(19,21);
const float HK37 = -HK11;
const float HK38 = HK37 + HK9;
const float HK39 = HK5 - 2*HK6 + HK7;
const float HK40 = magN*q1;
const float HK41 = HK2 + HK40;
const float HK42 = magN*q0;
const float HK43 = magE*q3;
const float HK44 = -HK0 + HK42 + 2*HK43;
const float HK45 = HK16 - q1*q2;
const float HK46 = 2*powf(q1, 2);
const float HK47 = HK14 + HK46;
const float HK48 = q0*q1;
const float HK49 = HK48 + q2*q3;
const float HK50 = 2*HK49;
const float HK51 = 2*HK41;
const float HK52 = 2*HK38;
const float HK53 = 2*HK45;
const float HK54 = 2*HK39;
const float HK55 = 2*HK44;
const float HK56 = -HK47*P(0,17) + HK50*P(0,18) + HK51*P(0,2) + HK52*P(0,0) - HK53*P(0,16) + HK54*P(0,1) - HK55*P(0,3) + P(0,20);
const float HK57 = -HK47*P(17,17) + HK50*P(17,18) + HK51*P(2,17) + HK52*P(0,17) - HK53*P(16,17) + HK54*P(1,17) - HK55*P(3,17) + P(17,20);
const float HK58 = -HK47*P(16,17) + HK50*P(16,18) + HK51*P(2,16) + HK52*P(0,16) - HK53*P(16,16) + HK54*P(1,16) - HK55*P(3,16) + P(16,20);
const float HK59 = -HK47*P(3,17) + HK50*P(3,18) + HK51*P(2,3) + HK52*P(0,3) - HK53*P(3,16) + HK54*P(1,3) - HK55*P(3,3) + P(3,20);
const float HK60 = -HK47*P(2,17) + HK50*P(2,18) + HK51*P(2,2) + HK52*P(0,2) - HK53*P(2,16) + HK54*P(1,2) - HK55*P(2,3) + P(2,20);
const float HK61 = -HK47*P(17,18) + HK50*P(18,18) + HK51*P(2,18) + HK52*P(0,18) - HK53*P(16,18) + HK54*P(1,18) - HK55*P(3,18) + P(18,20);
const float HK62 = -HK47*P(1,17) + HK50*P(1,18) + HK51*P(1,2) + HK52*P(0,1) - HK53*P(1,16) + HK54*P(1,1) - HK55*P(1,3) + P(1,20);
const float HK63 = -HK47*P(17,20) + HK50*P(18,20) + HK51*P(2,20) + HK52*P(0,20) - HK53*P(16,20) + HK54*P(1,20) - HK55*P(3,20) + P(20,20);
const float HK64 = 1.0F/(-HK47*HK57 + HK50*HK61 + HK51*HK60 + HK52*HK56 - HK53*HK58 + HK54*HK62 - HK55*HK59 + HK63 + R_MAG);
const float HK65 = HK6 - magN*q2;
const float HK66 = HK10 + HK37 + 2*HK9;
const float HK67 = -2*HK0 + HK42 + HK43;
const float HK68 = HK3 + HK40;
const float HK69 = HK18 + q1*q3;
const float HK70 = HK48 - q2*q3;
const float HK71 = HK13 + HK46 - 1;
const float HK72 = 2*HK65;
const float HK73 = 2*HK70;
const float HK74 = 2*HK66;
const float HK75 = -2*HK67*P(0,2) - 2*HK68*P(0,3) - 2*HK69*P(0,16) + HK71*P(0,18) + HK72*P(0,0) + HK73*P(0,17) + HK74*P(0,1) - P(0,21);
const float HK76 = -2*HK67*P(2,18) - 2*HK68*P(3,18) - 2*HK69*P(16,18) + HK71*P(18,18) + HK72*P(0,18) + HK73*P(17,18) + HK74*P(1,18) - P(18,21);
const float HK77 = -2*HK67*P(2,3) - 2*HK68*P(3,3) - 2*HK69*P(3,16) + HK71*P(3,18) + HK72*P(0,3) + HK73*P(3,17) + HK74*P(1,3) - P(3,21);
const float HK78 = 2*HK68;
const float HK79 = -2*HK67*P(2,16) - 2*HK68*P(3,16) - 2*HK69*P(16,16) + HK71*P(16,18) + HK72*P(0,16) + HK73*P(16,17) + HK74*P(1,16) - P(16,21);
const float HK80 = 2*HK69;
const float HK81 = -2*HK67*P(2,2) - 2*HK68*P(2,3) - 2*HK69*P(2,16) + HK71*P(2,18) + HK72*P(0,2) + HK73*P(2,17) + HK74*P(1,2) - P(2,21);
const float HK82 = 2*HK67;
const float HK83 = -2*HK67*P(2,17) - 2*HK68*P(3,17) - 2*HK69*P(16,17) + HK71*P(17,18) + HK72*P(0,17) + HK73*P(17,17) + HK74*P(1,17) - P(17,21);
const float HK84 = -2*HK67*P(1,2) - 2*HK68*P(1,3) - 2*HK69*P(1,16) + HK71*P(1,18) + HK72*P(0,1) + HK73*P(1,17) + HK74*P(1,1) - P(1,21);
const float HK85 = HK71*P(18,21) + HK72*P(0,21) + HK73*P(17,21) + HK74*P(1,21) - HK78*P(3,21) - HK80*P(16,21) - HK82*P(2,21) - P(21,21);
const float HK86 = 1.0F/(HK24 - HK71*HK76 - HK72*HK75 - HK73*HK83 - HK74*HK84 + HK77*HK78 + HK79*HK80 + HK81*HK82 + HK85);
// Observation Jacobians - axis 0
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<0>() = -2*HK1;
Hfusion.at<1>() = 2*HK4;
Hfusion.at<2>() = -2*HK8;
Hfusion.at<3>() = 2*HK12;
Hfusion.at<4>() = 0;
Hfusion.at<5>() = 0;
Hfusion.at<6>() = 0;
@@ -193,9 +105,9 @@ Hfusion.at<12>() = 0;
Hfusion.at<13>() = 0;
Hfusion.at<14>() = 0;
Hfusion.at<15>() = 0;
Hfusion.at<16>() = HK15 + HK17;
Hfusion.at<17>() = 2*HK20;
Hfusion.at<18>() = 2*HK21 - 2*HK22;
Hfusion.at<16>() = -HK15;
Hfusion.at<17>() = 2*HK17;
Hfusion.at<18>() = -2*HK19;
Hfusion.at<19>() = 1;
Hfusion.at<20>() = 0;
Hfusion.at<21>() = 0;
@@ -204,37 +116,37 @@ Hfusion.at<23>() = 0;
// Kalman gains - axis 0
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));
Kfusion(0) = HK23*HK35;
Kfusion(1) = HK26*HK35;
Kfusion(2) = HK33*HK35;
Kfusion(3) = HK30*HK35;
Kfusion(4) = HK35*(HK15*P(4,16) + HK20*P(0,4) + HK21*P(4,18) + HK22*P(2,4) - HK27*P(1,4) - HK29*P(4,17) - HK31*P(3,4) - P(4,19));
Kfusion(5) = HK35*(HK15*P(5,16) + HK20*P(0,5) + HK21*P(5,18) + HK22*P(2,5) - HK27*P(1,5) - HK29*P(5,17) - HK31*P(3,5) - P(5,19));
Kfusion(6) = HK35*(HK15*P(6,16) + HK20*P(0,6) + HK21*P(6,18) + HK22*P(2,6) - HK27*P(1,6) - HK29*P(6,17) - HK31*P(3,6) - P(6,19));
Kfusion(7) = HK35*(HK15*P(7,16) + HK20*P(0,7) + HK21*P(7,18) + HK22*P(2,7) - HK27*P(1,7) - HK29*P(7,17) - HK31*P(3,7) - P(7,19));
Kfusion(8) = HK35*(HK15*P(8,16) + HK20*P(0,8) + HK21*P(8,18) + HK22*P(2,8) - HK27*P(1,8) - HK29*P(8,17) - HK31*P(3,8) - P(8,19));
Kfusion(9) = HK35*(HK15*P(9,16) + HK20*P(0,9) + HK21*P(9,18) + HK22*P(2,9) - HK27*P(1,9) - HK29*P(9,17) - HK31*P(3,9) - P(9,19));
Kfusion(10) = HK35*(HK15*P(10,16) + HK20*P(0,10) + HK21*P(10,18) + HK22*P(2,10) - HK27*P(1,10) - HK29*P(10,17) - HK31*P(3,10) - P(10,19));
Kfusion(11) = HK35*(HK15*P(11,16) + HK20*P(0,11) + HK21*P(11,18) + HK22*P(2,11) - HK27*P(1,11) - HK29*P(11,17) - HK31*P(3,11) - P(11,19));
Kfusion(12) = HK35*(HK15*P(12,16) + HK20*P(0,12) + HK21*P(12,18) + HK22*P(2,12) - HK27*P(1,12) - HK29*P(12,17) - HK31*P(3,12) - P(12,19));
Kfusion(13) = HK35*(HK15*P(13,16) + HK20*P(0,13) + HK21*P(13,18) + HK22*P(2,13) - HK27*P(1,13) - HK29*P(13,17) - HK31*P(3,13) - P(13,19));
Kfusion(14) = HK35*(HK15*P(14,16) + HK20*P(0,14) + HK21*P(14,18) + HK22*P(2,14) - HK27*P(1,14) - HK29*P(14,17) - HK31*P(3,14) - P(14,19));
Kfusion(15) = HK35*(HK15*P(15,16) + HK20*P(0,15) + HK21*P(15,18) + HK22*P(2,15) - HK27*P(1,15) - HK29*P(15,17) - HK31*P(3,15) - P(15,19));
Kfusion(16) = HK25*HK35;
Kfusion(17) = HK28*HK35;
Kfusion(18) = HK32*HK35;
Kfusion(19) = HK34*HK35;
Kfusion(20) = HK35*(HK15*P(16,20) + HK20*P(0,20) + HK21*P(18,20) + HK22*P(2,20) - HK27*P(1,20) - HK29*P(17,20) - HK31*P(3,20) - P(19,20));
Kfusion(21) = HK35*(HK15*P(16,21) + HK20*P(0,21) + HK21*P(18,21) + HK22*P(2,21) - HK27*P(1,21) - HK29*P(17,21) - HK31*P(3,21) + HK36);
Kfusion(22) = HK35*(HK15*P(16,22) + HK20*P(0,22) + HK21*P(18,22) + HK22*P(2,22) - HK27*P(1,22) - HK29*P(17,22) - HK31*P(3,22) - P(19,22));
Kfusion(23) = HK35*(HK15*P(16,23) + HK20*P(0,23) + HK21*P(18,23) + HK22*P(2,23) - HK27*P(1,23) - HK29*P(17,23) - HK31*P(3,23) - P(19,23));
// Observation Jacobians - axis 1
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<0>() = 2*HK38;
Hfusion.at<1>() = 2*HK39;
Hfusion.at<2>() = 2*HK41;
Hfusion.at<3>() = -2*HK44;
Hfusion.at<4>() = 0;
Hfusion.at<5>() = 0;
Hfusion.at<6>() = 0;
@@ -247,9 +159,9 @@ Hfusion.at<12>() = 0;
Hfusion.at<13>() = 0;
Hfusion.at<14>() = 0;
Hfusion.at<15>() = 0;
Hfusion.at<16>() = -2*HK18 + 2*HK19;
Hfusion.at<17>() = HK17 + HK93;
Hfusion.at<18>() = 2*HK96;
Hfusion.at<16>() = -2*HK45;
Hfusion.at<17>() = -HK47;
Hfusion.at<18>() = 2*HK49;
Hfusion.at<19>() = 0;
Hfusion.at<20>() = 1;
Hfusion.at<21>() = 0;
@@ -258,37 +170,37 @@ Hfusion.at<23>() = 0;
// Kalman gains - axis 1
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));
Kfusion(0) = HK56*HK64;
Kfusion(1) = HK62*HK64;
Kfusion(2) = HK60*HK64;
Kfusion(3) = HK59*HK64;
Kfusion(4) = HK64*(-HK47*P(4,17) + HK50*P(4,18) + HK51*P(2,4) + HK52*P(0,4) - HK53*P(4,16) + HK54*P(1,4) - HK55*P(3,4) + P(4,20));
Kfusion(5) = HK64*(-HK47*P(5,17) + HK50*P(5,18) + HK51*P(2,5) + HK52*P(0,5) - HK53*P(5,16) + HK54*P(1,5) - HK55*P(3,5) + P(5,20));
Kfusion(6) = HK64*(-HK47*P(6,17) + HK50*P(6,18) + HK51*P(2,6) + HK52*P(0,6) - HK53*P(6,16) + HK54*P(1,6) - HK55*P(3,6) + P(6,20));
Kfusion(7) = HK64*(-HK47*P(7,17) + HK50*P(7,18) + HK51*P(2,7) + HK52*P(0,7) - HK53*P(7,16) + HK54*P(1,7) - HK55*P(3,7) + P(7,20));
Kfusion(8) = HK64*(-HK47*P(8,17) + HK50*P(8,18) + HK51*P(2,8) + HK52*P(0,8) - HK53*P(8,16) + HK54*P(1,8) - HK55*P(3,8) + P(8,20));
Kfusion(9) = HK64*(-HK47*P(9,17) + HK50*P(9,18) + HK51*P(2,9) + HK52*P(0,9) - HK53*P(9,16) + HK54*P(1,9) - HK55*P(3,9) + P(9,20));
Kfusion(10) = HK64*(-HK47*P(10,17) + HK50*P(10,18) + HK51*P(2,10) + HK52*P(0,10) - HK53*P(10,16) + HK54*P(1,10) - HK55*P(3,10) + P(10,20));
Kfusion(11) = HK64*(-HK47*P(11,17) + HK50*P(11,18) + HK51*P(2,11) + HK52*P(0,11) - HK53*P(11,16) + HK54*P(1,11) - HK55*P(3,11) + P(11,20));
Kfusion(12) = HK64*(-HK47*P(12,17) + HK50*P(12,18) + HK51*P(2,12) + HK52*P(0,12) - HK53*P(12,16) + HK54*P(1,12) - HK55*P(3,12) + P(12,20));
Kfusion(13) = HK64*(-HK47*P(13,17) + HK50*P(13,18) + HK51*P(2,13) + HK52*P(0,13) - HK53*P(13,16) + HK54*P(1,13) - HK55*P(3,13) + P(13,20));
Kfusion(14) = HK64*(-HK47*P(14,17) + HK50*P(14,18) + HK51*P(2,14) + HK52*P(0,14) - HK53*P(14,16) + HK54*P(1,14) - HK55*P(3,14) + P(14,20));
Kfusion(15) = HK64*(-HK47*P(15,17) + HK50*P(15,18) + HK51*P(2,15) + HK52*P(0,15) - HK53*P(15,16) + HK54*P(1,15) - HK55*P(3,15) + P(15,20));
Kfusion(16) = HK58*HK64;
Kfusion(17) = HK57*HK64;
Kfusion(18) = HK61*HK64;
Kfusion(19) = HK64*(-HK47*P(17,19) + HK50*P(18,19) + HK51*P(2,19) + HK52*P(0,19) - HK53*P(16,19) + HK54*P(1,19) - HK55*P(3,19) + P(19,20));
Kfusion(20) = HK63*HK64;
Kfusion(21) = HK64*(-HK47*P(17,21) + HK50*P(18,21) + HK51*P(2,21) + HK52*P(0,21) - HK53*P(16,21) + HK54*P(1,21) - HK55*P(3,21) + P(20,21));
Kfusion(22) = HK64*(-HK47*P(17,22) + HK50*P(18,22) + HK51*P(2,22) + HK52*P(0,22) - HK53*P(16,22) + HK54*P(1,22) - HK55*P(3,22) + P(20,22));
Kfusion(23) = HK64*(-HK47*P(17,23) + HK50*P(18,23) + HK51*P(2,23) + HK52*P(0,23) - HK53*P(16,23) + HK54*P(1,23) - HK55*P(3,23) + P(20,23));
// Observation Jacobians - axis 2
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<0>() = -2*HK65;
Hfusion.at<1>() = -2*HK66;
Hfusion.at<2>() = 2*HK67;
Hfusion.at<3>() = 2*HK68;
Hfusion.at<4>() = 0;
Hfusion.at<5>() = 0;
Hfusion.at<6>() = 0;
@@ -301,9 +213,9 @@ Hfusion.at<12>() = 0;
Hfusion.at<13>() = 0;
Hfusion.at<14>() = 0;
Hfusion.at<15>() = 0;
Hfusion.at<16>() = 2*HK116;
Hfusion.at<17>() = -2*HK94 + 2*HK95;
Hfusion.at<18>() = HK15 + HK93 + 1;
Hfusion.at<16>() = 2*HK69;
Hfusion.at<17>() = -2*HK70;
Hfusion.at<18>() = -HK71;
Hfusion.at<19>() = 0;
Hfusion.at<20>() = 0;
Hfusion.at<21>() = 1;
@@ -312,29 +224,29 @@ Hfusion.at<23>() = 0;
// Kalman gains - axis 2
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));
Kfusion(0) = HK75*HK86;
Kfusion(1) = HK84*HK86;
Kfusion(2) = HK81*HK86;
Kfusion(3) = HK77*HK86;
Kfusion(4) = HK86*(HK71*P(4,18) + HK72*P(0,4) + HK73*P(4,17) + HK74*P(1,4) - HK78*P(3,4) - HK80*P(4,16) - HK82*P(2,4) - P(4,21));
Kfusion(5) = HK86*(HK71*P(5,18) + HK72*P(0,5) + HK73*P(5,17) + HK74*P(1,5) - HK78*P(3,5) - HK80*P(5,16) - HK82*P(2,5) - P(5,21));
Kfusion(6) = HK86*(HK71*P(6,18) + HK72*P(0,6) + HK73*P(6,17) + HK74*P(1,6) - HK78*P(3,6) - HK80*P(6,16) - HK82*P(2,6) - P(6,21));
Kfusion(7) = HK86*(HK71*P(7,18) + HK72*P(0,7) + HK73*P(7,17) + HK74*P(1,7) - HK78*P(3,7) - HK80*P(7,16) - HK82*P(2,7) - P(7,21));
Kfusion(8) = HK86*(HK71*P(8,18) + HK72*P(0,8) + HK73*P(8,17) + HK74*P(1,8) - HK78*P(3,8) - HK80*P(8,16) - HK82*P(2,8) - P(8,21));
Kfusion(9) = HK86*(HK71*P(9,18) + HK72*P(0,9) + HK73*P(9,17) + HK74*P(1,9) - HK78*P(3,9) - HK80*P(9,16) - HK82*P(2,9) - P(9,21));
Kfusion(10) = HK86*(HK71*P(10,18) + HK72*P(0,10) + HK73*P(10,17) + HK74*P(1,10) - HK78*P(3,10) - HK80*P(10,16) - HK82*P(2,10) - P(10,21));
Kfusion(11) = HK86*(HK71*P(11,18) + HK72*P(0,11) + HK73*P(11,17) + HK74*P(1,11) - HK78*P(3,11) - HK80*P(11,16) - HK82*P(2,11) - P(11,21));
Kfusion(12) = HK86*(HK71*P(12,18) + HK72*P(0,12) + HK73*P(12,17) + HK74*P(1,12) - HK78*P(3,12) - HK80*P(12,16) - HK82*P(2,12) - P(12,21));
Kfusion(13) = HK86*(HK71*P(13,18) + HK72*P(0,13) + HK73*P(13,17) + HK74*P(1,13) - HK78*P(3,13) - HK80*P(13,16) - HK82*P(2,13) - P(13,21));
Kfusion(14) = HK86*(HK71*P(14,18) + HK72*P(0,14) + HK73*P(14,17) + HK74*P(1,14) - HK78*P(3,14) - HK80*P(14,16) - HK82*P(2,14) - P(14,21));
Kfusion(15) = HK86*(HK71*P(15,18) + HK72*P(0,15) + HK73*P(15,17) + HK74*P(1,15) - HK78*P(3,15) - HK80*P(15,16) - HK82*P(2,15) - P(15,21));
Kfusion(16) = HK79*HK86;
Kfusion(17) = HK83*HK86;
Kfusion(18) = HK76*HK86;
Kfusion(19) = HK86*(HK36 + HK71*P(18,19) + HK72*P(0,19) + HK73*P(17,19) + HK74*P(1,19) - HK78*P(3,19) - HK80*P(16,19) - HK82*P(2,19));
Kfusion(20) = HK86*(HK71*P(18,20) + HK72*P(0,20) + HK73*P(17,20) + HK74*P(1,20) - HK78*P(3,20) - HK80*P(16,20) - HK82*P(2,20) - P(20,21));
Kfusion(21) = HK85*HK86;
Kfusion(22) = HK86*(HK71*P(18,22) + HK72*P(0,22) + HK73*P(17,22) + HK74*P(1,22) - HK78*P(3,22) - HK80*P(16,22) - HK82*P(2,22) - P(21,22));
Kfusion(23) = HK86*(HK71*P(18,23) + HK72*P(0,23) + HK73*P(17,23) + HK74*P(1,23) - HK78*P(3,23) - HK80*P(16,23) - HK82*P(2,23) - P(21,23));
src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_innov_var_generated.cpp
+8 -6
View File
@@ -17,19 +17,19 @@ 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 IV18 = q0*q2 + q1*q3;
const float IV19 = IV3 + 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);
const float IV22 = IV12 + 2*powf(q2, 2);
const float IV23 = IV14 - 2*IV15 + IV16;
const float IV24 = 4*IV5 + 2*IV6 + 2*magE*q0;
// Observation Jacobians - axis 0
Hfusion.at<0>() = 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;
Hfusion.at<2>() = 2*IV18*P(16,21) + 2*IV18*(2*IV18*P(16,16) + 2*IV19*P(3,16) - IV20*P(0,16) - IV21*P(16,17) - IV22*P(16,18) + 2*IV23*P(2,16) - IV24*P(1,16) + P(16,21)) + 2*IV19*P(3,21) + 2*IV19*(2*IV18*P(3,16) + 2*IV19*P(3,3) - IV20*P(0,3) - IV21*P(3,17) - IV22*P(3,18) + 2*IV23*P(2,3) - IV24*P(1,3) + P(3,21)) - IV20*P(0,21) - IV20*(2*IV18*P(0,16) + 2*IV19*P(0,3) - IV20*P(0,0) - IV21*P(0,17) - IV22*P(0,18) + 2*IV23*P(0,2) - IV24*P(0,1) + P(0,21)) - IV21*P(17,21) - IV21*(2*IV18*P(16,17) + 2*IV19*P(3,17) - IV20*P(0,17) - IV21*P(17,17) - IV22*P(17,18) + 2*IV23*P(2,17) - IV24*P(1,17) + P(17,21)) - IV22*P(18,21) - IV22*(2*IV18*P(16,18) + 2*IV19*P(3,18) - IV20*P(0,18) - IV21*P(17,18) - IV22*P(18,18) + 2*IV23*P(2,18) - IV24*P(1,18) + P(18,21)) + 2*IV23*P(2,21) + 2*IV23*(2*IV18*P(2,16) + 2*IV19*P(2,3) - IV20*P(0,2) - IV21*P(2,17) - IV22*P(2,18) + 2*IV23*P(2,2) - IV24*P(1,2) + P(2,21)) - IV24*P(1,21) - IV24*(2*IV18*P(1,16) + 2*IV19*P(1,3) - IV20*P(0,1) - IV21*P(1,17) - IV22*P(1,18) + 2*IV23*P(1,2) - IV24*P(1,1) + P(1,21)) + P(21,21) + R_MAG;
// Kalman gains - axis 0
@@ -45,3 +45,5 @@ Hfusion.at<2>() = IV18*P(16,21) + IV18*(IV18*P(16,16) + IV19*P(3,16) - IV20*P(0,
// Kalman gains - axis 2
src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated.cpp
+116 -122
View File
@@ -1,61 +1,52 @@
// Axis 0 equations
// Sub Expressions
const float HK0 = q2*vd;
const float HK1 = ve - vwe;
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);
const float HK0 = ve - vwe;
const float HK1 = -HK0*q3 + q2*vd;
const float HK2 = 2*Kaccx;
const float HK3 = HK0*q2 + q3*vd;
const float HK4 = 2*vn - 2*vwn;
const float HK5 = -HK0*q1 + HK4*q2 + q0*vd;
const float HK6 = HK0*q0 - HK4*q3 + q1*vd;
const float HK7 = 2*powf(q2, 2) + 2*powf(q3, 2) - 1;
const float HK8 = HK7*Kaccx;
const float HK9 = q0*q3 + q1*q2;
const float HK10 = HK2*HK9;
const float HK11 = q0*q2 - q1*q3;
const float HK12 = 2*HK9;
const float HK13 = HK12*P(0,23);
const float HK14 = 2*HK11;
const float HK15 = 2*HK1;
const float HK16 = 2*HK5;
const float HK17 = HK12*P(23,23);
const float HK18 = HK12*P(5,23);
const float HK19 = -2*HK11;
const float HK20 = 2*HK3;
const float HK21 = -HK7;
const float HK22 = -2*HK1;
const float HK23 = 2*HK6;
const float HK24 = -2*HK5;
const float HK25 = powf(Kaccx, 2);
const float HK26 = HK12*HK25;
const float HK27 = -HK18;
const float HK28 = HK12*P(6,23);
const float HK29 = HK12*P(1,23);
const float HK30 = HK12*P(4,23);
const float HK31 = HK7*P(4,22);
const float HK32 = HK25*HK7;
const float HK33 = HK12*P(22,23);
const float HK34 = HK12*P(3,23);
const float HK35 = HK12*P(2,23);
const float HK36 = Kaccx/(HK14*HK25*(HK12*P(5,6) + HK19*P(6,6) + HK20*P(1,6) + HK21*P(4,6) + HK22*P(0,6) + HK23*P(3,6) + HK24*P(2,6) - HK28 + HK7*P(6,22)) + HK15*HK25*(HK12*P(0,5) - HK13 + HK19*P(0,6) + HK20*P(0,1) + HK21*P(0,4) + HK22*P(0,0) + HK23*P(0,3) + HK24*P(0,2) + HK7*P(0,22)) + HK16*HK25*(HK12*P(2,5) + HK19*P(2,6) + HK20*P(1,2) + HK21*P(2,4) + HK22*P(0,2) + HK23*P(2,3) + HK24*P(2,2) - HK35 + HK7*P(2,22)) - HK20*HK25*(HK12*P(1,5) + HK19*P(1,6) + HK20*P(1,1) + HK21*P(1,4) + HK22*P(0,1) + HK23*P(1,3) + HK24*P(1,2) - HK29 + HK7*P(1,22)) - HK23*HK25*(HK12*P(3,5) + HK19*P(3,6) + HK20*P(1,3) + HK21*P(3,4) + HK22*P(0,3) + HK23*P(3,3) + HK24*P(2,3) - HK34 + HK7*P(3,22)) + HK26*(-HK17 + HK18 + HK19*P(6,23) + HK20*P(1,23) + HK21*P(4,23) + HK22*P(0,23) + HK23*P(3,23) + HK24*P(2,23) + HK7*P(22,23)) - HK26*(HK12*P(5,5) + HK19*P(5,6) + HK20*P(1,5) + HK21*P(4,5) + HK22*P(0,5) + HK23*P(3,5) + HK24*P(2,5) + HK27 + HK7*P(5,22)) + HK32*(HK12*P(4,5) + HK19*P(4,6) + HK20*P(1,4) + HK21*P(4,4) + HK22*P(0,4) + HK23*P(3,4) + HK24*P(2,4) - HK30 + HK31) - HK32*(HK12*P(5,22) + HK19*P(6,22) + HK20*P(1,22) + HK21*P(4,22) + HK22*P(0,22) + HK23*P(3,22) + HK24*P(2,22) - HK33 + HK7*P(22,22)) - R_ACC);
// Observation Jacobians
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<0>() = HK1*HK2;
Hfusion.at<1>() = -HK2*HK3;
Hfusion.at<2>() = HK2*HK5;
Hfusion.at<3>() = -HK2*HK6;
Hfusion.at<4>() = HK8;
Hfusion.at<5>() = -HK10;
Hfusion.at<6>() = HK11*HK2;
Hfusion.at<7>() = 0;
Hfusion.at<8>() = 0;
Hfusion.at<9>() = 0;
@@ -71,35 +62,35 @@ Hfusion.at<18>() = 0;
Hfusion.at<19>() = 0;
Hfusion.at<20>() = 0;
Hfusion.at<21>() = 0;
Hfusion.at<22>() = -HK15;
Hfusion.at<23>() = HK17;
Hfusion.at<22>() = -HK8;
Hfusion.at<23>() = HK10;
// Kalman gains
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);
Kfusion(0) = HK36*(-HK13 - HK14*P(0,6) - HK15*P(0,0) - HK16*P(0,2) + 2*HK3*P(0,1) + 2*HK6*P(0,3) + HK7*P(0,22) - HK7*P(0,4) + 2*HK9*P(0,5));
Kfusion(1) = HK36*(-HK14*P(1,6) - HK15*P(0,1) - HK16*P(1,2) - HK29 + 2*HK3*P(1,1) + 2*HK6*P(1,3) + HK7*P(1,22) - HK7*P(1,4) + 2*HK9*P(1,5));
Kfusion(2) = HK36*(-HK14*P(2,6) - HK15*P(0,2) - HK16*P(2,2) + 2*HK3*P(1,2) - HK35 + 2*HK6*P(2,3) + HK7*P(2,22) - HK7*P(2,4) + 2*HK9*P(2,5));
Kfusion(3) = HK36*(-HK14*P(3,6) - HK15*P(0,3) - HK16*P(2,3) + 2*HK3*P(1,3) - HK34 + 2*HK6*P(3,3) + HK7*P(3,22) - HK7*P(3,4) + 2*HK9*P(3,5));
Kfusion(4) = HK36*(-HK14*P(4,6) - HK15*P(0,4) - HK16*P(2,4) + 2*HK3*P(1,4) - HK30 + HK31 + 2*HK6*P(3,4) - HK7*P(4,4) + 2*HK9*P(4,5));
Kfusion(5) = HK36*(-HK14*P(5,6) - HK15*P(0,5) - HK16*P(2,5) - HK18 + 2*HK3*P(1,5) + 2*HK6*P(3,5) - HK7*P(4,5) + HK7*P(5,22) + 2*HK9*P(5,5));
Kfusion(6) = HK36*(-HK14*P(6,6) - HK15*P(0,6) - HK16*P(2,6) - HK28 + 2*HK3*P(1,6) + 2*HK6*P(3,6) - HK7*P(4,6) + HK7*P(6,22) + 2*HK9*P(5,6));
Kfusion(7) = HK36*(-HK12*P(7,23) - HK14*P(6,7) - HK15*P(0,7) - HK16*P(2,7) + 2*HK3*P(1,7) + 2*HK6*P(3,7) - HK7*P(4,7) + HK7*P(7,22) + 2*HK9*P(5,7));
Kfusion(8) = HK36*(-HK12*P(8,23) - HK14*P(6,8) - HK15*P(0,8) - HK16*P(2,8) + 2*HK3*P(1,8) + 2*HK6*P(3,8) - HK7*P(4,8) + HK7*P(8,22) + 2*HK9*P(5,8));
Kfusion(9) = HK36*(-HK12*P(9,23) - HK14*P(6,9) - HK15*P(0,9) - HK16*P(2,9) + 2*HK3*P(1,9) + 2*HK6*P(3,9) - HK7*P(4,9) + HK7*P(9,22) + 2*HK9*P(5,9));
Kfusion(10) = HK36*(-HK12*P(10,23) - HK14*P(6,10) - HK15*P(0,10) - HK16*P(2,10) + 2*HK3*P(1,10) + 2*HK6*P(3,10) + HK7*P(10,22) - HK7*P(4,10) + 2*HK9*P(5,10));
Kfusion(11) = HK36*(-HK12*P(11,23) - HK14*P(6,11) - HK15*P(0,11) - HK16*P(2,11) + 2*HK3*P(1,11) + 2*HK6*P(3,11) + HK7*P(11,22) - HK7*P(4,11) + 2*HK9*P(5,11));
Kfusion(12) = HK36*(-HK12*P(12,23) - HK14*P(6,12) - HK15*P(0,12) - HK16*P(2,12) + 2*HK3*P(1,12) + 2*HK6*P(3,12) + HK7*P(12,22) - HK7*P(4,12) + 2*HK9*P(5,12));
Kfusion(13) = HK36*(-HK12*P(13,23) - HK14*P(6,13) - HK15*P(0,13) - HK16*P(2,13) + 2*HK3*P(1,13) + 2*HK6*P(3,13) + HK7*P(13,22) - HK7*P(4,13) + 2*HK9*P(5,13));
Kfusion(14) = HK36*(-HK12*P(14,23) - HK14*P(6,14) - HK15*P(0,14) - HK16*P(2,14) + 2*HK3*P(1,14) + 2*HK6*P(3,14) + HK7*P(14,22) - HK7*P(4,14) + 2*HK9*P(5,14));
Kfusion(15) = HK36*(-HK12*P(15,23) - HK14*P(6,15) - HK15*P(0,15) - HK16*P(2,15) + 2*HK3*P(1,15) + 2*HK6*P(3,15) + HK7*P(15,22) - HK7*P(4,15) + 2*HK9*P(5,15));
Kfusion(16) = HK36*(-HK12*P(16,23) - HK14*P(6,16) - HK15*P(0,16) - HK16*P(2,16) + 2*HK3*P(1,16) + 2*HK6*P(3,16) + HK7*P(16,22) - HK7*P(4,16) + 2*HK9*P(5,16));
Kfusion(17) = HK36*(-HK12*P(17,23) - HK14*P(6,17) - HK15*P(0,17) - HK16*P(2,17) + 2*HK3*P(1,17) + 2*HK6*P(3,17) + HK7*P(17,22) - HK7*P(4,17) + 2*HK9*P(5,17));
Kfusion(18) = HK36*(-HK12*P(18,23) - HK14*P(6,18) - HK15*P(0,18) - HK16*P(2,18) + 2*HK3*P(1,18) + 2*HK6*P(3,18) + HK7*P(18,22) - HK7*P(4,18) + 2*HK9*P(5,18));
Kfusion(19) = HK36*(-HK12*P(19,23) - HK14*P(6,19) - HK15*P(0,19) - HK16*P(2,19) + 2*HK3*P(1,19) + 2*HK6*P(3,19) + HK7*P(19,22) - HK7*P(4,19) + 2*HK9*P(5,19));
Kfusion(20) = HK36*(-HK12*P(20,23) - HK14*P(6,20) - HK15*P(0,20) - HK16*P(2,20) + 2*HK3*P(1,20) + 2*HK6*P(3,20) + HK7*P(20,22) - HK7*P(4,20) + 2*HK9*P(5,20));
Kfusion(21) = HK36*(-HK12*P(21,23) - HK14*P(6,21) - HK15*P(0,21) - HK16*P(2,21) + 2*HK3*P(1,21) + 2*HK6*P(3,21) + HK7*P(21,22) - HK7*P(4,21) + 2*HK9*P(5,21));
Kfusion(22) = HK36*(-HK14*P(6,22) - HK15*P(0,22) - HK16*P(2,22) + 2*HK3*P(1,22) - HK31 - HK33 + 2*HK6*P(3,22) + HK7*P(22,22) + 2*HK9*P(5,22));
Kfusion(23) = HK36*(-HK14*P(6,23) - HK15*P(0,23) - HK16*P(2,23) - HK17 - HK27 + 2*HK3*P(1,23) + 2*HK6*P(3,23) + HK7*P(22,23) - HK7*P(4,23));
// Axis 1 equations
@@ -116,27 +107,28 @@ 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 HK12 = 2*HK6;
const float HK13 = 2*HK7;
const float HK14 = 2*HK5;
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 HK14 = 2*HK1;
const float HK15 = 2*HK4;
const float HK16 = 2*HK5;
const float HK17 = 2*HK11;
const float HK18 = HK13*P(0,22) + HK14*P(0,0) + HK15*P(0,1) + HK16*P(0,2) + HK17*P(0,6) + HK9*P(0,23);
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 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 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 = 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);
const float HK20 = -HK9;
const float HK21 = -2*HK6;
const float HK22 = -2*HK7;
const float HK23 = HK13*P(6,22) + HK14*P(0,6) + HK15*P(1,6) + HK16*P(2,6) + HK17*P(6,6) + HK9*P(6,23);
const float HK24 = HK13*P(22,22) + HK14*P(0,22) + HK15*P(1,22) + HK16*P(2,22) + HK17*P(6,22) + HK9*P(22,23);
const float HK25 = HK13*P(4,22);
const float HK26 = HK14*P(0,4) + HK15*P(1,4) + HK16*P(2,4) + HK17*P(4,6) + HK25 + HK9*P(4,23);
const float HK27 = HK13*P(2,22) + HK14*P(0,2) + HK15*P(1,2) + HK16*P(2,2) + HK17*P(2,6) + HK9*P(2,23);
const float HK28 = HK13*P(22,23) + HK14*P(0,23) + HK15*P(1,23) + HK16*P(2,23) + HK17*P(6,23) + HK9*P(23,23);
const float HK29 = HK9*P(5,23);
const float HK30 = HK13*P(5,22) + HK14*P(0,5) + HK15*P(1,5) + HK16*P(2,5) + HK17*P(5,6) + HK29;
const float HK31 = HK13*P(1,22) + HK14*P(0,1) + HK15*P(1,1) + HK16*P(1,2) + HK17*P(1,6) + HK9*P(1,23);
const float HK32 = HK13*P(3,22) + HK14*P(0,3) + HK15*P(1,3) + HK16*P(2,3) + HK17*P(3,6) + HK9*P(3,23);
const float HK33 = Kaccy/(-HK13*HK19*(HK20*P(5,22) + HK21*P(3,22) + HK22*P(4,22) + HK24) - HK14*HK19*(HK18 + HK20*P(0,5) + HK21*P(0,3) + HK22*P(0,4)) - HK15*HK19*(HK20*P(1,5) + HK21*P(1,3) + HK22*P(1,4) + HK31) - HK16*HK19*(HK20*P(2,5) + HK21*P(2,3) + HK22*P(2,4) + HK27) - HK17*HK19*(HK20*P(5,6) + HK21*P(3,6) + HK22*P(4,6) + HK23) + 2*HK19*HK6*(HK20*P(3,5) + HK21*P(3,3) + HK22*P(3,4) + HK32) + 2*HK19*HK7*(HK20*P(4,5) + HK21*P(3,4) + HK22*P(4,4) + HK26) - HK19*HK9*(HK20*P(5,23) + HK21*P(3,23) + HK22*P(4,23) + HK28) + HK19*HK9*(HK20*P(5,5) + HK21*P(3,5) + HK22*P(4,5) + HK30) - R_ACC);
// Observation Jacobians
@@ -167,27 +159,29 @@ Hfusion.at<23>() = -HK10;
// Kalman gains
Kfusion(0) = -HK18*HK32;
Kfusion(1) = -HK30*HK32;
Kfusion(2) = -HK25*HK32;
Kfusion(3) = -HK31*HK32;
Kfusion(4) = -HK24*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(22) = -HK22*HK32;
Kfusion(23) = -HK27*HK32;
Kfusion(0) = HK33*(-HK12*P(0,3) - HK13*P(0,4) + HK18 - HK9*P(0,5));
Kfusion(1) = HK33*(-HK12*P(1,3) - HK13*P(1,4) + HK31 - HK9*P(1,5));
Kfusion(2) = HK33*(-HK12*P(2,3) - HK13*P(2,4) + HK27 - HK9*P(2,5));
Kfusion(3) = HK33*(-HK12*P(3,3) - HK13*P(3,4) + HK32 - HK9*P(3,5));
Kfusion(4) = HK33*(-HK12*P(3,4) - HK13*P(4,4) + HK26 - HK9*P(4,5));
Kfusion(5) = HK33*(-HK12*P(3,5) - HK13*P(4,5) + HK30 - HK9*P(5,5));
Kfusion(6) = HK33*(-HK12*P(3,6) - HK13*P(4,6) + HK23 - HK9*P(5,6));
Kfusion(7) = HK33*(-HK12*P(3,7) - HK13*P(4,7) + HK13*P(7,22) + HK14*P(0,7) + HK15*P(1,7) + HK16*P(2,7) + HK17*P(6,7) - HK9*P(5,7) + HK9*P(7,23));
Kfusion(8) = HK33*(-HK12*P(3,8) - HK13*P(4,8) + HK13*P(8,22) + HK14*P(0,8) + HK15*P(1,8) + HK16*P(2,8) + HK17*P(6,8) - HK9*P(5,8) + HK9*P(8,23));
Kfusion(9) = HK33*(-HK12*P(3,9) - HK13*P(4,9) + HK13*P(9,22) + HK14*P(0,9) + HK15*P(1,9) + HK16*P(2,9) + HK17*P(6,9) - HK9*P(5,9) + HK9*P(9,23));
Kfusion(10) = HK33*(-HK12*P(3,10) + HK13*P(10,22) - HK13*P(4,10) + HK14*P(0,10) + HK15*P(1,10) + HK16*P(2,10) + HK17*P(6,10) + HK9*P(10,23) - HK9*P(5,10));
Kfusion(11) = HK33*(-HK12*P(3,11) + HK13*P(11,22) - HK13*P(4,11) + HK14*P(0,11) + HK15*P(1,11) + HK16*P(2,11) + HK17*P(6,11) + HK9*P(11,23) - HK9*P(5,11));
Kfusion(12) = HK33*(-HK12*P(3,12) + HK13*P(12,22) - HK13*P(4,12) + HK14*P(0,12) + HK15*P(1,12) + HK16*P(2,12) + HK17*P(6,12) + HK9*P(12,23) - HK9*P(5,12));
Kfusion(13) = HK33*(-HK12*P(3,13) + HK13*P(13,22) - HK13*P(4,13) + HK14*P(0,13) + HK15*P(1,13) + HK16*P(2,13) + HK17*P(6,13) + HK9*P(13,23) - HK9*P(5,13));
Kfusion(14) = HK33*(-HK12*P(3,14) + HK13*P(14,22) - HK13*P(4,14) + HK14*P(0,14) + HK15*P(1,14) + HK16*P(2,14) + HK17*P(6,14) + HK9*P(14,23) - HK9*P(5,14));
Kfusion(15) = HK33*(-HK12*P(3,15) + HK13*P(15,22) - HK13*P(4,15) + HK14*P(0,15) + HK15*P(1,15) + HK16*P(2,15) + HK17*P(6,15) + HK9*P(15,23) - HK9*P(5,15));
Kfusion(16) = HK33*(-HK12*P(3,16) + HK13*P(16,22) - HK13*P(4,16) + HK14*P(0,16) + HK15*P(1,16) + HK16*P(2,16) + HK17*P(6,16) + HK9*P(16,23) - HK9*P(5,16));
Kfusion(17) = HK33*(-HK12*P(3,17) + HK13*P(17,22) - HK13*P(4,17) + HK14*P(0,17) + HK15*P(1,17) + HK16*P(2,17) + HK17*P(6,17) + HK9*P(17,23) - HK9*P(5,17));
Kfusion(18) = HK33*(-HK12*P(3,18) + HK13*P(18,22) - HK13*P(4,18) + HK14*P(0,18) + HK15*P(1,18) + HK16*P(2,18) + HK17*P(6,18) + HK9*P(18,23) - HK9*P(5,18));
Kfusion(19) = HK33*(-HK12*P(3,19) + HK13*P(19,22) - HK13*P(4,19) + HK14*P(0,19) + HK15*P(1,19) + HK16*P(2,19) + HK17*P(6,19) + HK9*P(19,23) - HK9*P(5,19));
Kfusion(20) = HK33*(-HK12*P(3,20) + HK13*P(20,22) - HK13*P(4,20) + HK14*P(0,20) + HK15*P(1,20) + HK16*P(2,20) + HK17*P(6,20) + HK9*P(20,23) - HK9*P(5,20));
Kfusion(21) = HK33*(-HK12*P(3,21) + HK13*P(21,22) - HK13*P(4,21) + HK14*P(0,21) + HK15*P(1,21) + HK16*P(2,21) + HK17*P(6,21) + HK9*P(21,23) - HK9*P(5,21));
Kfusion(22) = HK33*(-HK12*P(3,22) + HK24 - HK25 - HK9*P(5,22));
Kfusion(23) = HK33*(-HK12*P(3,23) - HK13*P(4,23) + HK28 - HK29);
src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated_alt.cpp
+135 -138
View File
@@ -10,90 +10,85 @@ 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);
const float HK11 = 2*HK10 + HK7 - HK8;
const float HK12 = q1*vd;
const float HK13 = HK9*q3;
const float HK14 = HK1*q0 + HK12 - 2*HK13;
const float HK15 = 2*powf(q3, 2) - 1;
const float HK16 = HK15 + 2*powf(q2, 2);
const float HK17 = HK16*Kaccx;
const float HK18 = q0*q3;
const float HK19 = q1*q2;
const float HK20 = HK18 + HK19;
const float HK21 = HK20*HK4;
const float HK22 = q0*q2 - q1*q3;
const float HK23 = 2*HK20;
const float HK24 = HK23*P(0,23);
const float HK25 = 2*HK22;
const float HK26 = 2*HK3;
const float HK27 = 2*HK11;
const float HK28 = HK23*P(23,23);
const float HK29 = HK23*P(5,23);
const float HK30 = -2*HK22;
const float HK31 = 2*HK6;
const float HK32 = -HK16;
const float HK33 = -2*HK3;
const float HK34 = 2*HK14;
const float HK35 = -2*HK11;
const float HK36 = powf(Kaccx, 2);
const float HK37 = HK23*HK36;
const float HK38 = -HK29;
const float HK39 = HK23*P(6,23);
const float HK40 = HK23*P(1,23);
const float HK41 = HK23*P(4,23);
const float HK42 = HK16*P(4,22);
const float HK43 = HK16*HK36;
const float HK44 = HK23*P(22,23);
const float HK45 = HK23*P(3,23);
const float HK46 = HK23*P(2,23);
const float HK47 = Kaccx/(HK25*HK36*(HK16*P(6,22) + HK23*P(5,6) + HK30*P(6,6) + HK31*P(1,6) + HK32*P(4,6) + HK33*P(0,6) + HK34*P(3,6) + HK35*P(2,6) - HK39) + HK26*HK36*(HK16*P(0,22) + HK23*P(0,5) - HK24 + HK30*P(0,6) + HK31*P(0,1) + HK32*P(0,4) + HK33*P(0,0) + HK34*P(0,3) + HK35*P(0,2)) + HK27*HK36*(HK16*P(2,22) + HK23*P(2,5) + HK30*P(2,6) + HK31*P(1,2) + HK32*P(2,4) + HK33*P(0,2) + HK34*P(2,3) + HK35*P(2,2) - HK46) - HK31*HK36*(HK16*P(1,22) + HK23*P(1,5) + HK30*P(1,6) + HK31*P(1,1) + HK32*P(1,4) + HK33*P(0,1) + HK34*P(1,3) + HK35*P(1,2) - HK40) - HK34*HK36*(HK16*P(3,22) + HK23*P(3,5) + HK30*P(3,6) + HK31*P(1,3) + HK32*P(3,4) + HK33*P(0,3) + HK34*P(3,3) + HK35*P(2,3) - HK45) + HK37*(HK16*P(22,23) - HK28 + HK29 + HK30*P(6,23) + HK31*P(1,23) + HK32*P(4,23) + HK33*P(0,23) + HK34*P(3,23) + HK35*P(2,23)) - HK37*(HK16*P(5,22) + HK23*P(5,5) + HK30*P(5,6) + HK31*P(1,5) + HK32*P(4,5) + HK33*P(0,5) + HK34*P(3,5) + HK35*P(2,5) + HK38) - HK43*(HK16*P(22,22) + HK23*P(5,22) + HK30*P(6,22) + HK31*P(1,22) + HK32*P(4,22) + HK33*P(0,22) + HK34*P(3,22) + HK35*P(2,22) - HK44) + HK43*(HK23*P(4,5) + HK30*P(4,6) + HK31*P(1,4) + HK32*P(4,4) + HK33*P(0,4) + HK34*P(3,4) + HK35*P(2,4) - HK41 + HK42) - R_ACC);
const float HK48 = HK12 - HK13;
const float HK49 = 2*Kaccy;
const float HK50 = HK10 + HK7 - 2*HK8;
const float HK51 = HK5 + HK9*q1;
const float HK52 = -HK0 + 2*HK2 + HK9*q0;
const float HK53 = HK18 - HK19;
const float HK54 = HK49*HK53;
const float HK55 = HK15 + 2*powf(q1, 2);
const float HK56 = HK55*Kaccy;
const float HK57 = q0*q1 + q2*q3;
const float HK58 = 2*HK52;
const float HK59 = 2*HK53;
const float HK60 = 2*HK48;
const float HK61 = 2*HK50;
const float HK62 = 2*HK51;
const float HK63 = 2*HK57;
const float HK64 = HK55*P(0,23) + HK59*P(0,22) + HK60*P(0,0) + HK61*P(0,1) + HK62*P(0,2) + HK63*P(0,6);
const float HK65 = powf(Kaccy, 2);
const float HK66 = -HK55;
const float HK67 = -2*HK52;
const float HK68 = -2*HK53;
const float HK69 = HK55*P(6,23) + HK59*P(6,22) + HK60*P(0,6) + HK61*P(1,6) + HK62*P(2,6) + HK63*P(6,6);
const float HK70 = HK55*P(22,23) + HK59*P(22,22) + HK60*P(0,22) + HK61*P(1,22) + HK62*P(2,22) + HK63*P(6,22);
const float HK71 = HK59*P(4,22);
const float HK72 = HK55*P(4,23) + HK60*P(0,4) + HK61*P(1,4) + HK62*P(2,4) + HK63*P(4,6) + HK71;
const float HK73 = HK55*P(2,23) + HK59*P(2,22) + HK60*P(0,2) + HK61*P(1,2) + HK62*P(2,2) + HK63*P(2,6);
const float HK74 = HK55*P(23,23) + HK59*P(22,23) + HK60*P(0,23) + HK61*P(1,23) + HK62*P(2,23) + HK63*P(6,23);
const float HK75 = HK55*P(5,23);
const float HK76 = HK59*P(5,22) + HK60*P(0,5) + HK61*P(1,5) + HK62*P(2,5) + HK63*P(5,6) + HK75;
const float HK77 = HK55*P(1,23) + HK59*P(1,22) + HK60*P(0,1) + HK61*P(1,1) + HK62*P(1,2) + HK63*P(1,6);
const float HK78 = HK55*P(3,23) + HK59*P(3,22) + HK60*P(0,3) + HK61*P(1,3) + HK62*P(2,3) + HK63*P(3,6);
const float HK79 = Kaccy/(2*HK52*HK65*(HK66*P(3,5) + HK67*P(3,3) + HK68*P(3,4) + HK78) + 2*HK53*HK65*(HK66*P(4,5) + HK67*P(3,4) + HK68*P(4,4) + HK72) - HK55*HK65*(HK66*P(5,23) + HK67*P(3,23) + HK68*P(4,23) + HK74) + HK55*HK65*(HK66*P(5,5) + HK67*P(3,5) + HK68*P(4,5) + HK76) - HK59*HK65*(HK66*P(5,22) + HK67*P(3,22) + HK68*P(4,22) + HK70) - HK60*HK65*(HK64 + HK66*P(0,5) + HK67*P(0,3) + HK68*P(0,4)) - HK61*HK65*(HK66*P(1,5) + HK67*P(1,3) + HK68*P(1,4) + HK77) - HK62*HK65*(HK66*P(2,5) + HK67*P(2,3) + HK68*P(2,4) + HK73) - HK63*HK65*(HK66*P(5,6) + HK67*P(3,6) + HK68*P(4,6) + HK69) - R_ACC);
// Observation Jacobians - axis 0
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<2>() = HK11*HK4;
Hfusion.at<3>() = -HK14*HK4;
Hfusion.at<4>() = HK17;
Hfusion.at<5>() = -HK21;
Hfusion.at<6>() = HK22*HK4;
Hfusion.at<7>() = 0;
Hfusion.at<8>() = 0;
Hfusion.at<9>() = 0;
@@ -109,45 +104,45 @@ Hfusion.at<18>() = 0;
Hfusion.at<19>() = 0;
Hfusion.at<20>() = 0;
Hfusion.at<21>() = 0;
Hfusion.at<22>() = -HK20;
Hfusion.at<23>() = HK24;
Hfusion.at<22>() = -HK17;
Hfusion.at<23>() = HK21;
// Kalman gains - axis 0
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);
Kfusion(0) = HK47*(2*HK14*P(0,3) + HK16*P(0,22) - HK16*P(0,4) + 2*HK20*P(0,5) - HK24 - HK25*P(0,6) - HK26*P(0,0) - HK27*P(0,2) + 2*HK6*P(0,1));
Kfusion(1) = HK47*(2*HK14*P(1,3) + HK16*P(1,22) - HK16*P(1,4) + 2*HK20*P(1,5) - HK25*P(1,6) - HK26*P(0,1) - HK27*P(1,2) - HK40 + 2*HK6*P(1,1));
Kfusion(2) = HK47*(2*HK14*P(2,3) + HK16*P(2,22) - HK16*P(2,4) + 2*HK20*P(2,5) - HK25*P(2,6) - HK26*P(0,2) - HK27*P(2,2) - HK46 + 2*HK6*P(1,2));
Kfusion(3) = HK47*(2*HK14*P(3,3) + HK16*P(3,22) - HK16*P(3,4) + 2*HK20*P(3,5) - HK25*P(3,6) - HK26*P(0,3) - HK27*P(2,3) - HK45 + 2*HK6*P(1,3));
Kfusion(4) = HK47*(2*HK14*P(3,4) - HK16*P(4,4) + 2*HK20*P(4,5) - HK25*P(4,6) - HK26*P(0,4) - HK27*P(2,4) - HK41 + HK42 + 2*HK6*P(1,4));
Kfusion(5) = HK47*(2*HK14*P(3,5) - HK16*P(4,5) + HK16*P(5,22) + 2*HK20*P(5,5) - HK25*P(5,6) - HK26*P(0,5) - HK27*P(2,5) - HK29 + 2*HK6*P(1,5));
Kfusion(6) = HK47*(2*HK14*P(3,6) - HK16*P(4,6) + HK16*P(6,22) + 2*HK20*P(5,6) - HK25*P(6,6) - HK26*P(0,6) - HK27*P(2,6) - HK39 + 2*HK6*P(1,6));
Kfusion(7) = HK47*(2*HK14*P(3,7) - HK16*P(4,7) + HK16*P(7,22) + 2*HK20*P(5,7) - HK23*P(7,23) - HK25*P(6,7) - HK26*P(0,7) - HK27*P(2,7) + 2*HK6*P(1,7));
Kfusion(8) = HK47*(2*HK14*P(3,8) - HK16*P(4,8) + HK16*P(8,22) + 2*HK20*P(5,8) - HK23*P(8,23) - HK25*P(6,8) - HK26*P(0,8) - HK27*P(2,8) + 2*HK6*P(1,8));
Kfusion(9) = HK47*(2*HK14*P(3,9) - HK16*P(4,9) + HK16*P(9,22) + 2*HK20*P(5,9) - HK23*P(9,23) - HK25*P(6,9) - HK26*P(0,9) - HK27*P(2,9) + 2*HK6*P(1,9));
Kfusion(10) = HK47*(2*HK14*P(3,10) + HK16*P(10,22) - HK16*P(4,10) + 2*HK20*P(5,10) - HK23*P(10,23) - HK25*P(6,10) - HK26*P(0,10) - HK27*P(2,10) + 2*HK6*P(1,10));
Kfusion(11) = HK47*(2*HK14*P(3,11) + HK16*P(11,22) - HK16*P(4,11) + 2*HK20*P(5,11) - HK23*P(11,23) - HK25*P(6,11) - HK26*P(0,11) - HK27*P(2,11) + 2*HK6*P(1,11));
Kfusion(12) = HK47*(2*HK14*P(3,12) + HK16*P(12,22) - HK16*P(4,12) + 2*HK20*P(5,12) - HK23*P(12,23) - HK25*P(6,12) - HK26*P(0,12) - HK27*P(2,12) + 2*HK6*P(1,12));
Kfusion(13) = HK47*(2*HK14*P(3,13) + HK16*P(13,22) - HK16*P(4,13) + 2*HK20*P(5,13) - HK23*P(13,23) - HK25*P(6,13) - HK26*P(0,13) - HK27*P(2,13) + 2*HK6*P(1,13));
Kfusion(14) = HK47*(2*HK14*P(3,14) + HK16*P(14,22) - HK16*P(4,14) + 2*HK20*P(5,14) - HK23*P(14,23) - HK25*P(6,14) - HK26*P(0,14) - HK27*P(2,14) + 2*HK6*P(1,14));
Kfusion(15) = HK47*(2*HK14*P(3,15) + HK16*P(15,22) - HK16*P(4,15) + 2*HK20*P(5,15) - HK23*P(15,23) - HK25*P(6,15) - HK26*P(0,15) - HK27*P(2,15) + 2*HK6*P(1,15));
Kfusion(16) = HK47*(2*HK14*P(3,16) + HK16*P(16,22) - HK16*P(4,16) + 2*HK20*P(5,16) - HK23*P(16,23) - HK25*P(6,16) - HK26*P(0,16) - HK27*P(2,16) + 2*HK6*P(1,16));
Kfusion(17) = HK47*(2*HK14*P(3,17) + HK16*P(17,22) - HK16*P(4,17) + 2*HK20*P(5,17) - HK23*P(17,23) - HK25*P(6,17) - HK26*P(0,17) - HK27*P(2,17) + 2*HK6*P(1,17));
Kfusion(18) = HK47*(2*HK14*P(3,18) + HK16*P(18,22) - HK16*P(4,18) + 2*HK20*P(5,18) - HK23*P(18,23) - HK25*P(6,18) - HK26*P(0,18) - HK27*P(2,18) + 2*HK6*P(1,18));
Kfusion(19) = HK47*(2*HK14*P(3,19) + HK16*P(19,22) - HK16*P(4,19) + 2*HK20*P(5,19) - HK23*P(19,23) - HK25*P(6,19) - HK26*P(0,19) - HK27*P(2,19) + 2*HK6*P(1,19));
Kfusion(20) = HK47*(2*HK14*P(3,20) + HK16*P(20,22) - HK16*P(4,20) + 2*HK20*P(5,20) - HK23*P(20,23) - HK25*P(6,20) - HK26*P(0,20) - HK27*P(2,20) + 2*HK6*P(1,20));
Kfusion(21) = HK47*(2*HK14*P(3,21) + HK16*P(21,22) - HK16*P(4,21) + 2*HK20*P(5,21) - HK23*P(21,23) - HK25*P(6,21) - HK26*P(0,21) - HK27*P(2,21) + 2*HK6*P(1,21));
Kfusion(22) = HK47*(2*HK14*P(3,22) + HK16*P(22,22) + 2*HK20*P(5,22) - HK25*P(6,22) - HK26*P(0,22) - HK27*P(2,22) - HK42 - HK44 + 2*HK6*P(1,22));
Kfusion(23) = HK47*(2*HK14*P(3,23) + HK16*P(22,23) - HK16*P(4,23) - HK25*P(6,23) - HK26*P(0,23) - HK27*P(2,23) - HK28 - HK38 + 2*HK6*P(1,23));
// Observation Jacobians - axis 1
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<0>() = -HK48*HK49;
Hfusion.at<1>() = -HK49*HK50;
Hfusion.at<2>() = -HK49*HK51;
Hfusion.at<3>() = HK49*HK52;
Hfusion.at<4>() = HK54;
Hfusion.at<5>() = HK56;
Hfusion.at<6>() = -HK49*HK57;
Hfusion.at<7>() = 0;
Hfusion.at<8>() = 0;
Hfusion.at<9>() = 0;
@@ -163,38 +158,40 @@ Hfusion.at<18>() = 0;
Hfusion.at<19>() = 0;
Hfusion.at<20>() = 0;
Hfusion.at<21>() = 0;
Hfusion.at<22>() = -HK60;
Hfusion.at<23>() = -HK62;
Hfusion.at<22>() = -HK54;
Hfusion.at<23>() = -HK56;
// Kalman gains - axis 1
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;
Kfusion(0) = HK79*(-HK55*P(0,5) - HK58*P(0,3) - HK59*P(0,4) + HK64);
Kfusion(1) = HK79*(-HK55*P(1,5) - HK58*P(1,3) - HK59*P(1,4) + HK77);
Kfusion(2) = HK79*(-HK55*P(2,5) - HK58*P(2,3) - HK59*P(2,4) + HK73);
Kfusion(3) = HK79*(-HK55*P(3,5) - HK58*P(3,3) - HK59*P(3,4) + HK78);
Kfusion(4) = HK79*(-HK55*P(4,5) - HK58*P(3,4) - HK59*P(4,4) + HK72);
Kfusion(5) = HK79*(-HK55*P(5,5) - HK58*P(3,5) - HK59*P(4,5) + HK76);
Kfusion(6) = HK79*(-HK55*P(5,6) - HK58*P(3,6) - HK59*P(4,6) + HK69);
Kfusion(7) = HK79*(-HK55*P(5,7) + HK55*P(7,23) - HK58*P(3,7) - HK59*P(4,7) + HK59*P(7,22) + HK60*P(0,7) + HK61*P(1,7) + HK62*P(2,7) + HK63*P(6,7));
Kfusion(8) = HK79*(-HK55*P(5,8) + HK55*P(8,23) - HK58*P(3,8) - HK59*P(4,8) + HK59*P(8,22) + HK60*P(0,8) + HK61*P(1,8) + HK62*P(2,8) + HK63*P(6,8));
Kfusion(9) = HK79*(-HK55*P(5,9) + HK55*P(9,23) - HK58*P(3,9) - HK59*P(4,9) + HK59*P(9,22) + HK60*P(0,9) + HK61*P(1,9) + HK62*P(2,9) + HK63*P(6,9));
Kfusion(10) = HK79*(HK55*P(10,23) - HK55*P(5,10) - HK58*P(3,10) + HK59*P(10,22) - HK59*P(4,10) + HK60*P(0,10) + HK61*P(1,10) + HK62*P(2,10) + HK63*P(6,10));
Kfusion(11) = HK79*(HK55*P(11,23) - HK55*P(5,11) - HK58*P(3,11) + HK59*P(11,22) - HK59*P(4,11) + HK60*P(0,11) + HK61*P(1,11) + HK62*P(2,11) + HK63*P(6,11));
Kfusion(12) = HK79*(HK55*P(12,23) - HK55*P(5,12) - HK58*P(3,12) + HK59*P(12,22) - HK59*P(4,12) + HK60*P(0,12) + HK61*P(1,12) + HK62*P(2,12) + HK63*P(6,12));
Kfusion(13) = HK79*(HK55*P(13,23) - HK55*P(5,13) - HK58*P(3,13) + HK59*P(13,22) - HK59*P(4,13) + HK60*P(0,13) + HK61*P(1,13) + HK62*P(2,13) + HK63*P(6,13));
Kfusion(14) = HK79*(HK55*P(14,23) - HK55*P(5,14) - HK58*P(3,14) + HK59*P(14,22) - HK59*P(4,14) + HK60*P(0,14) + HK61*P(1,14) + HK62*P(2,14) + HK63*P(6,14));
Kfusion(15) = HK79*(HK55*P(15,23) - HK55*P(5,15) - HK58*P(3,15) + HK59*P(15,22) - HK59*P(4,15) + HK60*P(0,15) + HK61*P(1,15) + HK62*P(2,15) + HK63*P(6,15));
Kfusion(16) = HK79*(HK55*P(16,23) - HK55*P(5,16) - HK58*P(3,16) + HK59*P(16,22) - HK59*P(4,16) + HK60*P(0,16) + HK61*P(1,16) + HK62*P(2,16) + HK63*P(6,16));
Kfusion(17) = HK79*(HK55*P(17,23) - HK55*P(5,17) - HK58*P(3,17) + HK59*P(17,22) - HK59*P(4,17) + HK60*P(0,17) + HK61*P(1,17) + HK62*P(2,17) + HK63*P(6,17));
Kfusion(18) = HK79*(HK55*P(18,23) - HK55*P(5,18) - HK58*P(3,18) + HK59*P(18,22) - HK59*P(4,18) + HK60*P(0,18) + HK61*P(1,18) + HK62*P(2,18) + HK63*P(6,18));
Kfusion(19) = HK79*(HK55*P(19,23) - HK55*P(5,19) - HK58*P(3,19) + HK59*P(19,22) - HK59*P(4,19) + HK60*P(0,19) + HK61*P(1,19) + HK62*P(2,19) + HK63*P(6,19));
Kfusion(20) = HK79*(HK55*P(20,23) - HK55*P(5,20) - HK58*P(3,20) + HK59*P(20,22) - HK59*P(4,20) + HK60*P(0,20) + HK61*P(1,20) + HK62*P(2,20) + HK63*P(6,20));
Kfusion(21) = HK79*(HK55*P(21,23) - HK55*P(5,21) - HK58*P(3,21) + HK59*P(21,22) - HK59*P(4,21) + HK60*P(0,21) + HK61*P(1,21) + HK62*P(2,21) + HK63*P(6,21));
Kfusion(22) = HK79*(-HK55*P(5,22) - HK58*P(3,22) + HK70 - HK71);
Kfusion(23) = HK79*(-HK58*P(3,23) - HK59*P(4,23) + HK74 - HK75);
// Observation Jacobians - axis 2
// Kalman gains - axis 2
src/modules/ekf2/EKF/python/ekf_derivation/generated/beta_generated.cpp
+76 -81
View File
@@ -7,64 +7,57 @@ 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 = 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);
const float HK8 = q0*q3;
const float HK9 = q1*q2;
const float HK10 = 2*HK8 + 2*HK9;
const float HK11 = 2*powf(q3, 2) - 1;
const float HK12 = HK11 + 2*powf(q2, 2);
const float HK13 = HK1*HK12 - HK10*HK4 + HK6*HK7;
const float HK14 = 1.0F/HK13;
const float HK15 = q0*q1 + q2*q3;
const float HK16 = HK11 + 2*powf(q1, 2);
const float HK17 = 2*HK1*(HK8 - HK9) - HK15*HK7 + HK16*HK4;
const float HK18 = HK14*HK17;
const float HK19 = 2*HK14;
const float HK20 = HK19*(HK0 + HK18*(HK3 - HK5) - HK2);
const float HK21 = q0*vd;
const float HK22 = HK1*q2;
const float HK23 = HK4*q1;
const float HK24 = q3*vd;
const float HK25 = -1/HK13;
const float HK26 = -HK17*HK25;
const float HK27 = HK21 + HK22 - 2*HK23 - HK26*(HK24 + HK4*q2);
const float HK28 = 2*HK25;
const float HK29 = HK19*(HK1*q1 + HK18*(HK21 + 2*HK22 - HK23) + HK24);
const float HK30 = HK28*(HK1*q0 + HK26*(HK0 - 2*HK2 + HK4*q0) - HK3 + 2*HK5);
const float HK31 = -2*HK8 + 2*HK9;
const float HK32 = HK12*HK26 + HK31;
const float HK33 = HK25*(HK10*HK26 + HK16);
const float HK34 = HK19*(HK15 + HK18*HK6);
const float HK35 = HK12*HK18 + HK31;
const float HK36 = HK14*(HK10*HK18 + HK16);
const float HK37 = -HK25*HK32;
const float HK38 = -HK27*HK28;
const float HK39 = HK14*HK35*P(0,22) - HK20*P(0,0) - HK29*P(0,2) - HK30*P(0,3) - HK33*P(0,5) - HK34*P(0,6) - HK36*P(0,23) - HK37*P(0,4) - HK38*P(0,1);
const float HK40 = HK14*HK35*P(6,22) - HK20*P(0,6) - HK29*P(2,6) - HK30*P(3,6) - HK33*P(5,6) - HK34*P(6,6) - HK36*P(6,23) - HK37*P(4,6) - HK38*P(1,6);
const float HK41 = HK14*HK35*P(22,23) - HK20*P(0,23) - HK29*P(2,23) - HK30*P(3,23) - HK33*P(5,23) - HK34*P(6,23) - HK36*P(23,23) - HK37*P(4,23) - HK38*P(1,23);
const float HK42 = HK14*HK35*P(22,22) - HK20*P(0,22) - HK29*P(2,22) - HK30*P(3,22) - HK33*P(5,22) - HK34*P(6,22) - HK36*P(22,23) - HK37*P(4,22) - HK38*P(1,22);
const float HK43 = HK14*HK35*P(5,22) - HK20*P(0,5) - HK29*P(2,5) - HK30*P(3,5) - HK33*P(5,5) - HK34*P(5,6) - HK36*P(5,23) - HK37*P(4,5) - HK38*P(1,5);
const float HK44 = HK14*HK35*P(4,22) - HK20*P(0,4) - HK29*P(2,4) - HK30*P(3,4) - HK33*P(4,5) - HK34*P(4,6) - HK36*P(4,23) - HK37*P(4,4) - HK38*P(1,4);
const float HK45 = HK14*HK35*P(2,22) - HK20*P(0,2) - HK29*P(2,2) - HK30*P(2,3) - HK33*P(2,5) - HK34*P(2,6) - HK36*P(2,23) - HK37*P(2,4) - HK38*P(1,2);
const float HK46 = HK14*HK35*P(1,22) - HK20*P(0,1) - HK29*P(1,2) - HK30*P(1,3) - HK33*P(1,5) - HK34*P(1,6) - HK36*P(1,23) - HK37*P(1,4) - HK38*P(1,1);
const float HK47 = HK14*HK35*P(3,22) - HK20*P(0,3) - HK29*P(2,3) - HK30*P(3,3) - HK33*P(3,5) - HK34*P(3,6) - HK36*P(3,23) - HK37*P(3,4) - HK38*P(1,3);
const float HK48 = 1.0F/(HK14*HK35*HK42 - HK20*HK39 - HK29*HK45 - HK30*HK47 - HK33*HK43 - HK34*HK40 - HK36*HK41 - HK37*HK44 - HK38*HK46 + R_BETA);
// Observation Jacobians
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<0>() = -HK20;
Hfusion.at<1>() = HK27*HK28;
Hfusion.at<2>() = -HK29;
Hfusion.at<3>() = -HK30;
Hfusion.at<4>() = HK25*HK32;
Hfusion.at<5>() = -HK33;
Hfusion.at<6>() = -HK34;
Hfusion.at<7>() = 0;
Hfusion.at<8>() = 0;
Hfusion.at<9>() = 0;
@@ -80,32 +73,34 @@ Hfusion.at<18>() = 0;
Hfusion.at<19>() = 0;
Hfusion.at<20>() = 0;
Hfusion.at<21>() = 0;
Hfusion.at<22>() = HK42;
Hfusion.at<23>() = -HK43;
Hfusion.at<22>() = HK14*HK35;
Hfusion.at<23>() = -HK36;
// Kalman gains
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;
Kfusion(0) = HK39*HK48;
Kfusion(1) = HK46*HK48;
Kfusion(2) = HK45*HK48;
Kfusion(3) = HK47*HK48;
Kfusion(4) = HK44*HK48;
Kfusion(5) = HK43*HK48;
Kfusion(6) = HK40*HK48;
Kfusion(7) = HK48*(HK14*HK35*P(7,22) - HK20*P(0,7) - HK29*P(2,7) - HK30*P(3,7) - HK33*P(5,7) - HK34*P(6,7) - HK36*P(7,23) - HK37*P(4,7) - HK38*P(1,7));
Kfusion(8) = HK48*(HK14*HK35*P(8,22) - HK20*P(0,8) - HK29*P(2,8) - HK30*P(3,8) - HK33*P(5,8) - HK34*P(6,8) - HK36*P(8,23) - HK37*P(4,8) - HK38*P(1,8));
Kfusion(9) = HK48*(HK14*HK35*P(9,22) - HK20*P(0,9) - HK29*P(2,9) - HK30*P(3,9) - HK33*P(5,9) - HK34*P(6,9) - HK36*P(9,23) - HK37*P(4,9) - HK38*P(1,9));
Kfusion(10) = HK48*(HK14*HK35*P(10,22) - HK20*P(0,10) - HK29*P(2,10) - HK30*P(3,10) - HK33*P(5,10) - HK34*P(6,10) - HK36*P(10,23) - HK37*P(4,10) - HK38*P(1,10));
Kfusion(11) = HK48*(HK14*HK35*P(11,22) - HK20*P(0,11) - HK29*P(2,11) - HK30*P(3,11) - HK33*P(5,11) - HK34*P(6,11) - HK36*P(11,23) - HK37*P(4,11) - HK38*P(1,11));
Kfusion(12) = HK48*(HK14*HK35*P(12,22) - HK20*P(0,12) - HK29*P(2,12) - HK30*P(3,12) - HK33*P(5,12) - HK34*P(6,12) - HK36*P(12,23) - HK37*P(4,12) - HK38*P(1,12));
Kfusion(13) = HK48*(HK14*HK35*P(13,22) - HK20*P(0,13) - HK29*P(2,13) - HK30*P(3,13) - HK33*P(5,13) - HK34*P(6,13) - HK36*P(13,23) - HK37*P(4,13) - HK38*P(1,13));
Kfusion(14) = HK48*(HK14*HK35*P(14,22) - HK20*P(0,14) - HK29*P(2,14) - HK30*P(3,14) - HK33*P(5,14) - HK34*P(6,14) - HK36*P(14,23) - HK37*P(4,14) - HK38*P(1,14));
Kfusion(15) = HK48*(HK14*HK35*P(15,22) - HK20*P(0,15) - HK29*P(2,15) - HK30*P(3,15) - HK33*P(5,15) - HK34*P(6,15) - HK36*P(15,23) - HK37*P(4,15) - HK38*P(1,15));
Kfusion(16) = HK48*(HK14*HK35*P(16,22) - HK20*P(0,16) - HK29*P(2,16) - HK30*P(3,16) - HK33*P(5,16) - HK34*P(6,16) - HK36*P(16,23) - HK37*P(4,16) - HK38*P(1,16));
Kfusion(17) = HK48*(HK14*HK35*P(17,22) - HK20*P(0,17) - HK29*P(2,17) - HK30*P(3,17) - HK33*P(5,17) - HK34*P(6,17) - HK36*P(17,23) - HK37*P(4,17) - HK38*P(1,17));
Kfusion(18) = HK48*(HK14*HK35*P(18,22) - HK20*P(0,18) - HK29*P(2,18) - HK30*P(3,18) - HK33*P(5,18) - HK34*P(6,18) - HK36*P(18,23) - HK37*P(4,18) - HK38*P(1,18));
Kfusion(19) = HK48*(HK14*HK35*P(19,22) - HK20*P(0,19) - HK29*P(2,19) - HK30*P(3,19) - HK33*P(5,19) - HK34*P(6,19) - HK36*P(19,23) - HK37*P(4,19) - HK38*P(1,19));
Kfusion(20) = HK48*(HK14*HK35*P(20,22) - HK20*P(0,20) - HK29*P(2,20) - HK30*P(3,20) - HK33*P(5,20) - HK34*P(6,20) - HK36*P(20,23) - HK37*P(4,20) - HK38*P(1,20));
Kfusion(21) = HK48*(HK14*HK35*P(21,22) - HK20*P(0,21) - HK29*P(2,21) - HK30*P(3,21) - HK33*P(5,21) - HK34*P(6,21) - HK36*P(21,23) - HK37*P(4,21) - HK38*P(1,21));
Kfusion(22) = HK42*HK48;
Kfusion(23) = HK41*HK48;
src/modules/ekf2/EKF/python/ekf_derivation/generated/covariance_generated.cpp
+2
View File
@@ -524,3 +524,5 @@ nextP(20,23) = P(20,23);
nextP(21,23) = P(21,23);
nextP(22,23) = P(22,23);
nextP(23,23) = P(23,23);
src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated.cpp
+124 -131
View File
@@ -35,18 +35,15 @@ 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);
const float HK35 = 2*HK14*P(0,1) + 2*HK17*P(0,2) + 2*HK18*P(0,3) - HK27*P(0,4) - HK32*P(0,5) - HK33*P(0,6) - HK34*P(0,0);
const float HK36 = powf(range, -2);
const float HK37 = 2*HK14*P(1,6) + 2*HK17*P(2,6) + 2*HK18*P(3,6) - HK27*P(4,6) - HK32*P(5,6) - HK33*P(6,6) - HK34*P(0,6);
const float HK38 = 2*HK14*P(1,5) + 2*HK17*P(2,5) + 2*HK18*P(3,5) - HK27*P(4,5) - HK32*P(5,5) - HK33*P(5,6) - HK34*P(0,5);
const float HK39 = 2*HK14*P(1,4) + 2*HK17*P(2,4) + 2*HK18*P(3,4) - HK27*P(4,4) - HK32*P(4,5) - HK33*P(4,6) - HK34*P(0,4);
const float HK40 = 2*HK14*P(1,3) + 2*HK17*P(2,3) + 2*HK18*P(3,3) - HK27*P(3,4) - HK32*P(3,5) - HK33*P(3,6) - HK34*P(0,3);
const float HK41 = 2*HK14*P(1,2) + 2*HK17*P(2,2) + 2*HK18*P(2,3) - HK27*P(2,4) - HK32*P(2,5) - HK33*P(2,6) - HK34*P(0,2);
const float HK42 = 2*HK14*P(1,1) + 2*HK17*P(1,2) + 2*HK18*P(1,3) - HK27*P(1,4) - HK32*P(1,5) - HK33*P(1,6) - HK34*P(0,1);
const float HK43 = HK8/(2*HK14*HK36*HK42 + 2*HK17*HK36*HK41 + 2*HK18*HK36*HK40 - HK27*HK36*HK39 - HK32*HK36*HK38 - HK33*HK36*HK37 - HK34*HK35*HK36 + R_LOS);
// Observation Jacobians
@@ -77,111 +74,105 @@ Hfusion.at<23>() = 0;
// Kalman gains
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));
Kfusion(0) = HK35*HK43;
Kfusion(1) = HK42*HK43;
Kfusion(2) = HK41*HK43;
Kfusion(3) = HK40*HK43;
Kfusion(4) = HK39*HK43;
Kfusion(5) = HK38*HK43;
Kfusion(6) = HK37*HK43;
Kfusion(7) = HK43*(2*HK14*P(1,7) + 2*HK17*P(2,7) + 2*HK18*P(3,7) - HK27*P(4,7) - HK32*P(5,7) - HK33*P(6,7) - HK34*P(0,7));
Kfusion(8) = HK43*(2*HK14*P(1,8) + 2*HK17*P(2,8) + 2*HK18*P(3,8) - HK27*P(4,8) - HK32*P(5,8) - HK33*P(6,8) - HK34*P(0,8));
Kfusion(9) = HK43*(2*HK14*P(1,9) + 2*HK17*P(2,9) + 2*HK18*P(3,9) - HK27*P(4,9) - HK32*P(5,9) - HK33*P(6,9) - HK34*P(0,9));
Kfusion(10) = HK43*(2*HK14*P(1,10) + 2*HK17*P(2,10) + 2*HK18*P(3,10) - HK27*P(4,10) - HK32*P(5,10) - HK33*P(6,10) - HK34*P(0,10));
Kfusion(11) = HK43*(2*HK14*P(1,11) + 2*HK17*P(2,11) + 2*HK18*P(3,11) - HK27*P(4,11) - HK32*P(5,11) - HK33*P(6,11) - HK34*P(0,11));
Kfusion(12) = HK43*(2*HK14*P(1,12) + 2*HK17*P(2,12) + 2*HK18*P(3,12) - HK27*P(4,12) - HK32*P(5,12) - HK33*P(6,12) - HK34*P(0,12));
Kfusion(13) = HK43*(2*HK14*P(1,13) + 2*HK17*P(2,13) + 2*HK18*P(3,13) - HK27*P(4,13) - HK32*P(5,13) - HK33*P(6,13) - HK34*P(0,13));
Kfusion(14) = HK43*(2*HK14*P(1,14) + 2*HK17*P(2,14) + 2*HK18*P(3,14) - HK27*P(4,14) - HK32*P(5,14) - HK33*P(6,14) - HK34*P(0,14));
Kfusion(15) = HK43*(2*HK14*P(1,15) + 2*HK17*P(2,15) + 2*HK18*P(3,15) - HK27*P(4,15) - HK32*P(5,15) - HK33*P(6,15) - HK34*P(0,15));
Kfusion(16) = HK43*(2*HK14*P(1,16) + 2*HK17*P(2,16) + 2*HK18*P(3,16) - HK27*P(4,16) - HK32*P(5,16) - HK33*P(6,16) - HK34*P(0,16));
Kfusion(17) = HK43*(2*HK14*P(1,17) + 2*HK17*P(2,17) + 2*HK18*P(3,17) - HK27*P(4,17) - HK32*P(5,17) - HK33*P(6,17) - HK34*P(0,17));
Kfusion(18) = HK43*(2*HK14*P(1,18) + 2*HK17*P(2,18) + 2*HK18*P(3,18) - HK27*P(4,18) - HK32*P(5,18) - HK33*P(6,18) - HK34*P(0,18));
Kfusion(19) = HK43*(2*HK14*P(1,19) + 2*HK17*P(2,19) + 2*HK18*P(3,19) - HK27*P(4,19) - HK32*P(5,19) - HK33*P(6,19) - HK34*P(0,19));
Kfusion(20) = HK43*(2*HK14*P(1,20) + 2*HK17*P(2,20) + 2*HK18*P(3,20) - HK27*P(4,20) - HK32*P(5,20) - HK33*P(6,20) - HK34*P(0,20));
Kfusion(21) = HK43*(2*HK14*P(1,21) + 2*HK17*P(2,21) + 2*HK18*P(3,21) - HK27*P(4,21) - HK32*P(5,21) - HK33*P(6,21) - HK34*P(0,21));
Kfusion(22) = HK43*(2*HK14*P(1,22) + 2*HK17*P(2,22) + 2*HK18*P(3,22) - HK27*P(4,22) - HK32*P(5,22) - HK33*P(6,22) - HK34*P(0,22));
Kfusion(23) = HK43*(2*HK14*P(1,23) + 2*HK17*P(2,23) + 2*HK18*P(3,23) - HK27*P(4,23) - HK32*P(5,23) - HK33*P(6,23) - HK34*P(0,23));
// Y Axis Equations
// Sub Expressions
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 HK0 = Tbs(0,0)*q2;
const float HK1 = Tbs(0,1)*q1;
const float HK2 = HK0 - HK1;
const float HK3 = Tbs(0,1)*q3;
const float HK4 = Tbs(0,2)*q2;
const float HK5 = HK3 - HK4;
const float HK6 = Tbs(0,0)*q3;
const float HK7 = Tbs(0,2)*q1;
const float HK8 = -HK7;
const float HK9 = HK6 + HK8;
const float HK10 = 1.0F/range;
const float HK11 = 2*HK10;
const float HK12 = Tbs(0,2)*q0;
const float HK13 = -HK0 + 2*HK1 + HK12;
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*(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);
const float HK17 = vd*(HK14 + HK6 - 2*HK7) + vn*(HK15 + HK16);
const float HK18 = Tbs(0,0)*q0;
const float HK19 = HK18 - HK3 + 2*HK4;
const float HK20 = Tbs(0,0)*q1;
const float HK21 = ve*(HK16 + HK20) + vn*(-2*HK0 + HK1 + HK12);
const float HK22 = HK14 + 2*HK6 + HK8;
const float HK23 = vd*(HK15 + HK20) + ve*(HK18 - 2*HK3 + HK4);
const float HK24 = q0*q2;
const float HK25 = q1*q3;
const float HK26 = HK24 + HK25;
const float HK27 = q0*q3;
const float HK28 = q1*q2;
const float HK29 = HK27 - HK28;
const float HK30 = 2*Tbs(0,1);
const float HK31 = 2*powf(q2, 2);
const float HK32 = 2*powf(q3, 2) - 1;
const float HK33 = HK31 + HK32;
const float HK34 = HK27 + HK28;
const float HK35 = q0*q1;
const float HK36 = q2*q3;
const float HK37 = HK35 - HK36;
const float HK38 = 2*Tbs(0,2);
const float HK39 = 2*powf(q1, 2);
const float HK40 = HK32 + HK39;
const float HK41 = HK35 + HK36;
const float HK42 = HK24 - HK25;
const float HK43 = 2*Tbs(0,0);
const float HK44 = HK31 + HK39 - 1;
const float HK45 = -2*HK2*vd - 2*HK5*vn + 2*HK9*ve;
const float HK46 = HK26*HK38 - HK29*HK30 - HK33*Tbs(0,0);
const float HK47 = HK34*HK43 - HK37*HK38 - HK40*Tbs(0,1);
const float HK48 = HK30*HK41 - HK42*HK43 - HK44*Tbs(0,2);
const float HK49 = -2*HK13*ve + 2*HK17;
const float HK50 = -2*HK19*vd + 2*HK21;
const float HK51 = -2*HK22*vn + 2*HK23;
const float HK52 = HK45*P(0,0) + HK46*P(0,4) + HK47*P(0,5) + HK48*P(0,6) + HK49*P(0,1) + HK50*P(0,2) + HK51*P(0,3);
const float HK53 = powf(range, -2);
const float HK54 = HK45*P(0,6) + HK46*P(4,6) + HK47*P(5,6) + HK48*P(6,6) + HK49*P(1,6) + HK50*P(2,6) + HK51*P(3,6);
const float HK55 = HK45*P(0,5) + HK46*P(4,5) + HK47*P(5,5) + HK48*P(5,6) + HK49*P(1,5) + HK50*P(2,5) + HK51*P(3,5);
const float HK56 = HK45*P(0,4) + HK46*P(4,4) + HK47*P(4,5) + HK48*P(4,6) + HK49*P(1,4) + HK50*P(2,4) + HK51*P(3,4);
const float HK57 = HK45*P(0,3) + HK46*P(3,4) + HK47*P(3,5) + HK48*P(3,6) + HK49*P(1,3) + HK50*P(2,3) + HK51*P(3,3);
const float HK58 = HK45*P(0,2) + HK46*P(2,4) + HK47*P(2,5) + HK48*P(2,6) + HK49*P(1,2) + HK50*P(2,2) + HK51*P(2,3);
const float HK59 = HK45*P(0,1) + HK46*P(1,4) + HK47*P(1,5) + HK48*P(1,6) + HK49*P(1,1) + HK50*P(1,2) + HK51*P(1,3);
const float HK60 = HK10/(HK45*HK52*HK53 + HK46*HK53*HK56 + HK47*HK53*HK55 + HK48*HK53*HK54 + HK49*HK53*HK59 + HK50*HK53*HK58 + HK51*HK53*HK57 + R_LOS);
// Observation Jacobians
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<0>() = -HK11*(-HK2*vd - HK5*vn + HK9*ve);
Hfusion.at<1>() = -HK11*(-HK13*ve + HK17);
Hfusion.at<2>() = -HK11*(-HK19*vd + HK21);
Hfusion.at<3>() = -HK11*(-HK22*vn + HK23);
Hfusion.at<4>() = -HK10*(2*HK26*Tbs(0,2) - HK29*HK30 - HK33*Tbs(0,0));
Hfusion.at<5>() = -HK10*(2*HK34*Tbs(0,0) - HK37*HK38 - HK40*Tbs(0,1));
Hfusion.at<6>() = -HK10*(2*HK41*Tbs(0,1) - HK42*HK43 - HK44*Tbs(0,2));
Hfusion.at<7>() = 0;
Hfusion.at<8>() = 0;
Hfusion.at<9>() = 0;
@@ -202,27 +193,29 @@ Hfusion.at<23>() = 0;
// Kalman gains
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));
Kfusion(0) = -HK52*HK60;
Kfusion(1) = -HK59*HK60;
Kfusion(2) = -HK58*HK60;
Kfusion(3) = -HK57*HK60;
Kfusion(4) = -HK56*HK60;
Kfusion(5) = -HK55*HK60;
Kfusion(6) = -HK54*HK60;
Kfusion(7) = -HK60*(HK45*P(0,7) + HK46*P(4,7) + HK47*P(5,7) + HK48*P(6,7) + HK49*P(1,7) + HK50*P(2,7) + HK51*P(3,7));
Kfusion(8) = -HK60*(HK45*P(0,8) + HK46*P(4,8) + HK47*P(5,8) + HK48*P(6,8) + HK49*P(1,8) + HK50*P(2,8) + HK51*P(3,8));
Kfusion(9) = -HK60*(HK45*P(0,9) + HK46*P(4,9) + HK47*P(5,9) + HK48*P(6,9) + HK49*P(1,9) + HK50*P(2,9) + HK51*P(3,9));
Kfusion(10) = -HK60*(HK45*P(0,10) + HK46*P(4,10) + HK47*P(5,10) + HK48*P(6,10) + HK49*P(1,10) + HK50*P(2,10) + HK51*P(3,10));
Kfusion(11) = -HK60*(HK45*P(0,11) + HK46*P(4,11) + HK47*P(5,11) + HK48*P(6,11) + HK49*P(1,11) + HK50*P(2,11) + HK51*P(3,11));
Kfusion(12) = -HK60*(HK45*P(0,12) + HK46*P(4,12) + HK47*P(5,12) + HK48*P(6,12) + HK49*P(1,12) + HK50*P(2,12) + HK51*P(3,12));
Kfusion(13) = -HK60*(HK45*P(0,13) + HK46*P(4,13) + HK47*P(5,13) + HK48*P(6,13) + HK49*P(1,13) + HK50*P(2,13) + HK51*P(3,13));
Kfusion(14) = -HK60*(HK45*P(0,14) + HK46*P(4,14) + HK47*P(5,14) + HK48*P(6,14) + HK49*P(1,14) + HK50*P(2,14) + HK51*P(3,14));
Kfusion(15) = -HK60*(HK45*P(0,15) + HK46*P(4,15) + HK47*P(5,15) + HK48*P(6,15) + HK49*P(1,15) + HK50*P(2,15) + HK51*P(3,15));
Kfusion(16) = -HK60*(HK45*P(0,16) + HK46*P(4,16) + HK47*P(5,16) + HK48*P(6,16) + HK49*P(1,16) + HK50*P(2,16) + HK51*P(3,16));
Kfusion(17) = -HK60*(HK45*P(0,17) + HK46*P(4,17) + HK47*P(5,17) + HK48*P(6,17) + HK49*P(1,17) + HK50*P(2,17) + HK51*P(3,17));
Kfusion(18) = -HK60*(HK45*P(0,18) + HK46*P(4,18) + HK47*P(5,18) + HK48*P(6,18) + HK49*P(1,18) + HK50*P(2,18) + HK51*P(3,18));
Kfusion(19) = -HK60*(HK45*P(0,19) + HK46*P(4,19) + HK47*P(5,19) + HK48*P(6,19) + HK49*P(1,19) + HK50*P(2,19) + HK51*P(3,19));
Kfusion(20) = -HK60*(HK45*P(0,20) + HK46*P(4,20) + HK47*P(5,20) + HK48*P(6,20) + HK49*P(1,20) + HK50*P(2,20) + HK51*P(3,20));
Kfusion(21) = -HK60*(HK45*P(0,21) + HK46*P(4,21) + HK47*P(5,21) + HK48*P(6,21) + HK49*P(1,21) + HK50*P(2,21) + HK51*P(3,21));
Kfusion(22) = -HK60*(HK45*P(0,22) + HK46*P(4,22) + HK47*P(5,22) + HK48*P(6,22) + HK49*P(1,22) + HK50*P(2,22) + HK51*P(3,22));
Kfusion(23) = -HK60*(HK45*P(0,23) + HK46*P(4,23) + HK47*P(5,23) + HK48*P(6,23) + HK49*P(1,23) + HK50*P(2,23) + HK51*P(3,23));
src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated_alt.cpp
+125 -141
View File
@@ -27,89 +27,71 @@ 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);
const float HK28 = 2*powf(q3, 2) - 1;
const float HK29 = HK27 + HK28;
const float HK30 = -HK21*HK22 + HK25*HK26 + HK29*Tbs(1,0);
const float HK31 = HK23 + HK24;
const float HK32 = 2*Tbs(1,0);
const float HK33 = q0*q1;
const float HK34 = q2*q3;
const float HK35 = HK33 - HK34;
const float HK36 = 2*powf(q1, 2);
const float HK37 = HK28 + HK36;
const float HK38 = HK22*HK35 - HK31*HK32 + HK37*Tbs(1,1);
const float HK39 = HK33 + HK34;
const float HK40 = HK19 - HK20;
const float HK41 = HK27 + HK36 - 1;
const float HK42 = -HK26*HK39 + HK32*HK40 + HK41*Tbs(1,2);
const float HK43 = 2*HK7;
const float HK44 = 2*HK14*P(0,1) + 2*HK17*P(0,2) + 2*HK18*P(0,3) - HK30*P(0,4) - HK38*P(0,5) - HK42*P(0,6) - HK43*P(0,0);
const float HK45 = powf(range, -2);
const float HK46 = 2*HK14*P(1,6) + 2*HK17*P(2,6) + 2*HK18*P(3,6) - HK30*P(4,6) - HK38*P(5,6) - HK42*P(6,6) - HK43*P(0,6);
const float HK47 = 2*HK14*P(1,5) + 2*HK17*P(2,5) + 2*HK18*P(3,5) - HK30*P(4,5) - HK38*P(5,5) - HK42*P(5,6) - HK43*P(0,5);
const float HK48 = 2*HK14*P(1,4) + 2*HK17*P(2,4) + 2*HK18*P(3,4) - HK30*P(4,4) - HK38*P(4,5) - HK42*P(4,6) - HK43*P(0,4);
const float HK49 = 2*HK14*P(1,3) + 2*HK17*P(2,3) + 2*HK18*P(3,3) - HK30*P(3,4) - HK38*P(3,5) - HK42*P(3,6) - HK43*P(0,3);
const float HK50 = 2*HK14*P(1,2) + 2*HK17*P(2,2) + 2*HK18*P(2,3) - HK30*P(2,4) - HK38*P(2,5) - HK42*P(2,6) - HK43*P(0,2);
const float HK51 = 2*HK14*P(1,1) + 2*HK17*P(1,2) + 2*HK18*P(1,3) - HK30*P(1,4) - HK38*P(1,5) - HK42*P(1,6) - HK43*P(0,1);
const float HK52 = HK8/(2*HK14*HK45*HK51 + 2*HK17*HK45*HK50 + 2*HK18*HK45*HK49 - HK30*HK45*HK48 - HK38*HK45*HK47 - HK42*HK45*HK46 - HK43*HK44*HK45 + R_LOS);
const float HK53 = Tbs(0,0)*q2;
const float HK54 = Tbs(0,1)*q1;
const float HK55 = HK53 - HK54;
const float HK56 = Tbs(0,1)*q3;
const float HK57 = Tbs(0,2)*q2;
const float HK58 = HK56 - HK57;
const float HK59 = Tbs(0,0)*q3;
const float HK60 = Tbs(0,2)*q1;
const float HK61 = -HK60;
const float HK62 = HK59 + HK61;
const float HK63 = Tbs(0,2)*q0;
const float HK64 = -HK53 + 2*HK54 + HK63;
const float HK65 = Tbs(0,1)*q0;
const float HK66 = Tbs(0,1)*q2;
const float HK67 = Tbs(0,2)*q3;
const float HK68 = vd*(HK59 - 2*HK60 + HK65) + vn*(HK66 + HK67);
const float HK69 = Tbs(0,0)*q0;
const float HK70 = -HK56 + 2*HK57 + HK69;
const float HK71 = Tbs(0,0)*q1;
const float HK72 = ve*(HK67 + HK71) + vn*(-2*HK53 + HK54 + HK63);
const float HK73 = 2*HK59 + HK61 + HK65;
const float HK74 = vd*(HK66 + HK71) + ve*(-2*HK56 + HK57 + HK69);
const float HK75 = 2*Tbs(0,1);
const float HK76 = 2*Tbs(0,2);
const float HK77 = 2*Tbs(0,0);
const float HK78 = -2*HK55*vd - 2*HK58*vn + 2*HK62*ve;
const float HK79 = HK21*HK76 - HK25*HK75 - HK29*Tbs(0,0);
const float HK80 = HK31*HK77 - HK35*HK76 - HK37*Tbs(0,1);
const float HK81 = HK39*HK75 - HK40*HK77 - HK41*Tbs(0,2);
const float HK82 = -2*HK64*ve + 2*HK68;
const float HK83 = -2*HK70*vd + 2*HK72;
const float HK84 = -2*HK73*vn + 2*HK74;
const float HK85 = HK78*P(0,0) + HK79*P(0,4) + HK80*P(0,5) + HK81*P(0,6) + HK82*P(0,1) + HK83*P(0,2) + HK84*P(0,3);
const float HK86 = HK78*P(0,6) + HK79*P(4,6) + HK80*P(5,6) + HK81*P(6,6) + HK82*P(1,6) + HK83*P(2,6) + HK84*P(3,6);
const float HK87 = HK78*P(0,5) + HK79*P(4,5) + HK80*P(5,5) + HK81*P(5,6) + HK82*P(1,5) + HK83*P(2,5) + HK84*P(3,5);
const float HK88 = HK78*P(0,4) + HK79*P(4,4) + HK80*P(4,5) + HK81*P(4,6) + HK82*P(1,4) + HK83*P(2,4) + HK84*P(3,4);
const float HK89 = HK78*P(0,3) + HK79*P(3,4) + HK80*P(3,5) + HK81*P(3,6) + HK82*P(1,3) + HK83*P(2,3) + HK84*P(3,3);
const float HK90 = HK78*P(0,2) + HK79*P(2,4) + HK80*P(2,5) + HK81*P(2,6) + HK82*P(1,2) + HK83*P(2,2) + HK84*P(2,3);
const float HK91 = HK78*P(0,1) + HK79*P(1,4) + HK80*P(1,5) + HK81*P(1,6) + HK82*P(1,1) + HK83*P(1,2) + HK84*P(1,3);
const float HK92 = HK8/(HK45*HK78*HK85 + HK45*HK79*HK88 + HK45*HK80*HK87 + HK45*HK81*HK86 + HK45*HK82*HK91 + HK45*HK83*HK90 + HK45*HK84*HK89 + R_LOS);
// Observation Jacobians - axis 0
@@ -117,9 +99,9 @@ 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<4>() = -HK30*HK8;
Hfusion.at<5>() = -HK38*HK8;
Hfusion.at<6>() = -HK42*HK8;
Hfusion.at<7>() = 0;
Hfusion.at<8>() = 0;
Hfusion.at<9>() = 0;
@@ -140,40 +122,40 @@ Hfusion.at<23>() = 0;
// Kalman gains - axis 0
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));
Kfusion(0) = HK44*HK52;
Kfusion(1) = HK51*HK52;
Kfusion(2) = HK50*HK52;
Kfusion(3) = HK49*HK52;
Kfusion(4) = HK48*HK52;
Kfusion(5) = HK47*HK52;
Kfusion(6) = HK46*HK52;
Kfusion(7) = HK52*(2*HK14*P(1,7) + 2*HK17*P(2,7) + 2*HK18*P(3,7) - HK30*P(4,7) - HK38*P(5,7) - HK42*P(6,7) - HK43*P(0,7));
Kfusion(8) = HK52*(2*HK14*P(1,8) + 2*HK17*P(2,8) + 2*HK18*P(3,8) - HK30*P(4,8) - HK38*P(5,8) - HK42*P(6,8) - HK43*P(0,8));
Kfusion(9) = HK52*(2*HK14*P(1,9) + 2*HK17*P(2,9) + 2*HK18*P(3,9) - HK30*P(4,9) - HK38*P(5,9) - HK42*P(6,9) - HK43*P(0,9));
Kfusion(10) = HK52*(2*HK14*P(1,10) + 2*HK17*P(2,10) + 2*HK18*P(3,10) - HK30*P(4,10) - HK38*P(5,10) - HK42*P(6,10) - HK43*P(0,10));
Kfusion(11) = HK52*(2*HK14*P(1,11) + 2*HK17*P(2,11) + 2*HK18*P(3,11) - HK30*P(4,11) - HK38*P(5,11) - HK42*P(6,11) - HK43*P(0,11));
Kfusion(12) = HK52*(2*HK14*P(1,12) + 2*HK17*P(2,12) + 2*HK18*P(3,12) - HK30*P(4,12) - HK38*P(5,12) - HK42*P(6,12) - HK43*P(0,12));
Kfusion(13) = HK52*(2*HK14*P(1,13) + 2*HK17*P(2,13) + 2*HK18*P(3,13) - HK30*P(4,13) - HK38*P(5,13) - HK42*P(6,13) - HK43*P(0,13));
Kfusion(14) = HK52*(2*HK14*P(1,14) + 2*HK17*P(2,14) + 2*HK18*P(3,14) - HK30*P(4,14) - HK38*P(5,14) - HK42*P(6,14) - HK43*P(0,14));
Kfusion(15) = HK52*(2*HK14*P(1,15) + 2*HK17*P(2,15) + 2*HK18*P(3,15) - HK30*P(4,15) - HK38*P(5,15) - HK42*P(6,15) - HK43*P(0,15));
Kfusion(16) = HK52*(2*HK14*P(1,16) + 2*HK17*P(2,16) + 2*HK18*P(3,16) - HK30*P(4,16) - HK38*P(5,16) - HK42*P(6,16) - HK43*P(0,16));
Kfusion(17) = HK52*(2*HK14*P(1,17) + 2*HK17*P(2,17) + 2*HK18*P(3,17) - HK30*P(4,17) - HK38*P(5,17) - HK42*P(6,17) - HK43*P(0,17));
Kfusion(18) = HK52*(2*HK14*P(1,18) + 2*HK17*P(2,18) + 2*HK18*P(3,18) - HK30*P(4,18) - HK38*P(5,18) - HK42*P(6,18) - HK43*P(0,18));
Kfusion(19) = HK52*(2*HK14*P(1,19) + 2*HK17*P(2,19) + 2*HK18*P(3,19) - HK30*P(4,19) - HK38*P(5,19) - HK42*P(6,19) - HK43*P(0,19));
Kfusion(20) = HK52*(2*HK14*P(1,20) + 2*HK17*P(2,20) + 2*HK18*P(3,20) - HK30*P(4,20) - HK38*P(5,20) - HK42*P(6,20) - HK43*P(0,20));
Kfusion(21) = HK52*(2*HK14*P(1,21) + 2*HK17*P(2,21) + 2*HK18*P(3,21) - HK30*P(4,21) - HK38*P(5,21) - HK42*P(6,21) - HK43*P(0,21));
Kfusion(22) = HK52*(2*HK14*P(1,22) + 2*HK17*P(2,22) + 2*HK18*P(3,22) - HK30*P(4,22) - HK38*P(5,22) - HK42*P(6,22) - HK43*P(0,22));
Kfusion(23) = HK52*(2*HK14*P(1,23) + 2*HK17*P(2,23) + 2*HK18*P(3,23) - HK30*P(4,23) - HK38*P(5,23) - HK42*P(6,23) - HK43*P(0,23));
// Observation Jacobians - axis 1
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<0>() = -HK9*(-HK55*vd - HK58*vn + HK62*ve);
Hfusion.at<1>() = -HK9*(-HK64*ve + HK68);
Hfusion.at<2>() = -HK9*(-HK70*vd + HK72);
Hfusion.at<3>() = -HK9*(-HK73*vn + HK74);
Hfusion.at<4>() = -HK8*(2*HK21*Tbs(0,2) - HK25*HK75 - HK29*Tbs(0,0));
Hfusion.at<5>() = -HK8*(2*HK31*Tbs(0,0) - HK35*HK76 - HK37*Tbs(0,1));
Hfusion.at<6>() = -HK8*(2*HK39*Tbs(0,1) - HK40*HK77 - HK41*Tbs(0,2));
Hfusion.at<7>() = 0;
Hfusion.at<8>() = 0;
Hfusion.at<9>() = 0;
@@ -194,27 +176,29 @@ Hfusion.at<23>() = 0;
// Kalman gains - axis 1
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));
Kfusion(0) = -HK85*HK92;
Kfusion(1) = -HK91*HK92;
Kfusion(2) = -HK90*HK92;
Kfusion(3) = -HK89*HK92;
Kfusion(4) = -HK88*HK92;
Kfusion(5) = -HK87*HK92;
Kfusion(6) = -HK86*HK92;
Kfusion(7) = -HK92*(HK78*P(0,7) + HK79*P(4,7) + HK80*P(5,7) + HK81*P(6,7) + HK82*P(1,7) + HK83*P(2,7) + HK84*P(3,7));
Kfusion(8) = -HK92*(HK78*P(0,8) + HK79*P(4,8) + HK80*P(5,8) + HK81*P(6,8) + HK82*P(1,8) + HK83*P(2,8) + HK84*P(3,8));
Kfusion(9) = -HK92*(HK78*P(0,9) + HK79*P(4,9) + HK80*P(5,9) + HK81*P(6,9) + HK82*P(1,9) + HK83*P(2,9) + HK84*P(3,9));
Kfusion(10) = -HK92*(HK78*P(0,10) + HK79*P(4,10) + HK80*P(5,10) + HK81*P(6,10) + HK82*P(1,10) + HK83*P(2,10) + HK84*P(3,10));
Kfusion(11) = -HK92*(HK78*P(0,11) + HK79*P(4,11) + HK80*P(5,11) + HK81*P(6,11) + HK82*P(1,11) + HK83*P(2,11) + HK84*P(3,11));
Kfusion(12) = -HK92*(HK78*P(0,12) + HK79*P(4,12) + HK80*P(5,12) + HK81*P(6,12) + HK82*P(1,12) + HK83*P(2,12) + HK84*P(3,12));
Kfusion(13) = -HK92*(HK78*P(0,13) + HK79*P(4,13) + HK80*P(5,13) + HK81*P(6,13) + HK82*P(1,13) + HK83*P(2,13) + HK84*P(3,13));
Kfusion(14) = -HK92*(HK78*P(0,14) + HK79*P(4,14) + HK80*P(5,14) + HK81*P(6,14) + HK82*P(1,14) + HK83*P(2,14) + HK84*P(3,14));
Kfusion(15) = -HK92*(HK78*P(0,15) + HK79*P(4,15) + HK80*P(5,15) + HK81*P(6,15) + HK82*P(1,15) + HK83*P(2,15) + HK84*P(3,15));
Kfusion(16) = -HK92*(HK78*P(0,16) + HK79*P(4,16) + HK80*P(5,16) + HK81*P(6,16) + HK82*P(1,16) + HK83*P(2,16) + HK84*P(3,16));
Kfusion(17) = -HK92*(HK78*P(0,17) + HK79*P(4,17) + HK80*P(5,17) + HK81*P(6,17) + HK82*P(1,17) + HK83*P(2,17) + HK84*P(3,17));
Kfusion(18) = -HK92*(HK78*P(0,18) + HK79*P(4,18) + HK80*P(5,18) + HK81*P(6,18) + HK82*P(1,18) + HK83*P(2,18) + HK84*P(3,18));
Kfusion(19) = -HK92*(HK78*P(0,19) + HK79*P(4,19) + HK80*P(5,19) + HK81*P(6,19) + HK82*P(1,19) + HK83*P(2,19) + HK84*P(3,19));
Kfusion(20) = -HK92*(HK78*P(0,20) + HK79*P(4,20) + HK80*P(5,20) + HK81*P(6,20) + HK82*P(1,20) + HK83*P(2,20) + HK84*P(3,20));
Kfusion(21) = -HK92*(HK78*P(0,21) + HK79*P(4,21) + HK80*P(5,21) + HK81*P(6,21) + HK82*P(1,21) + HK83*P(2,21) + HK84*P(3,21));
Kfusion(22) = -HK92*(HK78*P(0,22) + HK79*P(4,22) + HK80*P(5,22) + HK81*P(6,22) + HK82*P(1,22) + HK83*P(2,22) + HK84*P(3,22));
Kfusion(23) = -HK92*(HK78*P(0,23) + HK79*P(4,23) + HK80*P(5,23) + HK81*P(6,23) + HK82*P(1,23) + HK83*P(2,23) + HK84*P(3,23));
src/modules/ekf2/EKF/python/ekf_derivation/generated/gps_yaw_generated.cpp
+51 -50
View File
@@ -3,36 +3,35 @@ 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 HK4 = 2*HK0;
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);
const float HK6 = -HK1*(HK5 + 2*powf(q1, 2)) + HK4*(HK2 + HK3);
const float HK7 = 2*HK1;
const float HK8 = HK0*(HK5 + 2*powf(q2, 2)) + HK7*(HK2 - HK3);
const float HK9 = 1.0F/HK8;
const float HK10 = q3*(-HK0 + HK1*HK6*HK9);
const float HK11 = powf(HK8, -2);
const float HK12 = HK11*powf(HK6, 2) + 1;
const float HK13 = 2*HK9/HK12;
const float HK14 = HK0*q2;
const float HK15 = HK1*q1;
const float HK16 = HK6*HK9;
const float HK17 = HK1*HK16*q2 + HK14 - 2*HK15;
const float HK18 = HK0*q1 + HK16*(-2*HK14 + HK15);
const float HK19 = -HK0*q0 + HK16*(HK1*q0 + HK4*q3) + HK7*q3;
const float HK20 = HK10*P(0,0) - HK17*P(0,1) - HK18*P(0,2) + HK19*P(0,3);
const float HK21 = 4*HK11/powf(HK12, 2);
const float HK22 = HK10*P(0,1) - HK17*P(1,1) - HK18*P(1,2) + HK19*P(1,3);
const float HK23 = HK10*P(0,2) - HK17*P(1,2) - HK18*P(2,2) + HK19*P(2,3);
const float HK24 = HK10*P(0,3) - HK17*P(1,3) - HK18*P(2,3) + HK19*P(3,3);
const float HK25 = HK13/(HK10*HK20*HK21 - HK17*HK21*HK22 - HK18*HK21*HK23 + HK19*HK21*HK24 + R_YAW);
// Observation Jacobians
Hfusion.at<0>() = HK12*HK15;
Hfusion.at<1>() = -HK15*HK18;
Hfusion.at<2>() = -HK15*HK19;
Hfusion.at<3>() = HK15*HK20;
Hfusion.at<0>() = HK10*HK13;
Hfusion.at<1>() = -HK13*HK17;
Hfusion.at<2>() = -HK13*HK18;
Hfusion.at<3>() = HK13*HK19;
Hfusion.at<4>() = 0;
Hfusion.at<5>() = 0;
Hfusion.at<6>() = 0;
@@ -56,27 +55,29 @@ Hfusion.at<23>() = 0;
// Kalman gains
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));
Kfusion(0) = HK20*HK25;
Kfusion(1) = HK22*HK25;
Kfusion(2) = HK23*HK25;
Kfusion(3) = HK24*HK25;
Kfusion(4) = HK25*(HK10*P(0,4) - HK17*P(1,4) - HK18*P(2,4) + HK19*P(3,4));
Kfusion(5) = HK25*(HK10*P(0,5) - HK17*P(1,5) - HK18*P(2,5) + HK19*P(3,5));
Kfusion(6) = HK25*(HK10*P(0,6) - HK17*P(1,6) - HK18*P(2,6) + HK19*P(3,6));
Kfusion(7) = HK25*(HK10*P(0,7) - HK17*P(1,7) - HK18*P(2,7) + HK19*P(3,7));
Kfusion(8) = HK25*(HK10*P(0,8) - HK17*P(1,8) - HK18*P(2,8) + HK19*P(3,8));
Kfusion(9) = HK25*(HK10*P(0,9) - HK17*P(1,9) - HK18*P(2,9) + HK19*P(3,9));
Kfusion(10) = HK25*(HK10*P(0,10) - HK17*P(1,10) - HK18*P(2,10) + HK19*P(3,10));
Kfusion(11) = HK25*(HK10*P(0,11) - HK17*P(1,11) - HK18*P(2,11) + HK19*P(3,11));
Kfusion(12) = HK25*(HK10*P(0,12) - HK17*P(1,12) - HK18*P(2,12) + HK19*P(3,12));
Kfusion(13) = HK25*(HK10*P(0,13) - HK17*P(1,13) - HK18*P(2,13) + HK19*P(3,13));
Kfusion(14) = HK25*(HK10*P(0,14) - HK17*P(1,14) - HK18*P(2,14) + HK19*P(3,14));
Kfusion(15) = HK25*(HK10*P(0,15) - HK17*P(1,15) - HK18*P(2,15) + HK19*P(3,15));
Kfusion(16) = HK25*(HK10*P(0,16) - HK17*P(1,16) - HK18*P(2,16) + HK19*P(3,16));
Kfusion(17) = HK25*(HK10*P(0,17) - HK17*P(1,17) - HK18*P(2,17) + HK19*P(3,17));
Kfusion(18) = HK25*(HK10*P(0,18) - HK17*P(1,18) - HK18*P(2,18) + HK19*P(3,18));
Kfusion(19) = HK25*(HK10*P(0,19) - HK17*P(1,19) - HK18*P(2,19) + HK19*P(3,19));
Kfusion(20) = HK25*(HK10*P(0,20) - HK17*P(1,20) - HK18*P(2,20) + HK19*P(3,20));
Kfusion(21) = HK25*(HK10*P(0,21) - HK17*P(1,21) - HK18*P(2,21) + HK19*P(3,21));
Kfusion(22) = HK25*(HK10*P(0,22) - HK17*P(1,22) - HK18*P(2,22) + HK19*P(3,22));
Kfusion(23) = HK25*(HK10*P(0,23) - HK17*P(1,23) - HK18*P(2,23) + HK19*P(3,23));
src/modules/ekf2/EKF/python/ekf_derivation/generated/mag_decl_generated.cpp
+2
View File
@@ -63,3 +63,5 @@ Kfusion(20) = -HK9*(HK5*P(16,20) - P(17,20));
Kfusion(21) = -HK9*(HK5*P(16,21) - P(17,21));
Kfusion(22) = -HK9*(HK5*P(16,22) - P(17,22));
Kfusion(23) = -HK9*(HK5*P(16,23) - P(17,23));
src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_generated.cpp
+2
View File
@@ -70,3 +70,5 @@ Kfusion(20) = HK16*(-HK0*P(20,22) + HK0*P(4,20) - HK1*P(20,23) + HK1*P(5,20) + P
Kfusion(21) = HK16*(-HK0*P(21,22) + HK0*P(4,21) - HK1*P(21,23) + HK1*P(5,21) + P(6,21)*vd);
Kfusion(22) = HK15*HK16;
Kfusion(23) = HK14*HK16;
src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated.cpp
+154 -169
View File
@@ -1,41 +1,35 @@
// axis 0
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);
const float HK0 = q2*vd - q3*ve;
const float HK1 = q2*ve + q3*vd;
const float HK2 = 2*vn;
const float HK3 = HK2*q2 + q0*vd - q1*ve;
const float HK4 = -HK2*q3 + q0*ve + q1*vd;
const float HK5 = 2*powf(q2, 2) + 2*powf(q3, 2) - 1;
const float HK6 = q0*q3 + q1*q2;
const float HK7 = q0*q2 - q1*q3;
const float HK8 = 2*HK1;
const float HK9 = 2*HK6;
const float HK10 = 2*HK0;
const float HK11 = 2*HK7;
const float HK12 = 2*HK4;
const float HK13 = 2*HK3;
const float HK14 = HK10*P(0,0) + HK11*P(0,6) - HK12*P(0,3) + HK13*P(0,2) + HK5*P(0,4) - HK8*P(0,1) - HK9*P(0,5);
const float HK15 = HK10*P(0,5) + HK11*P(5,6) - HK12*P(3,5) + HK13*P(2,5) + HK5*P(4,5) - HK8*P(1,5) - HK9*P(5,5);
const float HK16 = HK10*P(0,1) + HK11*P(1,6) - HK12*P(1,3) + HK13*P(1,2) + HK5*P(1,4) - HK8*P(1,1) - HK9*P(1,5);
const float HK17 = HK10*P(0,6) + HK11*P(6,6) - HK12*P(3,6) + HK13*P(2,6) + HK5*P(4,6) - HK8*P(1,6) - HK9*P(5,6);
const float HK18 = HK10*P(0,4) + HK11*P(4,6) - HK12*P(3,4) + HK13*P(2,4) + HK5*P(4,4) - HK8*P(1,4) - HK9*P(4,5);
const float HK19 = HK10*P(0,3) + HK11*P(3,6) - HK12*P(3,3) + HK13*P(2,3) + HK5*P(3,4) - HK8*P(1,3) - HK9*P(3,5);
const float HK20 = HK10*P(0,2) + HK11*P(2,6) - HK12*P(2,3) + HK13*P(2,2) + HK5*P(2,4) - HK8*P(1,2) - HK9*P(2,5);
const float HK21 = 1.0F/(HK10*HK14 + HK11*HK17 - HK12*HK19 + HK13*HK20 - HK15*HK9 - HK16*HK8 + HK18*HK5 + R_VEL);
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(0) = -2*HK0;
H_VEL(1) = 2*HK1;
H_VEL(2) = -2*HK3;
H_VEL(3) = 2*HK4;
H_VEL(4) = -HK5;
H_VEL(5) = 2*HK6;
H_VEL(6) = -2*HK7;
H_VEL(7) = 0;
H_VEL(8) = 0;
H_VEL(9) = 0;
@@ -55,30 +49,30 @@ H_VEL(22) = 0;
H_VEL(23) = 0;
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));
Kfusion(0) = -HK14*HK21;
Kfusion(1) = -HK16*HK21;
Kfusion(2) = -HK20*HK21;
Kfusion(3) = -HK19*HK21;
Kfusion(4) = -HK18*HK21;
Kfusion(5) = -HK15*HK21;
Kfusion(6) = -HK17*HK21;
Kfusion(7) = -HK21*(HK10*P(0,7) + HK11*P(6,7) - HK12*P(3,7) + HK13*P(2,7) + HK5*P(4,7) - HK8*P(1,7) - HK9*P(5,7));
Kfusion(8) = -HK21*(HK10*P(0,8) + HK11*P(6,8) - HK12*P(3,8) + HK13*P(2,8) + HK5*P(4,8) - HK8*P(1,8) - HK9*P(5,8));
Kfusion(9) = -HK21*(HK10*P(0,9) + HK11*P(6,9) - HK12*P(3,9) + HK13*P(2,9) + HK5*P(4,9) - HK8*P(1,9) - HK9*P(5,9));
Kfusion(10) = -HK21*(HK10*P(0,10) + HK11*P(6,10) - HK12*P(3,10) + HK13*P(2,10) + HK5*P(4,10) - HK8*P(1,10) - HK9*P(5,10));
Kfusion(11) = -HK21*(HK10*P(0,11) + HK11*P(6,11) - HK12*P(3,11) + HK13*P(2,11) + HK5*P(4,11) - HK8*P(1,11) - HK9*P(5,11));
Kfusion(12) = -HK21*(HK10*P(0,12) + HK11*P(6,12) - HK12*P(3,12) + HK13*P(2,12) + HK5*P(4,12) - HK8*P(1,12) - HK9*P(5,12));
Kfusion(13) = -HK21*(HK10*P(0,13) + HK11*P(6,13) - HK12*P(3,13) + HK13*P(2,13) + HK5*P(4,13) - HK8*P(1,13) - HK9*P(5,13));
Kfusion(14) = -HK21*(HK10*P(0,14) + HK11*P(6,14) - HK12*P(3,14) + HK13*P(2,14) + HK5*P(4,14) - HK8*P(1,14) - HK9*P(5,14));
Kfusion(15) = -HK21*(HK10*P(0,15) + HK11*P(6,15) - HK12*P(3,15) + HK13*P(2,15) + HK5*P(4,15) - HK8*P(1,15) - HK9*P(5,15));
Kfusion(16) = -HK21*(HK10*P(0,16) + HK11*P(6,16) - HK12*P(3,16) + HK13*P(2,16) + HK5*P(4,16) - HK8*P(1,16) - HK9*P(5,16));
Kfusion(17) = -HK21*(HK10*P(0,17) + HK11*P(6,17) - HK12*P(3,17) + HK13*P(2,17) + HK5*P(4,17) - HK8*P(1,17) - HK9*P(5,17));
Kfusion(18) = -HK21*(HK10*P(0,18) + HK11*P(6,18) - HK12*P(3,18) + HK13*P(2,18) + HK5*P(4,18) - HK8*P(1,18) - HK9*P(5,18));
Kfusion(19) = -HK21*(HK10*P(0,19) + HK11*P(6,19) - HK12*P(3,19) + HK13*P(2,19) + HK5*P(4,19) - HK8*P(1,19) - HK9*P(5,19));
Kfusion(20) = -HK21*(HK10*P(0,20) + HK11*P(6,20) - HK12*P(3,20) + HK13*P(2,20) + HK5*P(4,20) - HK8*P(1,20) - HK9*P(5,20));
Kfusion(21) = -HK21*(HK10*P(0,21) + HK11*P(6,21) - HK12*P(3,21) + HK13*P(2,21) + HK5*P(4,21) - HK8*P(1,21) - HK9*P(5,21));
Kfusion(22) = -HK21*(HK10*P(0,22) + HK11*P(6,22) - HK12*P(3,22) + HK13*P(2,22) + HK5*P(4,22) - HK8*P(1,22) - HK9*P(5,22));
Kfusion(23) = -HK21*(HK10*P(0,23) + HK11*P(6,23) - HK12*P(3,23) + HK13*P(2,23) + HK5*P(4,23) - HK8*P(1,23) - HK9*P(5,23));
// axis 1
@@ -86,38 +80,33 @@ 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);
const float HK4 = HK1*q3 + q0*vn - q2*vd;
const float HK5 = q0*q3 - q1*q2;
const float HK6 = 2*powf(q1, 2) + 2*powf(q3, 2) - 1;
const float HK7 = q0*q1 + q2*q3;
const float HK8 = 2*HK3;
const float HK9 = 2*HK7;
const float HK10 = 2*HK0;
const float HK11 = 2*HK5;
const float HK12 = 2*HK2;
const float HK13 = 2*HK4;
const float HK14 = HK10*P(0,0) - HK11*P(0,4) + HK12*P(0,1) - HK13*P(0,3) - HK6*P(0,5) + HK8*P(0,2) + HK9*P(0,6);
const float HK15 = HK10*P(0,6) - HK11*P(4,6) + HK12*P(1,6) - HK13*P(3,6) - HK6*P(5,6) + HK8*P(2,6) + HK9*P(6,6);
const float HK16 = HK10*P(0,2) - HK11*P(2,4) + HK12*P(1,2) - HK13*P(2,3) - HK6*P(2,5) + HK8*P(2,2) + HK9*P(2,6);
const float HK17 = HK10*P(0,4) - HK11*P(4,4) + HK12*P(1,4) - HK13*P(3,4) - HK6*P(4,5) + HK8*P(2,4) + HK9*P(4,6);
const float HK18 = HK10*P(0,1) - HK11*P(1,4) + HK12*P(1,1) - HK13*P(1,3) - HK6*P(1,5) + HK8*P(1,2) + HK9*P(1,6);
const float HK19 = HK10*P(0,5) - HK11*P(4,5) + HK12*P(1,5) - HK13*P(3,5) - HK6*P(5,5) + HK8*P(2,5) + HK9*P(5,6);
const float HK20 = HK10*P(0,3) - HK11*P(3,4) + HK12*P(1,3) - HK13*P(3,3) - HK6*P(3,5) + HK8*P(2,3) + HK9*P(3,6);
const float HK21 = 1.0F/(HK10*HK14 - HK11*HK17 + HK12*HK18 - HK13*HK20 + HK15*HK9 + HK16*HK8 - HK19*HK6 + R_VEL);
H_VEL(0) = 2*HK0;
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(3) = -2*HK4;
H_VEL(4) = -2*HK5;
H_VEL(5) = -HK6;
H_VEL(6) = 2*HK7;
H_VEL(7) = 0;
H_VEL(8) = 0;
H_VEL(9) = 0;
@@ -137,70 +126,64 @@ H_VEL(22) = 0;
H_VEL(23) = 0;
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));
Kfusion(0) = HK14*HK21;
Kfusion(1) = HK18*HK21;
Kfusion(2) = HK16*HK21;
Kfusion(3) = HK20*HK21;
Kfusion(4) = HK17*HK21;
Kfusion(5) = HK19*HK21;
Kfusion(6) = HK15*HK21;
Kfusion(7) = HK21*(HK10*P(0,7) - HK11*P(4,7) + HK12*P(1,7) - HK13*P(3,7) - HK6*P(5,7) + HK8*P(2,7) + HK9*P(6,7));
Kfusion(8) = HK21*(HK10*P(0,8) - HK11*P(4,8) + HK12*P(1,8) - HK13*P(3,8) - HK6*P(5,8) + HK8*P(2,8) + HK9*P(6,8));
Kfusion(9) = HK21*(HK10*P(0,9) - HK11*P(4,9) + HK12*P(1,9) - HK13*P(3,9) - HK6*P(5,9) + HK8*P(2,9) + HK9*P(6,9));
Kfusion(10) = HK21*(HK10*P(0,10) - HK11*P(4,10) + HK12*P(1,10) - HK13*P(3,10) - HK6*P(5,10) + HK8*P(2,10) + HK9*P(6,10));
Kfusion(11) = HK21*(HK10*P(0,11) - HK11*P(4,11) + HK12*P(1,11) - HK13*P(3,11) - HK6*P(5,11) + HK8*P(2,11) + HK9*P(6,11));
Kfusion(12) = HK21*(HK10*P(0,12) - HK11*P(4,12) + HK12*P(1,12) - HK13*P(3,12) - HK6*P(5,12) + HK8*P(2,12) + HK9*P(6,12));
Kfusion(13) = HK21*(HK10*P(0,13) - HK11*P(4,13) + HK12*P(1,13) - HK13*P(3,13) - HK6*P(5,13) + HK8*P(2,13) + HK9*P(6,13));
Kfusion(14) = HK21*(HK10*P(0,14) - HK11*P(4,14) + HK12*P(1,14) - HK13*P(3,14) - HK6*P(5,14) + HK8*P(2,14) + HK9*P(6,14));
Kfusion(15) = HK21*(HK10*P(0,15) - HK11*P(4,15) + HK12*P(1,15) - HK13*P(3,15) - HK6*P(5,15) + HK8*P(2,15) + HK9*P(6,15));
Kfusion(16) = HK21*(HK10*P(0,16) - HK11*P(4,16) + HK12*P(1,16) - HK13*P(3,16) - HK6*P(5,16) + HK8*P(2,16) + HK9*P(6,16));
Kfusion(17) = HK21*(HK10*P(0,17) - HK11*P(4,17) + HK12*P(1,17) - HK13*P(3,17) - HK6*P(5,17) + HK8*P(2,17) + HK9*P(6,17));
Kfusion(18) = HK21*(HK10*P(0,18) - HK11*P(4,18) + HK12*P(1,18) - HK13*P(3,18) - HK6*P(5,18) + HK8*P(2,18) + HK9*P(6,18));
Kfusion(19) = HK21*(HK10*P(0,19) - HK11*P(4,19) + HK12*P(1,19) - HK13*P(3,19) - HK6*P(5,19) + HK8*P(2,19) + HK9*P(6,19));
Kfusion(20) = HK21*(HK10*P(0,20) - HK11*P(4,20) + HK12*P(1,20) - HK13*P(3,20) - HK6*P(5,20) + HK8*P(2,20) + HK9*P(6,20));
Kfusion(21) = HK21*(HK10*P(0,21) - HK11*P(4,21) + HK12*P(1,21) - HK13*P(3,21) - HK6*P(5,21) + HK8*P(2,21) + HK9*P(6,21));
Kfusion(22) = HK21*(HK10*P(0,22) - HK11*P(4,22) + HK12*P(1,22) - HK13*P(3,22) - HK6*P(5,22) + HK8*P(2,22) + HK9*P(6,22));
Kfusion(23) = HK21*(HK10*P(0,23) - HK11*P(4,23) + HK12*P(1,23) - HK13*P(3,23) - HK6*P(5,23) + HK8*P(2,23) + HK9*P(6,23));
// axis 2
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);
const float HK0 = q1*ve - q2*vn;
const float HK1 = 2*vd;
const float HK2 = HK1*q1 + q0*ve - q3*vn;
const float HK3 = -HK1*q2 + q0*vn + q3*ve;
const float HK4 = q1*vn + q2*ve;
const float HK5 = q0*q2 + q1*q3;
const float HK6 = q0*q1 - q2*q3;
const float HK7 = 2*powf(q1, 2) + 2*powf(q2, 2) - 1;
const float HK8 = 2*HK4;
const float HK9 = 2*HK5;
const float HK10 = 2*HK0;
const float HK11 = 2*HK6;
const float HK12 = 2*HK3;
const float HK13 = 2*HK2;
const float HK14 = HK10*P(0,0) + HK11*P(0,5) - HK12*P(0,2) + HK13*P(0,1) + HK7*P(0,6) - HK8*P(0,3) - HK9*P(0,4);
const float HK15 = HK10*P(0,4) + HK11*P(4,5) - HK12*P(2,4) + HK13*P(1,4) + HK7*P(4,6) - HK8*P(3,4) - HK9*P(4,4);
const float HK16 = HK10*P(0,3) + HK11*P(3,5) - HK12*P(2,3) + HK13*P(1,3) + HK7*P(3,6) - HK8*P(3,3) - HK9*P(3,4);
const float HK17 = HK10*P(0,5) + HK11*P(5,5) - HK12*P(2,5) + HK13*P(1,5) + HK7*P(5,6) - HK8*P(3,5) - HK9*P(4,5);
const float HK18 = HK10*P(0,6) + HK11*P(5,6) - HK12*P(2,6) + HK13*P(1,6) + HK7*P(6,6) - HK8*P(3,6) - HK9*P(4,6);
const float HK19 = HK10*P(0,2) + HK11*P(2,5) - HK12*P(2,2) + HK13*P(1,2) + HK7*P(2,6) - HK8*P(2,3) - HK9*P(2,4);
const float HK20 = HK10*P(0,1) + HK11*P(1,5) - HK12*P(1,2) + HK13*P(1,1) + HK7*P(1,6) - HK8*P(1,3) - HK9*P(1,4);
const float HK21 = 1.0F/(HK10*HK14 + HK11*HK17 - HK12*HK19 + HK13*HK20 - HK15*HK9 - HK16*HK8 + HK18*HK7 + R_VEL);
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(0) = -2*HK0;
H_VEL(1) = -2*HK2;
H_VEL(2) = 2*HK3;
H_VEL(3) = 2*HK4;
H_VEL(4) = 2*HK5;
H_VEL(5) = -2*HK6;
H_VEL(6) = -HK7;
H_VEL(7) = 0;
H_VEL(8) = 0;
H_VEL(9) = 0;
@@ -220,27 +203,29 @@ H_VEL(22) = 0;
H_VEL(23) = 0;
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));
Kfusion(0) = -HK14*HK21;
Kfusion(1) = -HK20*HK21;
Kfusion(2) = -HK19*HK21;
Kfusion(3) = -HK16*HK21;
Kfusion(4) = -HK15*HK21;
Kfusion(5) = -HK17*HK21;
Kfusion(6) = -HK18*HK21;
Kfusion(7) = -HK21*(HK10*P(0,7) + HK11*P(5,7) - HK12*P(2,7) + HK13*P(1,7) + HK7*P(6,7) - HK8*P(3,7) - HK9*P(4,7));
Kfusion(8) = -HK21*(HK10*P(0,8) + HK11*P(5,8) - HK12*P(2,8) + HK13*P(1,8) + HK7*P(6,8) - HK8*P(3,8) - HK9*P(4,8));
Kfusion(9) = -HK21*(HK10*P(0,9) + HK11*P(5,9) - HK12*P(2,9) + HK13*P(1,9) + HK7*P(6,9) - HK8*P(3,9) - HK9*P(4,9));
Kfusion(10) = -HK21*(HK10*P(0,10) + HK11*P(5,10) - HK12*P(2,10) + HK13*P(1,10) + HK7*P(6,10) - HK8*P(3,10) - HK9*P(4,10));
Kfusion(11) = -HK21*(HK10*P(0,11) + HK11*P(5,11) - HK12*P(2,11) + HK13*P(1,11) + HK7*P(6,11) - HK8*P(3,11) - HK9*P(4,11));
Kfusion(12) = -HK21*(HK10*P(0,12) + HK11*P(5,12) - HK12*P(2,12) + HK13*P(1,12) + HK7*P(6,12) - HK8*P(3,12) - HK9*P(4,12));
Kfusion(13) = -HK21*(HK10*P(0,13) + HK11*P(5,13) - HK12*P(2,13) + HK13*P(1,13) + HK7*P(6,13) - HK8*P(3,13) - HK9*P(4,13));
Kfusion(14) = -HK21*(HK10*P(0,14) + HK11*P(5,14) - HK12*P(2,14) + HK13*P(1,14) + HK7*P(6,14) - HK8*P(3,14) - HK9*P(4,14));
Kfusion(15) = -HK21*(HK10*P(0,15) + HK11*P(5,15) - HK12*P(2,15) + HK13*P(1,15) + HK7*P(6,15) - HK8*P(3,15) - HK9*P(4,15));
Kfusion(16) = -HK21*(HK10*P(0,16) + HK11*P(5,16) - HK12*P(2,16) + HK13*P(1,16) + HK7*P(6,16) - HK8*P(3,16) - HK9*P(4,16));
Kfusion(17) = -HK21*(HK10*P(0,17) + HK11*P(5,17) - HK12*P(2,17) + HK13*P(1,17) + HK7*P(6,17) - HK8*P(3,17) - HK9*P(4,17));
Kfusion(18) = -HK21*(HK10*P(0,18) + HK11*P(5,18) - HK12*P(2,18) + HK13*P(1,18) + HK7*P(6,18) - HK8*P(3,18) - HK9*P(4,18));
Kfusion(19) = -HK21*(HK10*P(0,19) + HK11*P(5,19) - HK12*P(2,19) + HK13*P(1,19) + HK7*P(6,19) - HK8*P(3,19) - HK9*P(4,19));
Kfusion(20) = -HK21*(HK10*P(0,20) + HK11*P(5,20) - HK12*P(2,20) + HK13*P(1,20) + HK7*P(6,20) - HK8*P(3,20) - HK9*P(4,20));
Kfusion(21) = -HK21*(HK10*P(0,21) + HK11*P(5,21) - HK12*P(2,21) + HK13*P(1,21) + HK7*P(6,21) - HK8*P(3,21) - HK9*P(4,21));
Kfusion(22) = -HK21*(HK10*P(0,22) + HK11*P(5,22) - HK12*P(2,22) + HK13*P(1,22) + HK7*P(6,22) - HK8*P(3,22) - HK9*P(4,22));
Kfusion(23) = -HK21*(HK10*P(0,23) + HK11*P(5,23) - HK12*P(2,23) + HK13*P(1,23) + HK7*P(6,23) - HK8*P(3,23) - HK9*P(4,23));
src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated_alt.cpp
+176 -178
View File
@@ -1,100 +1,96 @@
// Sub Expressions
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 HK0 = q2*vd;
const float HK1 = HK0 - q3*ve;
const float HK2 = q2*ve;
const float HK3 = q3*vd;
const float HK4 = HK2 + HK3;
const float HK5 = q0*vd;
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 - 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*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);
const float HK7 = q2*vn;
const float HK8 = HK5 - HK6 + 2*HK7;
const float HK9 = q0*ve;
const float HK10 = q1*vd;
const float HK11 = q3*vn;
const float HK12 = HK10 - 2*HK11 + HK9;
const float HK13 = 2*powf(q2, 2);
const float HK14 = 2*powf(q3, 2) - 1;
const float HK15 = HK13 + HK14;
const float HK16 = q0*q3;
const float HK17 = HK16 + q1*q2;
const float HK18 = q0*q2;
const float HK19 = HK18 - q1*q3;
const float HK20 = 2*HK4;
const float HK21 = 2*HK17;
const float HK22 = 2*HK1;
const float HK23 = 2*HK19;
const float HK24 = 2*HK12;
const float HK25 = 2*HK8;
const float HK26 = HK15*P(0,4) - HK20*P(0,1) - HK21*P(0,5) + HK22*P(0,0) + HK23*P(0,6) - HK24*P(0,3) + HK25*P(0,2);
const float HK27 = HK15*P(4,5) - HK20*P(1,5) - HK21*P(5,5) + HK22*P(0,5) + HK23*P(5,6) - HK24*P(3,5) + HK25*P(2,5);
const float HK28 = HK15*P(1,4) - HK20*P(1,1) - HK21*P(1,5) + HK22*P(0,1) + HK23*P(1,6) - HK24*P(1,3) + HK25*P(1,2);
const float HK29 = HK15*P(4,6) - HK20*P(1,6) - HK21*P(5,6) + HK22*P(0,6) + HK23*P(6,6) - HK24*P(3,6) + HK25*P(2,6);
const float HK30 = HK15*P(4,4) - HK20*P(1,4) - HK21*P(4,5) + HK22*P(0,4) + HK23*P(4,6) - HK24*P(3,4) + HK25*P(2,4);
const float HK31 = HK15*P(3,4) - HK20*P(1,3) - HK21*P(3,5) + HK22*P(0,3) + HK23*P(3,6) - HK24*P(3,3) + HK25*P(2,3);
const float HK32 = HK15*P(2,4) - HK20*P(1,2) - HK21*P(2,5) + HK22*P(0,2) + HK23*P(2,6) - HK24*P(2,3) + HK25*P(2,2);
const float HK33 = 1.0F/(HK15*HK30 - HK20*HK28 - HK21*HK27 + HK22*HK26 + HK23*HK29 - HK24*HK31 + HK25*HK32 + R_VEL);
const float HK34 = -HK11;
const float HK35 = HK10 + HK34;
const float HK36 = HK5 - 2*HK6 + HK7;
const float HK37 = q1*vn;
const float HK38 = HK3 + HK37;
const float HK39 = q0*vn;
const float HK40 = q3*ve;
const float HK41 = -HK0 + HK39 + 2*HK40;
const float HK42 = HK16 - q1*q2;
const float HK43 = 2*powf(q1, 2);
const float HK44 = HK14 + HK43;
const float HK45 = q0*q1;
const float HK46 = HK45 + q2*q3;
const float HK47 = 2*HK38;
const float HK48 = 2*HK46;
const float HK49 = 2*HK35;
const float HK50 = 2*HK42;
const float HK51 = 2*HK36;
const float HK52 = 2*HK41;
const float HK53 = -HK44*P(0,5) + HK47*P(0,2) + HK48*P(0,6) + HK49*P(0,0) - HK50*P(0,4) + HK51*P(0,1) - HK52*P(0,3);
const float HK54 = -HK44*P(5,6) + HK47*P(2,6) + HK48*P(6,6) + HK49*P(0,6) - HK50*P(4,6) + HK51*P(1,6) - HK52*P(3,6);
const float HK55 = -HK44*P(2,5) + HK47*P(2,2) + HK48*P(2,6) + HK49*P(0,2) - HK50*P(2,4) + HK51*P(1,2) - HK52*P(2,3);
const float HK56 = -HK44*P(4,5) + HK47*P(2,4) + HK48*P(4,6) + HK49*P(0,4) - HK50*P(4,4) + HK51*P(1,4) - HK52*P(3,4);
const float HK57 = -HK44*P(1,5) + HK47*P(1,2) + HK48*P(1,6) + HK49*P(0,1) - HK50*P(1,4) + HK51*P(1,1) - HK52*P(1,3);
const float HK58 = -HK44*P(5,5) + HK47*P(2,5) + HK48*P(5,6) + HK49*P(0,5) - HK50*P(4,5) + HK51*P(1,5) - HK52*P(3,5);
const float HK59 = -HK44*P(3,5) + HK47*P(2,3) + HK48*P(3,6) + HK49*P(0,3) - HK50*P(3,4) + HK51*P(1,3) - HK52*P(3,3);
const float HK60 = 1.0F/(-HK44*HK58 + HK47*HK55 + HK48*HK54 + HK49*HK53 - HK50*HK56 + HK51*HK57 - HK52*HK59 + R_VEL);
const float HK61 = HK6 - q2*vn;
const float HK62 = 2*HK10 + HK34 + HK9;
const float HK63 = -2*HK0 + HK39 + HK40;
const float HK64 = HK2 + HK37;
const float HK65 = HK18 + q1*q3;
const float HK66 = HK45 - q2*q3;
const float HK67 = HK13 + HK43 - 1;
const float HK68 = 2*HK64;
const float HK69 = 2*HK65;
const float HK70 = 2*HK61;
const float HK71 = 2*HK66;
const float HK72 = 2*HK63;
const float HK73 = 2*HK62;
const float HK74 = HK67*P(0,6) - HK68*P(0,3) - HK69*P(0,4) + HK70*P(0,0) + HK71*P(0,5) - HK72*P(0,2) + HK73*P(0,1);
const float HK75 = HK67*P(4,6) - HK68*P(3,4) - HK69*P(4,4) + HK70*P(0,4) + HK71*P(4,5) - HK72*P(2,4) + HK73*P(1,4);
const float HK76 = HK67*P(3,6) - HK68*P(3,3) - HK69*P(3,4) + HK70*P(0,3) + HK71*P(3,5) - HK72*P(2,3) + HK73*P(1,3);
const float HK77 = HK67*P(5,6) - HK68*P(3,5) - HK69*P(4,5) + HK70*P(0,5) + HK71*P(5,5) - HK72*P(2,5) + HK73*P(1,5);
const float HK78 = HK67*P(6,6) - HK68*P(3,6) - HK69*P(4,6) + HK70*P(0,6) + HK71*P(5,6) - HK72*P(2,6) + HK73*P(1,6);
const float HK79 = HK67*P(2,6) - HK68*P(2,3) - HK69*P(2,4) + HK70*P(0,2) + HK71*P(2,5) - HK72*P(2,2) + HK73*P(1,2);
const float HK80 = HK67*P(1,6) - HK68*P(1,3) - HK69*P(1,4) + HK70*P(0,1) + HK71*P(1,5) - HK72*P(1,2) + HK73*P(1,1);
const float HK81 = 1.0F/(HK67*HK78 - HK68*HK76 - HK69*HK75 + HK70*HK74 + HK71*HK77 - HK72*HK79 + HK73*HK80 + R_VEL);
// Observation Jacobians - axis 0
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<0>() = -2*HK1;
Hfusion.at<1>() = 2*HK4;
Hfusion.at<2>() = -2*HK8;
Hfusion.at<3>() = 2*HK12;
Hfusion.at<4>() = -HK15;
Hfusion.at<5>() = 2*HK17;
Hfusion.at<6>() = -2*HK19;
Hfusion.at<7>() = 0;
Hfusion.at<8>() = 0;
Hfusion.at<9>() = 0;
@@ -115,40 +111,40 @@ Hfusion.at<23>() = 0;
// Kalman gains - axis 0
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));
Kfusion(0) = -HK26*HK33;
Kfusion(1) = -HK28*HK33;
Kfusion(2) = -HK32*HK33;
Kfusion(3) = -HK31*HK33;
Kfusion(4) = -HK30*HK33;
Kfusion(5) = -HK27*HK33;
Kfusion(6) = -HK29*HK33;
Kfusion(7) = -HK33*(HK15*P(4,7) - HK20*P(1,7) - HK21*P(5,7) + HK22*P(0,7) + HK23*P(6,7) - HK24*P(3,7) + HK25*P(2,7));
Kfusion(8) = -HK33*(HK15*P(4,8) - HK20*P(1,8) - HK21*P(5,8) + HK22*P(0,8) + HK23*P(6,8) - HK24*P(3,8) + HK25*P(2,8));
Kfusion(9) = -HK33*(HK15*P(4,9) - HK20*P(1,9) - HK21*P(5,9) + HK22*P(0,9) + HK23*P(6,9) - HK24*P(3,9) + HK25*P(2,9));
Kfusion(10) = -HK33*(HK15*P(4,10) - HK20*P(1,10) - HK21*P(5,10) + HK22*P(0,10) + HK23*P(6,10) - HK24*P(3,10) + HK25*P(2,10));
Kfusion(11) = -HK33*(HK15*P(4,11) - HK20*P(1,11) - HK21*P(5,11) + HK22*P(0,11) + HK23*P(6,11) - HK24*P(3,11) + HK25*P(2,11));
Kfusion(12) = -HK33*(HK15*P(4,12) - HK20*P(1,12) - HK21*P(5,12) + HK22*P(0,12) + HK23*P(6,12) - HK24*P(3,12) + HK25*P(2,12));
Kfusion(13) = -HK33*(HK15*P(4,13) - HK20*P(1,13) - HK21*P(5,13) + HK22*P(0,13) + HK23*P(6,13) - HK24*P(3,13) + HK25*P(2,13));
Kfusion(14) = -HK33*(HK15*P(4,14) - HK20*P(1,14) - HK21*P(5,14) + HK22*P(0,14) + HK23*P(6,14) - HK24*P(3,14) + HK25*P(2,14));
Kfusion(15) = -HK33*(HK15*P(4,15) - HK20*P(1,15) - HK21*P(5,15) + HK22*P(0,15) + HK23*P(6,15) - HK24*P(3,15) + HK25*P(2,15));
Kfusion(16) = -HK33*(HK15*P(4,16) - HK20*P(1,16) - HK21*P(5,16) + HK22*P(0,16) + HK23*P(6,16) - HK24*P(3,16) + HK25*P(2,16));
Kfusion(17) = -HK33*(HK15*P(4,17) - HK20*P(1,17) - HK21*P(5,17) + HK22*P(0,17) + HK23*P(6,17) - HK24*P(3,17) + HK25*P(2,17));
Kfusion(18) = -HK33*(HK15*P(4,18) - HK20*P(1,18) - HK21*P(5,18) + HK22*P(0,18) + HK23*P(6,18) - HK24*P(3,18) + HK25*P(2,18));
Kfusion(19) = -HK33*(HK15*P(4,19) - HK20*P(1,19) - HK21*P(5,19) + HK22*P(0,19) + HK23*P(6,19) - HK24*P(3,19) + HK25*P(2,19));
Kfusion(20) = -HK33*(HK15*P(4,20) - HK20*P(1,20) - HK21*P(5,20) + HK22*P(0,20) + HK23*P(6,20) - HK24*P(3,20) + HK25*P(2,20));
Kfusion(21) = -HK33*(HK15*P(4,21) - HK20*P(1,21) - HK21*P(5,21) + HK22*P(0,21) + HK23*P(6,21) - HK24*P(3,21) + HK25*P(2,21));
Kfusion(22) = -HK33*(HK15*P(4,22) - HK20*P(1,22) - HK21*P(5,22) + HK22*P(0,22) + HK23*P(6,22) - HK24*P(3,22) + HK25*P(2,22));
Kfusion(23) = -HK33*(HK15*P(4,23) - HK20*P(1,23) - HK21*P(5,23) + HK22*P(0,23) + HK23*P(6,23) - HK24*P(3,23) + HK25*P(2,23));
// Observation Jacobians - axis 1
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<0>() = 2*HK35;
Hfusion.at<1>() = 2*HK36;
Hfusion.at<2>() = 2*HK38;
Hfusion.at<3>() = -2*HK41;
Hfusion.at<4>() = -2*HK42;
Hfusion.at<5>() = -HK44;
Hfusion.at<6>() = 2*HK46;
Hfusion.at<7>() = 0;
Hfusion.at<8>() = 0;
Hfusion.at<9>() = 0;
@@ -169,40 +165,40 @@ Hfusion.at<23>() = 0;
// Kalman gains - axis 1
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));
Kfusion(0) = HK53*HK60;
Kfusion(1) = HK57*HK60;
Kfusion(2) = HK55*HK60;
Kfusion(3) = HK59*HK60;
Kfusion(4) = HK56*HK60;
Kfusion(5) = HK58*HK60;
Kfusion(6) = HK54*HK60;
Kfusion(7) = HK60*(-HK44*P(5,7) + HK47*P(2,7) + HK48*P(6,7) + HK49*P(0,7) - HK50*P(4,7) + HK51*P(1,7) - HK52*P(3,7));
Kfusion(8) = HK60*(-HK44*P(5,8) + HK47*P(2,8) + HK48*P(6,8) + HK49*P(0,8) - HK50*P(4,8) + HK51*P(1,8) - HK52*P(3,8));
Kfusion(9) = HK60*(-HK44*P(5,9) + HK47*P(2,9) + HK48*P(6,9) + HK49*P(0,9) - HK50*P(4,9) + HK51*P(1,9) - HK52*P(3,9));
Kfusion(10) = HK60*(-HK44*P(5,10) + HK47*P(2,10) + HK48*P(6,10) + HK49*P(0,10) - HK50*P(4,10) + HK51*P(1,10) - HK52*P(3,10));
Kfusion(11) = HK60*(-HK44*P(5,11) + HK47*P(2,11) + HK48*P(6,11) + HK49*P(0,11) - HK50*P(4,11) + HK51*P(1,11) - HK52*P(3,11));
Kfusion(12) = HK60*(-HK44*P(5,12) + HK47*P(2,12) + HK48*P(6,12) + HK49*P(0,12) - HK50*P(4,12) + HK51*P(1,12) - HK52*P(3,12));
Kfusion(13) = HK60*(-HK44*P(5,13) + HK47*P(2,13) + HK48*P(6,13) + HK49*P(0,13) - HK50*P(4,13) + HK51*P(1,13) - HK52*P(3,13));
Kfusion(14) = HK60*(-HK44*P(5,14) + HK47*P(2,14) + HK48*P(6,14) + HK49*P(0,14) - HK50*P(4,14) + HK51*P(1,14) - HK52*P(3,14));
Kfusion(15) = HK60*(-HK44*P(5,15) + HK47*P(2,15) + HK48*P(6,15) + HK49*P(0,15) - HK50*P(4,15) + HK51*P(1,15) - HK52*P(3,15));
Kfusion(16) = HK60*(-HK44*P(5,16) + HK47*P(2,16) + HK48*P(6,16) + HK49*P(0,16) - HK50*P(4,16) + HK51*P(1,16) - HK52*P(3,16));
Kfusion(17) = HK60*(-HK44*P(5,17) + HK47*P(2,17) + HK48*P(6,17) + HK49*P(0,17) - HK50*P(4,17) + HK51*P(1,17) - HK52*P(3,17));
Kfusion(18) = HK60*(-HK44*P(5,18) + HK47*P(2,18) + HK48*P(6,18) + HK49*P(0,18) - HK50*P(4,18) + HK51*P(1,18) - HK52*P(3,18));
Kfusion(19) = HK60*(-HK44*P(5,19) + HK47*P(2,19) + HK48*P(6,19) + HK49*P(0,19) - HK50*P(4,19) + HK51*P(1,19) - HK52*P(3,19));
Kfusion(20) = HK60*(-HK44*P(5,20) + HK47*P(2,20) + HK48*P(6,20) + HK49*P(0,20) - HK50*P(4,20) + HK51*P(1,20) - HK52*P(3,20));
Kfusion(21) = HK60*(-HK44*P(5,21) + HK47*P(2,21) + HK48*P(6,21) + HK49*P(0,21) - HK50*P(4,21) + HK51*P(1,21) - HK52*P(3,21));
Kfusion(22) = HK60*(-HK44*P(5,22) + HK47*P(2,22) + HK48*P(6,22) + HK49*P(0,22) - HK50*P(4,22) + HK51*P(1,22) - HK52*P(3,22));
Kfusion(23) = HK60*(-HK44*P(5,23) + HK47*P(2,23) + HK48*P(6,23) + HK49*P(0,23) - HK50*P(4,23) + HK51*P(1,23) - HK52*P(3,23));
// Observation Jacobians - axis 2
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*HK49 + 2*HK50;
Hfusion.at<6>() = HK15 + HK48 + 1;
Hfusion.at<0>() = -2*HK61;
Hfusion.at<1>() = -2*HK62;
Hfusion.at<2>() = 2*HK63;
Hfusion.at<3>() = 2*HK64;
Hfusion.at<4>() = 2*HK65;
Hfusion.at<5>() = -2*HK66;
Hfusion.at<6>() = -HK67;
Hfusion.at<7>() = 0;
Hfusion.at<8>() = 0;
Hfusion.at<9>() = 0;
@@ -223,27 +219,29 @@ Hfusion.at<23>() = 0;
// Kalman gains - axis 2
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));
Kfusion(0) = -HK74*HK81;
Kfusion(1) = -HK80*HK81;
Kfusion(2) = -HK79*HK81;
Kfusion(3) = -HK76*HK81;
Kfusion(4) = -HK75*HK81;
Kfusion(5) = -HK77*HK81;
Kfusion(6) = -HK78*HK81;
Kfusion(7) = -HK81*(HK67*P(6,7) - HK68*P(3,7) - HK69*P(4,7) + HK70*P(0,7) + HK71*P(5,7) - HK72*P(2,7) + HK73*P(1,7));
Kfusion(8) = -HK81*(HK67*P(6,8) - HK68*P(3,8) - HK69*P(4,8) + HK70*P(0,8) + HK71*P(5,8) - HK72*P(2,8) + HK73*P(1,8));
Kfusion(9) = -HK81*(HK67*P(6,9) - HK68*P(3,9) - HK69*P(4,9) + HK70*P(0,9) + HK71*P(5,9) - HK72*P(2,9) + HK73*P(1,9));
Kfusion(10) = -HK81*(HK67*P(6,10) - HK68*P(3,10) - HK69*P(4,10) + HK70*P(0,10) + HK71*P(5,10) - HK72*P(2,10) + HK73*P(1,10));
Kfusion(11) = -HK81*(HK67*P(6,11) - HK68*P(3,11) - HK69*P(4,11) + HK70*P(0,11) + HK71*P(5,11) - HK72*P(2,11) + HK73*P(1,11));
Kfusion(12) = -HK81*(HK67*P(6,12) - HK68*P(3,12) - HK69*P(4,12) + HK70*P(0,12) + HK71*P(5,12) - HK72*P(2,12) + HK73*P(1,12));
Kfusion(13) = -HK81*(HK67*P(6,13) - HK68*P(3,13) - HK69*P(4,13) + HK70*P(0,13) + HK71*P(5,13) - HK72*P(2,13) + HK73*P(1,13));
Kfusion(14) = -HK81*(HK67*P(6,14) - HK68*P(3,14) - HK69*P(4,14) + HK70*P(0,14) + HK71*P(5,14) - HK72*P(2,14) + HK73*P(1,14));
Kfusion(15) = -HK81*(HK67*P(6,15) - HK68*P(3,15) - HK69*P(4,15) + HK70*P(0,15) + HK71*P(5,15) - HK72*P(2,15) + HK73*P(1,15));
Kfusion(16) = -HK81*(HK67*P(6,16) - HK68*P(3,16) - HK69*P(4,16) + HK70*P(0,16) + HK71*P(5,16) - HK72*P(2,16) + HK73*P(1,16));
Kfusion(17) = -HK81*(HK67*P(6,17) - HK68*P(3,17) - HK69*P(4,17) + HK70*P(0,17) + HK71*P(5,17) - HK72*P(2,17) + HK73*P(1,17));
Kfusion(18) = -HK81*(HK67*P(6,18) - HK68*P(3,18) - HK69*P(4,18) + HK70*P(0,18) + HK71*P(5,18) - HK72*P(2,18) + HK73*P(1,18));
Kfusion(19) = -HK81*(HK67*P(6,19) - HK68*P(3,19) - HK69*P(4,19) + HK70*P(0,19) + HK71*P(5,19) - HK72*P(2,19) + HK73*P(1,19));
Kfusion(20) = -HK81*(HK67*P(6,20) - HK68*P(3,20) - HK69*P(4,20) + HK70*P(0,20) + HK71*P(5,20) - HK72*P(2,20) + HK73*P(1,20));
Kfusion(21) = -HK81*(HK67*P(6,21) - HK68*P(3,21) - HK69*P(4,21) + HK70*P(0,21) + HK71*P(5,21) - HK72*P(2,21) + HK73*P(1,21));
Kfusion(22) = -HK81*(HK67*P(6,22) - HK68*P(3,22) - HK69*P(4,22) + HK70*P(0,22) + HK71*P(5,22) - HK72*P(2,22) + HK73*P(1,22));
Kfusion(23) = -HK81*(HK67*P(6,23) - HK68*P(3,23) - HK69*P(4,23) + HK70*P(0,23) + HK71*P(5,23) - HK72*P(2,23) + HK73*P(1,23));
src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_estimator_covariance_prediction_generated.cpp
+2
View File
@@ -17,3 +17,5 @@ _ekf_gsf[model_index].P(1,1) = P(1,1) + P(1,2)*S6 + S1*dvyVar + S3*dvxVar + S6*S
_ekf_gsf[model_index].P(0,2) = S5;
_ekf_gsf[model_index].P(1,2) = S9;
_ekf_gsf[model_index].P(2,2) = P(2,2) + dazVar;
src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_estimator_measurement_update_generated.cpp
+32 -36
View File
@@ -9,33 +9,27 @@ const float t6 = -t5;
const float t7 = P(0,0) + velObsVar;
const float t8 = P(0,0)*t4 + t1;
const float t9 = t5*velObsVar;
const float t10 = P(1,1)*t7;
const float t11 = t1 + t10;
const float t12 = P(0,1)*P(1,2);
const float t13 = P(0,2)*t4;
const float t14 = P(0,1)*P(0,2);
const float t15 = P(1,2)*t7;
const float t16 = t0*velObsVar;
const float t17 = powf(t2, -2);
const float t18 = t4*velObsVar + t8;
const float t19 = t17*t18;
const float t20 = t17*(t16 + t7*t8);
const float t21 = t0 - t10;
const float t22 = t17*t21;
const float t23 = t14 - t15;
const float t24 = P(0,1)*t23;
const float t25 = t12 - t13;
const float t26 = t16 - t21*t4;
const float t27 = t17*t26;
const float t28 = t11 + t7*velObsVar;
const float t29 = t17*t8;
const float t30 = t17*t28;
const float t31 = P(0,1)*t25;
const float t32 = t23*t4 + t31;
const float t33 = t17*t32;
const float t34 = P(0,1)*velObsVar;
const float t35 = t24 + t25*t7;
const float t36 = t17*t35;
const float t10 = -P(1,1)*t7 + t0;
const float t11 = P(0,1)*P(1,2) - P(0,2)*t4;
const float t12 = P(0,1)*P(0,2) - P(1,2)*t7;
const float t13 = t0*velObsVar;
const float t14 = powf(t2, -2);
const float t15 = t4*velObsVar + t8;
const float t16 = t14*t15;
const float t17 = t14*(t13 + t7*t8);
const float t18 = t10*t14;
const float t19 = P(0,1)*t12;
const float t20 = -t10*t4 + t13;
const float t21 = t14*t20;
const float t22 = P(1,1)*t7 + t1 + t7*velObsVar;
const float t23 = t14*t8;
const float t24 = t14*t22;
const float t25 = P(0,1)*t11;
const float t26 = t12*t4 + t25;
const float t27 = t14*t26;
const float t28 = P(0,1)*velObsVar;
const float t29 = t11*t7 + t19;
const float t30 = t14*t29;
// Equations for NE velocity innovation variance's determinante inverse
@@ -51,16 +45,18 @@ _ekf_gsf[model_index].S_inverse(1,1) = t3*t7;
// Equations for NE velocity Kalman gain
K(0,0) = t3*t8;
K(1,0) = t9;
K(2,0) = t3*(-t12 + t13);
K(2,0) = -t11*t3;
K(0,1) = t9;
K(1,1) = t11*t3;
K(2,1) = t3*(-t14 + t15);
K(1,1) = -t10*t3;
K(2,1) = -t12*t3;
// Equations for covariance matrix update
_ekf_gsf[model_index].P(0,0) = P(0,0) - t16*t19 - t20*t8;
_ekf_gsf[model_index].P(0,1) = P(0,1)*(t18*t22 - t20*velObsVar + 1);
_ekf_gsf[model_index].P(1,1) = P(1,1) - t16*t30 + t22*t26;
_ekf_gsf[model_index].P(0,2) = P(0,2) + t19*t24 + t20*t25;
_ekf_gsf[model_index].P(1,2) = P(1,2) + t23*t27 + t30*t31;
_ekf_gsf[model_index].P(2,2) = P(2,2) - t23*t33 - t25*t36;
_ekf_gsf[model_index].P(0,0) = P(0,0) - t13*t16 - t17*t8;
_ekf_gsf[model_index].P(0,1) = P(0,1)*(t15*t18 - t17*velObsVar + 1);
_ekf_gsf[model_index].P(1,1) = P(1,1) - t13*t24 + t18*t20;
_ekf_gsf[model_index].P(0,2) = P(0,2) + t11*t17 + t16*t19;
_ekf_gsf[model_index].P(1,2) = P(1,2) + t12*t21 + t24*t25;
_ekf_gsf[model_index].P(2,2) = P(2,2) - t11*t30 - t12*t27;
src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_generated.cpp
+13 -12
View File
@@ -114,20 +114,19 @@ H_YAW.at<23>() = 0;
// calculate 312 yaw observation matrix - option B
const float SB0 = 2*q0;
const float SB1 = 2*q2;
const float SB2 = -SB0*q3 + SB1*q1;
const float SB3 = powf(SB2, -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 = SB3*SB4;
const float SB7 = 2*SB5*SB6;
const float SB8 = 4/SB2;
const float SB1 = -SB0*q3 + 2*q1*q2;
const float SB2 = powf(SB1, -2);
const float SB3 = 2*powf(q1, 2) + 2*powf(q3, 2) - 1;
const float SB4 = 1.0F/(SB2*powf(SB3, 2) + 1);
const float SB5 = SB2*SB3;
const float SB6 = 2*SB4*SB5;
const float SB7 = 4/SB1;
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<0>() = -SB6*q3;
H_YAW.at<1>() = -SB4*(-2*SB5*q2 + SB7*q1);
H_YAW.at<2>() = SB6*q1;
H_YAW.at<3>() = -SB4*(SB0*SB5 + SB7*q3);
H_YAW.at<4>() = 0;
H_YAW.at<5>() = 0;
H_YAW.at<6>() = 0;
@@ -148,3 +147,5 @@ H_YAW.at<20>() = 0;
H_YAW.at<21>() = 0;
H_YAW.at<22>() = 0;
H_YAW.at<23>() = 0;