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

ekf2: fix bug in derivation of flow observation equations

Browse Source
This commit is contained in:
Paul Riseborough 2022-01-11 20:54:22 +11:00 committed by bresch
parent 294e3abfeb
commit dd841b6765
3 changed files with 519 additions and 330 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

429
src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated.cpp
View File

@ -3,58 +3,146 @@
const float HK0 = -Tbs(1,0)*q2 + Tbs(1,1)*q1 + Tbs(1,2)*q0;
const float HK1 = Tbs(1,0)*q3 + Tbs(1,1)*q0 - Tbs(1,2)*q1;
const float HK2 = Tbs(1,0)*q0 - Tbs(1,1)*q3 + Tbs(1,2)*q2;
const float HK3 = HK0*vd + HK1*ve + HK2*vn;
const float HK4 = 1.0F/range;
const float HK5 = 2*HK4;
const float HK6 = Tbs(1,0)*q1 + Tbs(1,1)*q2 + Tbs(1,2)*q3;
const float HK7 = -HK0*ve + HK1*vd + HK6*vn;
const float HK8 = HK0*vn - HK2*vd + HK6*ve;
const float HK9 = -HK1*vn + HK2*ve + HK6*vd;
const float HK10 = q0*q2;
const float HK11 = q1*q3;
const float HK12 = 2*Tbs(1,2);
const float HK13 = q0*q3;
const float HK14 = q1*q2;
const float HK15 = 2*Tbs(1,1);
const float HK16 = powf(q1, 2);
const float HK17 = powf(q2, 2);
const float HK18 = -HK17;
const float HK19 = powf(q0, 2);
const float HK20 = powf(q3, 2);
const float HK21 = HK19 - HK20;
const float HK22 = HK12*(HK10 + HK11) - HK15*(HK13 - HK14) + Tbs(1,0)*(HK16 + HK18 + HK21);
const float HK23 = 2*Tbs(1,0);
const float HK24 = q0*q1;
const float HK25 = q2*q3;
const float HK26 = -HK16;
const float HK27 = -HK12*(HK24 - HK25) + HK23*(HK13 + HK14) + Tbs(1,1)*(HK17 + HK21 + HK26);
const float HK28 = HK15*(HK24 + HK25) - HK23*(HK10 - HK11) + Tbs(1,2)*(HK18 + HK19 + HK20 + HK26);
const float HK29 = 2*HK3;
const float HK30 = 2*HK7;
const float HK31 = 2*HK8;
const float HK32 = 2*HK9;
const float HK33 = HK22*P(0,4) + HK27*P(0,5) + HK28*P(0,6) + HK29*P(0,0) + HK30*P(0,1) + HK31*P(0,2) + HK32*P(0,3);
const float HK34 = powf(range, -2);
const float HK35 = HK22*P(4,6) + HK27*P(5,6) + HK28*P(6,6) + HK29*P(0,6) + HK30*P(1,6) + HK31*P(2,6) + HK32*P(3,6);
const float HK36 = HK22*P(4,5) + HK27*P(5,5) + HK28*P(5,6) + HK29*P(0,5) + HK30*P(1,5) + HK31*P(2,5) + HK32*P(3,5);
const float HK37 = HK22*P(4,4) + HK27*P(4,5) + HK28*P(4,6) + HK29*P(0,4) + HK30*P(1,4) + HK31*P(2,4) + HK32*P(3,4);
const float HK38 = HK22*P(2,4) + HK27*P(2,5) + HK28*P(2,6) + HK29*P(0,2) + HK30*P(1,2) + HK31*P(2,2) + HK32*P(2,3);
const float HK39 = HK22*P(3,4) + HK27*P(3,5) + HK28*P(3,6) + HK29*P(0,3) + HK30*P(1,3) + HK31*P(2,3) + HK32*P(3,3);
const float HK40 = HK22*P(1,4) + HK27*P(1,5) + HK28*P(1,6) + HK29*P(0,1) + HK30*P(1,1) + HK31*P(1,2) + HK32*P(1,3);
const float HK41 = HK4/(HK22*HK34*HK37 + HK27*HK34*HK36 + HK28*HK34*HK35 + HK29*HK33*HK34 + HK30*HK34*HK40 + HK31*HK34*HK38 + HK32*HK34*HK39 + R_LOS);
const float HK3 = q0*q1;
const float HK4 = q2*q3;
const float HK5 = HK3 + HK4;
const float HK6 = q0*q2;
const float HK7 = q1*q3;
const float HK8 = HK6 - HK7;
const float HK9 = powf(q3, 2);
const float HK10 = powf(q2, 2);
const float HK11 = -HK10;
const float HK12 = powf(q0, 2);
const float HK13 = powf(q1, 2);
const float HK14 = HK12 - HK13;
const float HK15 = HK11 + HK14 + HK9;
const float HK16 = HK15*Tbs(2,2) + 2*HK5*Tbs(2,1) - 2*HK8*Tbs(2,0);
const float HK17 = 2*Tbs(1,1);
const float HK18 = 2*Tbs(1,0);
const float HK19 = HK15*Tbs(1,2) + HK17*HK5 - HK18*HK8;
const float HK20 = q0*q3;
const float HK21 = q1*q2;
const float HK22 = 2*Tbs(1,2);
const float HK23 = -HK9;
const float HK24 = HK18*(HK20 + HK21) - HK22*(HK3 - HK4) + Tbs(1,1)*(HK10 + HK14 + HK23);
const float HK25 = -HK17*(HK20 - HK21) + HK22*(HK6 + HK7) + Tbs(1,0)*(HK11 + HK12 + HK13 + HK23);
const float HK26 = HK19*vd + HK24*ve + HK25*vn;
const float HK27 = HK16*(HK0*vd + HK1*ve + HK2*vn) + HK26*(-Tbs(2,0)*q2 + Tbs(2,1)*q1 + Tbs(2,2)*q0);
const float HK28 = pd - ptd;
const float HK29 = 1.0F/HK28;
const float HK30 = 2*HK29;
const float HK31 = Tbs(1,0)*q1 + Tbs(1,1)*q2 + Tbs(1,2)*q3;
const float HK32 = HK16*(-HK0*ve + HK1*vd + HK31*vn) + HK26*(Tbs(2,0)*q3 + Tbs(2,1)*q0 - Tbs(2,2)*q1);
const float HK33 = -HK16*(HK0*vn - HK2*vd + HK31*ve) + HK26*(Tbs(2,0)*q0 - Tbs(2,1)*q3 + Tbs(2,2)*q2);
const float HK34 = HK16*(-HK1*vn + HK2*ve + HK31*vd) + HK26*(Tbs(2,0)*q1 + Tbs(2,1)*q2 + Tbs(2,2)*q3);
const float HK35 = HK16*HK29;
const float HK36 = powf(HK28, -2);
const float HK37 = HK16*HK26;
const float HK38 = HK36*HK37;
const float HK39 = HK16*HK25;
const float HK40 = HK39*P(0,4);
const float HK41 = HK16*HK24;
const float HK42 = HK41*P(0,5);
const float HK43 = HK16*HK19;
const float HK44 = HK43*P(0,6);
const float HK45 = HK26*HK35;
const float HK46 = HK45*P(0,24);
const float HK47 = HK45*P(0,9);
const float HK48 = 2*HK34;
const float HK49 = HK48*P(0,3);
const float HK50 = 2*HK27;
const float HK51 = HK50*P(0,0);
const float HK52 = 2*HK32;
const float HK53 = HK52*P(0,1);
const float HK54 = 2*HK33;
const float HK55 = HK54*P(0,2);
const float HK56 = HK39*P(4,6);
const float HK57 = HK41*P(5,6);
const float HK58 = HK43*P(6,6);
const float HK59 = HK45*P(6,9);
const float HK60 = HK45*P(6,24);
const float HK61 = HK48*P(3,6);
const float HK62 = HK50*P(0,6);
const float HK63 = HK52*P(1,6);
const float HK64 = HK54*P(2,6);
const float HK65 = HK39*P(4,5);
const float HK66 = HK41*P(5,5);
const float HK67 = HK43*P(5,6);
const float HK68 = HK45*P(5,9);
const float HK69 = HK45*P(5,24);
const float HK70 = HK48*P(3,5);
const float HK71 = HK50*P(0,5);
const float HK72 = HK52*P(1,5);
const float HK73 = HK54*P(2,5);
const float HK74 = HK39*P(4,4);
const float HK75 = HK41*P(4,5);
const float HK76 = HK43*P(4,6);
const float HK77 = HK45*P(4,9);
const float HK78 = HK45*P(4,24);
const float HK79 = HK48*P(3,4);
const float HK80 = HK50*P(0,4);
const float HK81 = HK52*P(1,4);
const float HK82 = HK54*P(2,4);
const float HK83 = HK39*P(4,24);
const float HK84 = HK41*P(5,24);
const float HK85 = HK43*P(6,24);
const float HK86 = HK45*P(9,24);
const float HK87 = HK45*P(24,24);
const float HK88 = HK48*P(3,24);
const float HK89 = HK50*P(0,24);
const float HK90 = HK52*P(1,24);
const float HK91 = HK54*P(2,24);
const float HK92 = HK37/powf(HK28, 3);
const float HK93 = HK39*P(4,9);
const float HK94 = HK41*P(5,9);
const float HK95 = HK43*P(6,9);
const float HK96 = HK45*P(9,9);
const float HK97 = -HK86;
const float HK98 = HK48*P(3,9);
const float HK99 = HK50*P(0,9);
const float HK100 = HK52*P(1,9);
const float HK101 = HK54*P(2,9);
const float HK102 = HK39*P(3,4);
const float HK103 = HK41*P(3,5);
const float HK104 = HK43*P(3,6);
const float HK105 = HK45*P(3,9);
const float HK106 = HK45*P(3,24);
const float HK107 = HK48*P(3,3);
const float HK108 = HK50*P(0,3);
const float HK109 = HK52*P(1,3);
const float HK110 = HK54*P(2,3);
const float HK111 = HK39*P(1,4);
const float HK112 = HK41*P(1,5);
const float HK113 = HK43*P(1,6);
const float HK114 = HK45*P(1,9);
const float HK115 = HK45*P(1,24);
const float HK116 = HK48*P(1,3);
const float HK117 = HK50*P(0,1);
const float HK118 = HK52*P(1,1);
const float HK119 = HK54*P(1,2);
const float HK120 = HK39*P(2,4);
const float HK121 = HK41*P(2,5);
const float HK122 = HK43*P(2,6);
const float HK123 = HK45*P(2,9);
const float HK124 = HK45*P(2,24);
const float HK125 = HK48*P(2,3);
const float HK126 = HK50*P(0,2);
const float HK127 = HK52*P(1,2);
const float HK128 = HK54*P(2,2);
const float HK129 = HK29/(HK36*HK39*(-HK74 - HK75 - HK76 + HK77 - HK78 - HK79 - HK80 - HK81 + HK82) + HK36*HK41*(-HK65 - HK66 - HK67 + HK68 - HK69 - HK70 - HK71 - HK72 + HK73) + HK36*HK43*(-HK56 - HK57 - HK58 + HK59 - HK60 - HK61 - HK62 - HK63 + HK64) + HK36*HK48*(-HK102 - HK103 - HK104 + HK105 - HK106 - HK107 - HK108 - HK109 + HK110) + HK36*HK50*(-HK40 - HK42 - HK44 - HK46 + HK47 - HK49 - HK51 - HK53 + HK55) + HK36*HK52*(-HK111 - HK112 - HK113 + HK114 - HK115 - HK116 - HK117 - HK118 + HK119) - HK36*HK54*(-HK120 - HK121 - HK122 + HK123 - HK124 - HK125 - HK126 - HK127 + HK128) - HK92*(-HK100 + HK101 - HK93 - HK94 - HK95 + HK96 + HK97 - HK98 - HK99) + HK92*(-HK83 - HK84 - HK85 + HK86 - HK87 - HK88 - HK89 - HK90 + HK91) - R_LOS);
// Observation Jacobians
Hfusion.at<0>() = HK3*HK5;
Hfusion.at<1>() = HK5*HK7;
Hfusion.at<2>() = HK5*HK8;
Hfusion.at<3>() = HK5*HK9;
Hfusion.at<4>() = HK22*HK4;
Hfusion.at<5>() = HK27*HK4;
Hfusion.at<6>() = HK28*HK4;
Hfusion.at<0>() = -HK27*HK30;
Hfusion.at<1>() = -HK30*HK32;
Hfusion.at<2>() = HK30*HK33;
Hfusion.at<3>() = -HK30*HK34;
Hfusion.at<4>() = -HK25*HK35;
Hfusion.at<5>() = -HK24*HK35;
Hfusion.at<6>() = -HK19*HK35;
Hfusion.at<7>() = 0;
Hfusion.at<8>() = 0;
Hfusion.at<9>() = 0;
Hfusion.at<9>() = HK38;
Hfusion.at<10>() = 0;
Hfusion.at<11>() = 0;
Hfusion.at<12>() = 0;
@ -69,113 +157,118 @@ Hfusion.at<20>() = 0;
Hfusion.at<21>() = 0;
Hfusion.at<22>() = 0;
Hfusion.at<23>() = 0;
Hfusion.at<24>() = 0;
Hfusion.at<24>() = -HK38;
// Kalman gains
Kfusion(0) = HK33*HK41;
Kfusion(1) = HK40*HK41;
Kfusion(2) = HK38*HK41;
Kfusion(3) = HK39*HK41;
Kfusion(4) = HK37*HK41;
Kfusion(5) = HK36*HK41;
Kfusion(6) = HK35*HK41;
Kfusion(7) = HK41*(HK22*P(4,7) + HK27*P(5,7) + HK28*P(6,7) + HK29*P(0,7) + HK30*P(1,7) + HK31*P(2,7) + HK32*P(3,7));
Kfusion(8) = HK41*(HK22*P(4,8) + HK27*P(5,8) + HK28*P(6,8) + HK29*P(0,8) + HK30*P(1,8) + HK31*P(2,8) + HK32*P(3,8));
Kfusion(9) = HK41*(HK22*P(4,9) + HK27*P(5,9) + HK28*P(6,9) + HK29*P(0,9) + HK30*P(1,9) + HK31*P(2,9) + HK32*P(3,9));
Kfusion(10) = HK41*(HK22*P(4,10) + HK27*P(5,10) + HK28*P(6,10) + HK29*P(0,10) + HK30*P(1,10) + HK31*P(2,10) + HK32*P(3,10));
Kfusion(11) = HK41*(HK22*P(4,11) + HK27*P(5,11) + HK28*P(6,11) + HK29*P(0,11) + HK30*P(1,11) + HK31*P(2,11) + HK32*P(3,11));
Kfusion(12) = HK41*(HK22*P(4,12) + HK27*P(5,12) + HK28*P(6,12) + HK29*P(0,12) + HK30*P(1,12) + HK31*P(2,12) + HK32*P(3,12));
Kfusion(13) = HK41*(HK22*P(4,13) + HK27*P(5,13) + HK28*P(6,13) + HK29*P(0,13) + HK30*P(1,13) + HK31*P(2,13) + HK32*P(3,13));
Kfusion(14) = HK41*(HK22*P(4,14) + HK27*P(5,14) + HK28*P(6,14) + HK29*P(0,14) + HK30*P(1,14) + HK31*P(2,14) + HK32*P(3,14));
Kfusion(15) = HK41*(HK22*P(4,15) + HK27*P(5,15) + HK28*P(6,15) + HK29*P(0,15) + HK30*P(1,15) + HK31*P(2,15) + HK32*P(3,15));
Kfusion(16) = HK41*(HK22*P(4,16) + HK27*P(5,16) + HK28*P(6,16) + HK29*P(0,16) + HK30*P(1,16) + HK31*P(2,16) + HK32*P(3,16));
Kfusion(17) = HK41*(HK22*P(4,17) + HK27*P(5,17) + HK28*P(6,17) + HK29*P(0,17) + HK30*P(1,17) + HK31*P(2,17) + HK32*P(3,17));
Kfusion(18) = HK41*(HK22*P(4,18) + HK27*P(5,18) + HK28*P(6,18) + HK29*P(0,18) + HK30*P(1,18) + HK31*P(2,18) + HK32*P(3,18));
Kfusion(19) = HK41*(HK22*P(4,19) + HK27*P(5,19) + HK28*P(6,19) + HK29*P(0,19) + HK30*P(1,19) + HK31*P(2,19) + HK32*P(3,19));
Kfusion(20) = HK41*(HK22*P(4,20) + HK27*P(5,20) + HK28*P(6,20) + HK29*P(0,20) + HK30*P(1,20) + HK31*P(2,20) + HK32*P(3,20));
Kfusion(21) = HK41*(HK22*P(4,21) + HK27*P(5,21) + HK28*P(6,21) + HK29*P(0,21) + HK30*P(1,21) + HK31*P(2,21) + HK32*P(3,21));
Kfusion(22) = HK41*(HK22*P(4,22) + HK27*P(5,22) + HK28*P(6,22) + HK29*P(0,22) + HK30*P(1,22) + HK31*P(2,22) + HK32*P(3,22));
Kfusion(23) = HK41*(HK22*P(4,23) + HK27*P(5,23) + HK28*P(6,23) + HK29*P(0,23) + HK30*P(1,23) + HK31*P(2,23) + HK32*P(3,23));
Kfusion(24) = HK41*(HK22*P(4,24) + HK27*P(5,24) + HK28*P(6,24) + HK29*P(0,24) + HK30*P(1,24) + HK31*P(2,24) + HK32*P(3,24));
Kfusion(0) = HK129*(HK40 + HK42 + HK44 + HK46 - HK47 + HK49 + HK51 + HK53 - HK55);
Kfusion(1) = HK129*(HK111 + HK112 + HK113 - HK114 + HK115 + HK116 + HK117 + HK118 - HK119);
Kfusion(2) = HK129*(HK120 + HK121 + HK122 - HK123 + HK124 + HK125 + HK126 + HK127 - HK128);
Kfusion(3) = HK129*(HK102 + HK103 + HK104 - HK105 + HK106 + HK107 + HK108 + HK109 - HK110);
Kfusion(4) = HK129*(HK74 + HK75 + HK76 - HK77 + HK78 + HK79 + HK80 + HK81 - HK82);
Kfusion(5) = HK129*(HK65 + HK66 + HK67 - HK68 + HK69 + HK70 + HK71 + HK72 - HK73);
Kfusion(6) = HK129*(HK56 + HK57 + HK58 - HK59 + HK60 + HK61 + HK62 + HK63 - HK64);
Kfusion(7) = HK129*(HK39*P(4,7) + HK41*P(5,7) + HK43*P(6,7) + HK45*P(7,24) - HK45*P(7,9) + HK48*P(3,7) + HK50*P(0,7) + HK52*P(1,7) - HK54*P(2,7));
Kfusion(8) = HK129*(HK39*P(4,8) + HK41*P(5,8) + HK43*P(6,8) + HK45*P(8,24) - HK45*P(8,9) + HK48*P(3,8) + HK50*P(0,8) + HK52*P(1,8) - HK54*P(2,8));
Kfusion(9) = HK129*(HK100 - HK101 + HK86 + HK93 + HK94 + HK95 - HK96 + HK98 + HK99);
Kfusion(10) = HK129*(HK39*P(4,10) + HK41*P(5,10) + HK43*P(6,10) + HK45*P(10,24) - HK45*P(9,10) + HK48*P(3,10) + HK50*P(0,10) + HK52*P(1,10) - HK54*P(2,10));
Kfusion(11) = HK129*(HK39*P(4,11) + HK41*P(5,11) + HK43*P(6,11) + HK45*P(11,24) - HK45*P(9,11) + HK48*P(3,11) + HK50*P(0,11) + HK52*P(1,11) - HK54*P(2,11));
Kfusion(12) = HK129*(HK39*P(4,12) + HK41*P(5,12) + HK43*P(6,12) + HK45*P(12,24) - HK45*P(9,12) + HK48*P(3,12) + HK50*P(0,12) + HK52*P(1,12) - HK54*P(2,12));
Kfusion(13) = HK129*(HK39*P(4,13) + HK41*P(5,13) + HK43*P(6,13) + HK45*P(13,24) - HK45*P(9,13) + HK48*P(3,13) + HK50*P(0,13) + HK52*P(1,13) - HK54*P(2,13));
Kfusion(14) = HK129*(HK39*P(4,14) + HK41*P(5,14) + HK43*P(6,14) + HK45*P(14,24) - HK45*P(9,14) + HK48*P(3,14) + HK50*P(0,14) + HK52*P(1,14) - HK54*P(2,14));
Kfusion(15) = HK129*(HK39*P(4,15) + HK41*P(5,15) + HK43*P(6,15) + HK45*P(15,24) - HK45*P(9,15) + HK48*P(3,15) + HK50*P(0,15) + HK52*P(1,15) - HK54*P(2,15));
Kfusion(16) = HK129*(HK39*P(4,16) + HK41*P(5,16) + HK43*P(6,16) + HK45*P(16,24) - HK45*P(9,16) + HK48*P(3,16) + HK50*P(0,16) + HK52*P(1,16) - HK54*P(2,16));
Kfusion(17) = HK129*(HK39*P(4,17) + HK41*P(5,17) + HK43*P(6,17) + HK45*P(17,24) - HK45*P(9,17) + HK48*P(3,17) + HK50*P(0,17) + HK52*P(1,17) - HK54*P(2,17));
Kfusion(18) = HK129*(HK39*P(4,18) + HK41*P(5,18) + HK43*P(6,18) + HK45*P(18,24) - HK45*P(9,18) + HK48*P(3,18) + HK50*P(0,18) + HK52*P(1,18) - HK54*P(2,18));
Kfusion(19) = HK129*(HK39*P(4,19) + HK41*P(5,19) + HK43*P(6,19) + HK45*P(19,24) - HK45*P(9,19) + HK48*P(3,19) + HK50*P(0,19) + HK52*P(1,19) - HK54*P(2,19));
Kfusion(20) = HK129*(HK39*P(4,20) + HK41*P(5,20) + HK43*P(6,20) + HK45*P(20,24) - HK45*P(9,20) + HK48*P(3,20) + HK50*P(0,20) + HK52*P(1,20) - HK54*P(2,20));
Kfusion(21) = HK129*(HK39*P(4,21) + HK41*P(5,21) + HK43*P(6,21) + HK45*P(21,24) - HK45*P(9,21) + HK48*P(3,21) + HK50*P(0,21) + HK52*P(1,21) - HK54*P(2,21));
Kfusion(22) = HK129*(HK39*P(4,22) + HK41*P(5,22) + HK43*P(6,22) + HK45*P(22,24) - HK45*P(9,22) + HK48*P(3,22) + HK50*P(0,22) + HK52*P(1,22) - HK54*P(2,22));
Kfusion(23) = HK129*(HK39*P(4,23) + HK41*P(5,23) + HK43*P(6,23) + HK45*P(23,24) - HK45*P(9,23) + HK48*P(3,23) + HK50*P(0,23) + HK52*P(1,23) - HK54*P(2,23));
Kfusion(24) = HK129*(HK83 + HK84 + HK85 + HK87 + HK88 + HK89 + HK90 - HK91 + HK97);
// Y Axis Equations
// Sub Expressions
const float HK0 = Tbs(0,1)*q1;
const float HK1 = Tbs(0,2)*q0;
const float HK2 = Tbs(0,0)*q2;
const float HK3 = HK0 + HK1 - HK2;
const float HK4 = Tbs(0,0)*q3;
const float HK5 = Tbs(0,1)*q0;
const float HK6 = Tbs(0,2)*q1;
const float HK7 = HK4 + HK5 - HK6;
const float HK8 = Tbs(0,0)*q0;
const float HK9 = Tbs(0,2)*q2;
const float HK10 = Tbs(0,1)*q3;
const float HK11 = -HK10 + HK8 + HK9;
const float HK12 = HK11*vn + HK3*vd + HK7*ve;
const float HK13 = 1.0F/range;
const float HK14 = 2*HK13;
const float HK15 = Tbs(0,0)*q1 + Tbs(0,1)*q2 + Tbs(0,2)*q3;
const float HK16 = HK15*vn + HK7*vd;
const float HK17 = HK16 - HK3*ve;
const float HK18 = HK15*ve + HK3*vn;
const float HK19 = -HK11*vd + HK18;
const float HK20 = HK11*ve + HK15*vd;
const float HK21 = HK20 - HK7*vn;
const float HK22 = q0*q3;
const float HK23 = q1*q2;
const float HK24 = 2*Tbs(0,1);
const float HK25 = powf(q1, 2);
const float HK26 = powf(q2, 2);
const float HK27 = -HK26;
const float HK28 = powf(q0, 2);
const float HK29 = powf(q3, 2);
const float HK30 = HK28 - HK29;
const float HK31 = q0*q2;
const float HK32 = q1*q3;
const float HK33 = 2*Tbs(0,2);
const float HK34 = HK33*(HK31 + HK32) + Tbs(0,0)*(HK25 + HK27 + HK30);
const float HK35 = -HK24*(HK22 - HK23) + HK34;
const float HK36 = q0*q1;
const float HK37 = q2*q3;
const float HK38 = -HK25;
const float HK39 = 2*Tbs(0,0);
const float HK40 = HK39*(HK22 + HK23) + Tbs(0,1)*(HK26 + HK30 + HK38);
const float HK41 = -HK33*(HK36 - HK37) + HK40;
const float HK42 = HK24*(HK36 + HK37) + Tbs(0,2)*(HK27 + HK28 + HK29 + HK38);
const float HK43 = -HK39*(HK31 - HK32) + HK42;
const float HK44 = 2*HK12;
const float HK45 = 2*HK16 + 2*ve*(-HK0 - HK1 + HK2);
const float HK46 = 2*HK18 + 2*vd*(HK10 - HK8 - HK9);
const float HK47 = 2*HK20 + 2*vn*(-HK4 - HK5 + HK6);
const float HK48 = HK24*(-HK22 + HK23) + HK34;
const float HK49 = HK33*(-HK36 + HK37) + HK40;
const float HK50 = HK39*(-HK31 + HK32) + HK42;
const float HK51 = HK44*P(0,0) + HK45*P(0,1) + HK46*P(0,2) + HK47*P(0,3) + HK48*P(0,4) + HK49*P(0,5) + HK50*P(0,6);
const float HK52 = powf(range, -2);
const float HK53 = HK44*P(0,6) + HK45*P(1,6) + HK46*P(2,6) + HK47*P(3,6) + HK48*P(4,6) + HK49*P(5,6) + HK50*P(6,6);
const float HK54 = HK44*P(0,5) + HK45*P(1,5) + HK46*P(2,5) + HK47*P(3,5) + HK48*P(4,5) + HK49*P(5,5) + HK50*P(5,6);
const float HK55 = HK44*P(0,4) + HK45*P(1,4) + HK46*P(2,4) + HK47*P(3,4) + HK48*P(4,4) + HK49*P(4,5) + HK50*P(4,6);
const float HK56 = HK44*P(0,2) + HK45*P(1,2) + HK46*P(2,2) + HK47*P(2,3) + HK48*P(2,4) + HK49*P(2,5) + HK50*P(2,6);
const float HK57 = 2*HK52;
const float HK58 = HK44*P(0,3) + HK45*P(1,3) + HK46*P(2,3) + HK47*P(3,3) + HK48*P(3,4) + HK49*P(3,5) + HK50*P(3,6);
const float HK59 = HK44*P(0,1) + HK45*P(1,1) + HK46*P(1,2) + HK47*P(1,3) + HK48*P(1,4) + HK49*P(1,5) + HK50*P(1,6);
const float HK60 = HK13/(HK17*HK57*HK59 + HK19*HK56*HK57 + HK21*HK57*HK58 + HK35*HK52*HK55 + HK41*HK52*HK54 + HK43*HK52*HK53 + HK44*HK51*HK52 + R_LOS);
const float HK0 = -Tbs(0,0)*q2 + Tbs(0,1)*q1 + Tbs(0,2)*q0;
const float HK1 = Tbs(0,0)*q3 + Tbs(0,1)*q0 - Tbs(0,2)*q1;
const float HK2 = Tbs(0,0)*q0;
const float HK3 = Tbs(0,2)*q2;
const float HK4 = Tbs(0,1)*q3;
const float HK5 = HK2 + HK3 - HK4;
const float HK6 = q0*q1;
const float HK7 = q2*q3;
const float HK8 = HK6 + HK7;
const float HK9 = q0*q2;
const float HK10 = q1*q3;
const float HK11 = -HK10 + HK9;
const float HK12 = powf(q3, 2);
const float HK13 = powf(q2, 2);
const float HK14 = -HK13;
const float HK15 = powf(q0, 2);
const float HK16 = powf(q1, 2);
const float HK17 = HK15 - HK16;
const float HK18 = HK12 + HK14 + HK17;
const float HK19 = -2*HK11*Tbs(2,0) + HK18*Tbs(2,2) + 2*HK8*Tbs(2,1);
const float HK20 = 2*Tbs(0,0);
const float HK21 = 2*Tbs(0,1);
const float HK22 = HK18*Tbs(0,2) + HK21*HK8;
const float HK23 = -HK11*HK20 + HK22;
const float HK24 = 2*Tbs(0,2);
const float HK25 = -HK12;
const float HK26 = q0*q3;
const float HK27 = q1*q2;
const float HK28 = HK20*(HK26 + HK27) + Tbs(0,1)*(HK13 + HK17 + HK25);
const float HK29 = -HK24*(HK6 - HK7) + HK28;
const float HK30 = HK24*(HK10 + HK9) + Tbs(0,0)*(HK14 + HK15 + HK16 + HK25);
const float HK31 = -HK21*(HK26 - HK27) + HK30;
const float HK32 = HK23*vd + HK29*ve + HK31*vn;
const float HK33 = HK19*(HK0*vd + HK1*ve + HK5*vn) + HK32*(-Tbs(2,0)*q2 + Tbs(2,1)*q1 + Tbs(2,2)*q0);
const float HK34 = pd - ptd;
const float HK35 = 1.0F/HK34;
const float HK36 = 2*HK35;
const float HK37 = Tbs(0,0)*q1 + Tbs(0,1)*q2 + Tbs(0,2)*q3;
const float HK38 = HK19*(-HK0*ve + HK1*vd + HK37*vn) + HK32*(Tbs(2,0)*q3 + Tbs(2,1)*q0 - Tbs(2,2)*q1);
const float HK39 = HK19*(HK0*vn + HK37*ve + vd*(-HK2 - HK3 + HK4));
const float HK40 = (Tbs(2,0)*q0 - Tbs(2,1)*q3 + Tbs(2,2)*q2)*(vd*(HK20*(HK10 - HK9) + HK22) + ve*(HK24*(-HK6 + HK7) + HK28) + vn*(HK21*(-HK26 + HK27) + HK30));
const float HK41 = HK19*(-HK1*vn + HK37*vd + HK5*ve) + HK32*(Tbs(2,0)*q1 + Tbs(2,1)*q2 + Tbs(2,2)*q3);
const float HK42 = HK19*HK35;
const float HK43 = powf(HK34, -2);
const float HK44 = HK19*HK32;
const float HK45 = HK43*HK44;
const float HK46 = HK19*HK31;
const float HK47 = HK19*HK29;
const float HK48 = HK19*HK23;
const float HK49 = HK32*HK42;
const float HK50 = 2*HK41;
const float HK51 = 2*HK33;
const float HK52 = 2*HK38;
const float HK53 = -2*HK39 + 2*HK40;
const float HK54 = HK46*P(0,4) + HK47*P(0,5) + HK48*P(0,6) + HK49*P(0,24) - HK49*P(0,9) + HK50*P(0,3) + HK51*P(0,0) + HK52*P(0,1) - HK53*P(0,2);
const float HK55 = HK46*P(4,6) + HK47*P(5,6) + HK48*P(6,6) + HK49*P(6,24) - HK49*P(6,9) + HK50*P(3,6) + HK51*P(0,6) + HK52*P(1,6) - HK53*P(2,6);
const float HK56 = HK46*P(4,5) + HK47*P(5,5) + HK48*P(5,6) + HK49*P(5,24) - HK49*P(5,9) + HK50*P(3,5) + HK51*P(0,5) + HK52*P(1,5) - HK53*P(2,5);
const float HK57 = HK46*P(4,4) + HK47*P(4,5) + HK48*P(4,6) + HK49*P(4,24) - HK49*P(4,9) + HK50*P(3,4) + HK51*P(0,4) + HK52*P(1,4) - HK53*P(2,4);
const float HK58 = HK49*P(9,24);
const float HK59 = HK46*P(4,24) + HK47*P(5,24) + HK48*P(6,24) + HK49*P(24,24) + HK50*P(3,24) + HK51*P(0,24) + HK52*P(1,24) - HK53*P(2,24) - HK58;
const float HK60 = HK44/powf(HK34, 3);
const float HK61 = HK46*P(4,9) + HK47*P(5,9) + HK48*P(6,9) - HK49*P(9,9) + HK50*P(3,9) + HK51*P(0,9) + HK52*P(1,9) - HK53*P(2,9) + HK58;
const float HK62 = HK46*P(3,4) + HK47*P(3,5) + HK48*P(3,6) + HK49*P(3,24) - HK49*P(3,9) + HK50*P(3,3) + HK51*P(0,3) + HK52*P(1,3) - HK53*P(2,3);
const float HK63 = HK46*P(1,4) + HK47*P(1,5) + HK48*P(1,6) + HK49*P(1,24) - HK49*P(1,9) + HK50*P(1,3) + HK51*P(0,1) + HK52*P(1,1) - HK53*P(1,2);
const float HK64 = HK46*P(2,4) + HK47*P(2,5) + HK48*P(2,6) + HK49*P(2,24) - HK49*P(2,9) + HK50*P(2,3) + HK51*P(0,2) + HK52*P(1,2) - HK53*P(2,2);
const float HK65 = HK35/(HK43*HK46*HK57 + HK43*HK47*HK56 + HK43*HK48*HK55 + HK43*HK50*HK62 + HK43*HK51*HK54 + HK43*HK52*HK63 - HK43*HK53*HK64 + HK59*HK60 - HK60*HK61 + R_LOS);
// Observation Jacobians
Hfusion.at<0>() = -HK12*HK14;
Hfusion.at<1>() = -HK14*HK17;
Hfusion.at<2>() = -HK14*HK19;
Hfusion.at<3>() = -HK14*HK21;
Hfusion.at<4>() = -HK13*HK35;
Hfusion.at<5>() = -HK13*HK41;
Hfusion.at<6>() = -HK13*HK43;
Hfusion.at<0>() = HK33*HK36;
Hfusion.at<1>() = HK36*HK38;
Hfusion.at<2>() = HK36*(HK39 - HK40);
Hfusion.at<3>() = HK36*HK41;
Hfusion.at<4>() = HK31*HK42;
Hfusion.at<5>() = HK29*HK42;
Hfusion.at<6>() = HK23*HK42;
Hfusion.at<7>() = 0;
Hfusion.at<8>() = 0;
Hfusion.at<9>() = 0;
Hfusion.at<9>() = -HK45;
Hfusion.at<10>() = 0;
Hfusion.at<11>() = 0;
Hfusion.at<12>() = 0;
@ -190,32 +283,32 @@ Hfusion.at<20>() = 0;
Hfusion.at<21>() = 0;
Hfusion.at<22>() = 0;
Hfusion.at<23>() = 0;
Hfusion.at<24>() = 0;
Hfusion.at<24>() = HK45;
// Kalman gains
Kfusion(0) = -HK51*HK60;
Kfusion(1) = -HK59*HK60;
Kfusion(2) = -HK56*HK60;
Kfusion(3) = -HK58*HK60;
Kfusion(4) = -HK55*HK60;
Kfusion(5) = -HK54*HK60;
Kfusion(6) = -HK53*HK60;
Kfusion(7) = -HK60*(HK44*P(0,7) + HK45*P(1,7) + HK46*P(2,7) + HK47*P(3,7) + HK48*P(4,7) + HK49*P(5,7) + HK50*P(6,7));
Kfusion(8) = -HK60*(HK44*P(0,8) + HK45*P(1,8) + HK46*P(2,8) + HK47*P(3,8) + HK48*P(4,8) + HK49*P(5,8) + HK50*P(6,8));
Kfusion(9) = -HK60*(HK44*P(0,9) + HK45*P(1,9) + HK46*P(2,9) + HK47*P(3,9) + HK48*P(4,9) + HK49*P(5,9) + HK50*P(6,9));
Kfusion(10) = -HK60*(HK44*P(0,10) + HK45*P(1,10) + HK46*P(2,10) + HK47*P(3,10) + HK48*P(4,10) + HK49*P(5,10) + HK50*P(6,10));
Kfusion(11) = -HK60*(HK44*P(0,11) + HK45*P(1,11) + HK46*P(2,11) + HK47*P(3,11) + HK48*P(4,11) + HK49*P(5,11) + HK50*P(6,11));
Kfusion(12) = -HK60*(HK44*P(0,12) + HK45*P(1,12) + HK46*P(2,12) + HK47*P(3,12) + HK48*P(4,12) + HK49*P(5,12) + HK50*P(6,12));
Kfusion(13) = -HK60*(HK44*P(0,13) + HK45*P(1,13) + HK46*P(2,13) + HK47*P(3,13) + HK48*P(4,13) + HK49*P(5,13) + HK50*P(6,13));
Kfusion(14) = -HK60*(HK44*P(0,14) + HK45*P(1,14) + HK46*P(2,14) + HK47*P(3,14) + HK48*P(4,14) + HK49*P(5,14) + HK50*P(6,14));
Kfusion(15) = -HK60*(HK44*P(0,15) + HK45*P(1,15) + HK46*P(2,15) + HK47*P(3,15) + HK48*P(4,15) + HK49*P(5,15) + HK50*P(6,15));
Kfusion(16) = -HK60*(HK44*P(0,16) + HK45*P(1,16) + HK46*P(2,16) + HK47*P(3,16) + HK48*P(4,16) + HK49*P(5,16) + HK50*P(6,16));
Kfusion(17) = -HK60*(HK44*P(0,17) + HK45*P(1,17) + HK46*P(2,17) + HK47*P(3,17) + HK48*P(4,17) + HK49*P(5,17) + HK50*P(6,17));
Kfusion(18) = -HK60*(HK44*P(0,18) + HK45*P(1,18) + HK46*P(2,18) + HK47*P(3,18) + HK48*P(4,18) + HK49*P(5,18) + HK50*P(6,18));
Kfusion(19) = -HK60*(HK44*P(0,19) + HK45*P(1,19) + HK46*P(2,19) + HK47*P(3,19) + HK48*P(4,19) + HK49*P(5,19) + HK50*P(6,19));
Kfusion(20) = -HK60*(HK44*P(0,20) + HK45*P(1,20) + HK46*P(2,20) + HK47*P(3,20) + HK48*P(4,20) + HK49*P(5,20) + HK50*P(6,20));
Kfusion(21) = -HK60*(HK44*P(0,21) + HK45*P(1,21) + HK46*P(2,21) + HK47*P(3,21) + HK48*P(4,21) + HK49*P(5,21) + HK50*P(6,21));
Kfusion(22) = -HK60*(HK44*P(0,22) + HK45*P(1,22) + HK46*P(2,22) + HK47*P(3,22) + HK48*P(4,22) + HK49*P(5,22) + HK50*P(6,22));
Kfusion(23) = -HK60*(HK44*P(0,23) + HK45*P(1,23) + HK46*P(2,23) + HK47*P(3,23) + HK48*P(4,23) + HK49*P(5,23) + HK50*P(6,23));
Kfusion(24) = -HK60*(HK44*P(0,24) + HK45*P(1,24) + HK46*P(2,24) + HK47*P(3,24) + HK48*P(4,24) + HK49*P(5,24) + HK50*P(6,24));
Kfusion(0) = HK54*HK65;
Kfusion(1) = HK63*HK65;
Kfusion(2) = HK64*HK65;
Kfusion(3) = HK62*HK65;
Kfusion(4) = HK57*HK65;
Kfusion(5) = HK56*HK65;
Kfusion(6) = HK55*HK65;
Kfusion(7) = HK65*(HK46*P(4,7) + HK47*P(5,7) + HK48*P(6,7) + HK49*P(7,24) - HK49*P(7,9) + HK50*P(3,7) + HK51*P(0,7) + HK52*P(1,7) - HK53*P(2,7));
Kfusion(8) = HK65*(HK46*P(4,8) + HK47*P(5,8) + HK48*P(6,8) + HK49*P(8,24) - HK49*P(8,9) + HK50*P(3,8) + HK51*P(0,8) + HK52*P(1,8) - HK53*P(2,8));
Kfusion(9) = HK61*HK65;
Kfusion(10) = HK65*(HK46*P(4,10) + HK47*P(5,10) + HK48*P(6,10) + HK49*P(10,24) - HK49*P(9,10) + HK50*P(3,10) + HK51*P(0,10) + HK52*P(1,10) - HK53*P(2,10));
Kfusion(11) = HK65*(HK46*P(4,11) + HK47*P(5,11) + HK48*P(6,11) + HK49*P(11,24) - HK49*P(9,11) + HK50*P(3,11) + HK51*P(0,11) + HK52*P(1,11) - HK53*P(2,11));
Kfusion(12) = HK65*(HK46*P(4,12) + HK47*P(5,12) + HK48*P(6,12) + HK49*P(12,24) - HK49*P(9,12) + HK50*P(3,12) + HK51*P(0,12) + HK52*P(1,12) - HK53*P(2,12));
Kfusion(13) = HK65*(HK46*P(4,13) + HK47*P(5,13) + HK48*P(6,13) + HK49*P(13,24) - HK49*P(9,13) + HK50*P(3,13) + HK51*P(0,13) + HK52*P(1,13) - HK53*P(2,13));
Kfusion(14) = HK65*(HK46*P(4,14) + HK47*P(5,14) + HK48*P(6,14) + HK49*P(14,24) - HK49*P(9,14) + HK50*P(3,14) + HK51*P(0,14) + HK52*P(1,14) - HK53*P(2,14));
Kfusion(15) = HK65*(HK46*P(4,15) + HK47*P(5,15) + HK48*P(6,15) + HK49*P(15,24) - HK49*P(9,15) + HK50*P(3,15) + HK51*P(0,15) + HK52*P(1,15) - HK53*P(2,15));
Kfusion(16) = HK65*(HK46*P(4,16) + HK47*P(5,16) + HK48*P(6,16) + HK49*P(16,24) - HK49*P(9,16) + HK50*P(3,16) + HK51*P(0,16) + HK52*P(1,16) - HK53*P(2,16));
Kfusion(17) = HK65*(HK46*P(4,17) + HK47*P(5,17) + HK48*P(6,17) + HK49*P(17,24) - HK49*P(9,17) + HK50*P(3,17) + HK51*P(0,17) + HK52*P(1,17) - HK53*P(2,17));
Kfusion(18) = HK65*(HK46*P(4,18) + HK47*P(5,18) + HK48*P(6,18) + HK49*P(18,24) - HK49*P(9,18) + HK50*P(3,18) + HK51*P(0,18) + HK52*P(1,18) - HK53*P(2,18));
Kfusion(19) = HK65*(HK46*P(4,19) + HK47*P(5,19) + HK48*P(6,19) + HK49*P(19,24) - HK49*P(9,19) + HK50*P(3,19) + HK51*P(0,19) + HK52*P(1,19) - HK53*P(2,19));
Kfusion(20) = HK65*(HK46*P(4,20) + HK47*P(5,20) + HK48*P(6,20) + HK49*P(20,24) - HK49*P(9,20) + HK50*P(3,20) + HK51*P(0,20) + HK52*P(1,20) - HK53*P(2,20));
Kfusion(21) = HK65*(HK46*P(4,21) + HK47*P(5,21) + HK48*P(6,21) + HK49*P(21,24) - HK49*P(9,21) + HK50*P(3,21) + HK51*P(0,21) + HK52*P(1,21) - HK53*P(2,21));
Kfusion(22) = HK65*(HK46*P(4,22) + HK47*P(5,22) + HK48*P(6,22) + HK49*P(22,24) - HK49*P(9,22) + HK50*P(3,22) + HK51*P(0,22) + HK52*P(1,22) - HK53*P(2,22));
Kfusion(23) = HK65*(HK46*P(4,23) + HK47*P(5,23) + HK48*P(6,23) + HK49*P(23,24) - HK49*P(9,23) + HK50*P(3,23) + HK51*P(0,23) + HK52*P(1,23) - HK53*P(2,23));
Kfusion(24) = HK59*HK65;

410
src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated_alt.cpp
View File

@ -2,112 +2,202 @@
const float HK0 = -Tbs(1,0)*q2 + Tbs(1,1)*q1 + Tbs(1,2)*q0;
const float HK1 = Tbs(1,0)*q3 + Tbs(1,1)*q0 - Tbs(1,2)*q1;
const float HK2 = Tbs(1,0)*q0 - Tbs(1,1)*q3 + Tbs(1,2)*q2;
const float HK3 = HK0*vd + HK1*ve + HK2*vn;
const float HK4 = 1.0F/range;
const float HK5 = 2*HK4;
const float HK6 = Tbs(1,0)*q1 + Tbs(1,1)*q2 + Tbs(1,2)*q3;
const float HK7 = -HK0*ve + HK1*vd + HK6*vn;
const float HK8 = HK0*vn - HK2*vd + HK6*ve;
const float HK9 = -HK1*vn + HK2*ve + HK6*vd;
const float HK10 = q0*q2;
const float HK11 = q1*q3;
const float HK12 = HK10 + HK11;
const float HK13 = 2*Tbs(1,2);
const float HK14 = q0*q3;
const float HK15 = q1*q2;
const float HK3 = q0*q1;
const float HK4 = q2*q3;
const float HK5 = HK3 + HK4;
const float HK6 = 2*HK5;
const float HK7 = q0*q2;
const float HK8 = q1*q3;
const float HK9 = HK7 - HK8;
const float HK10 = 2*HK9;
const float HK11 = powf(q3, 2);
const float HK12 = powf(q2, 2);
const float HK13 = -HK12;
const float HK14 = powf(q0, 2);
const float HK15 = powf(q1, 2);
const float HK16 = HK14 - HK15;
const float HK17 = 2*Tbs(1,1);
const float HK18 = powf(q1, 2);
const float HK19 = powf(q2, 2);
const float HK20 = -HK19;
const float HK21 = powf(q0, 2);
const float HK22 = powf(q3, 2);
const float HK23 = HK21 - HK22;
const float HK24 = HK18 + HK20 + HK23;
const float HK25 = HK12*HK13 - HK16*HK17 + HK24*Tbs(1,0);
const float HK26 = HK14 + HK15;
const float HK27 = 2*Tbs(1,0);
const float HK28 = q0*q1;
const float HK29 = q2*q3;
const float HK30 = HK28 - HK29;
const float HK31 = -HK18;
const float HK32 = HK19 + HK23 + HK31;
const float HK33 = -HK13*HK30 + HK26*HK27 + HK32*Tbs(1,1);
const float HK34 = HK28 + HK29;
const float HK35 = HK10 - HK11;
const float HK36 = HK20 + HK21 + HK22 + HK31;
const float HK37 = HK17*HK34 - HK27*HK35 + HK36*Tbs(1,2);
const float HK38 = 2*HK3;
const float HK39 = 2*HK7;
const float HK40 = 2*HK8;
const float HK41 = 2*HK9;
const float HK42 = HK25*P(0,4) + HK33*P(0,5) + HK37*P(0,6) + HK38*P(0,0) + HK39*P(0,1) + HK40*P(0,2) + HK41*P(0,3);
const float HK43 = powf(range, -2);
const float HK44 = HK25*P(4,6) + HK33*P(5,6) + HK37*P(6,6) + HK38*P(0,6) + HK39*P(1,6) + HK40*P(2,6) + HK41*P(3,6);
const float HK45 = HK25*P(4,5) + HK33*P(5,5) + HK37*P(5,6) + HK38*P(0,5) + HK39*P(1,5) + HK40*P(2,5) + HK41*P(3,5);
const float HK46 = HK25*P(4,4) + HK33*P(4,5) + HK37*P(4,6) + HK38*P(0,4) + HK39*P(1,4) + HK40*P(2,4) + HK41*P(3,4);
const float HK47 = HK25*P(2,4) + HK33*P(2,5) + HK37*P(2,6) + HK38*P(0,2) + HK39*P(1,2) + HK40*P(2,2) + HK41*P(2,3);
const float HK48 = HK25*P(3,4) + HK33*P(3,5) + HK37*P(3,6) + HK38*P(0,3) + HK39*P(1,3) + HK40*P(2,3) + HK41*P(3,3);
const float HK49 = HK25*P(1,4) + HK33*P(1,5) + HK37*P(1,6) + HK38*P(0,1) + HK39*P(1,1) + HK40*P(1,2) + HK41*P(1,3);
const float HK50 = HK4/(HK25*HK43*HK46 + HK33*HK43*HK45 + HK37*HK43*HK44 + HK38*HK42*HK43 + HK39*HK43*HK49 + HK40*HK43*HK47 + HK41*HK43*HK48 + R_LOS);
const float HK51 = Tbs(0,1)*q1;
const float HK52 = Tbs(0,2)*q0;
const float HK53 = Tbs(0,0)*q2;
const float HK54 = HK51 + HK52 - HK53;
const float HK55 = Tbs(0,0)*q3;
const float HK56 = Tbs(0,1)*q0;
const float HK57 = Tbs(0,2)*q1;
const float HK58 = HK55 + HK56 - HK57;
const float HK59 = Tbs(0,0)*q0;
const float HK60 = Tbs(0,2)*q2;
const float HK61 = Tbs(0,1)*q3;
const float HK62 = HK59 + HK60 - HK61;
const float HK63 = HK54*vd + HK58*ve + HK62*vn;
const float HK64 = Tbs(0,0)*q1 + Tbs(0,1)*q2 + Tbs(0,2)*q3;
const float HK65 = HK58*vd + HK64*vn;
const float HK66 = -HK54*ve + HK65;
const float HK67 = HK54*vn + HK64*ve;
const float HK68 = -HK62*vd + HK67;
const float HK69 = HK62*ve + HK64*vd;
const float HK70 = -HK58*vn + HK69;
const float HK71 = 2*Tbs(0,1);
const float HK72 = 2*Tbs(0,2);
const float HK73 = HK12*HK72 + HK24*Tbs(0,0);
const float HK74 = -HK16*HK71 + HK73;
const float HK75 = 2*Tbs(0,0);
const float HK76 = HK26*HK75 + HK32*Tbs(0,1);
const float HK77 = -HK30*HK72 + HK76;
const float HK78 = HK34*HK71 + HK36*Tbs(0,2);
const float HK79 = -HK35*HK75 + HK78;
const float HK80 = 2*HK63;
const float HK81 = 2*HK65 + 2*ve*(-HK51 - HK52 + HK53);
const float HK82 = 2*HK67 + 2*vd*(-HK59 - HK60 + HK61);
const float HK83 = 2*HK69 + 2*vn*(-HK55 - HK56 + HK57);
const float HK84 = HK71*(-HK14 + HK15) + HK73;
const float HK85 = HK72*(-HK28 + HK29) + HK76;
const float HK86 = HK75*(-HK10 + HK11) + HK78;
const float HK87 = HK80*P(0,0) + HK81*P(0,1) + HK82*P(0,2) + HK83*P(0,3) + HK84*P(0,4) + HK85*P(0,5) + HK86*P(0,6);
const float HK88 = HK80*P(0,6) + HK81*P(1,6) + HK82*P(2,6) + HK83*P(3,6) + HK84*P(4,6) + HK85*P(5,6) + HK86*P(6,6);
const float HK89 = HK80*P(0,5) + HK81*P(1,5) + HK82*P(2,5) + HK83*P(3,5) + HK84*P(4,5) + HK85*P(5,5) + HK86*P(5,6);
const float HK90 = HK80*P(0,4) + HK81*P(1,4) + HK82*P(2,4) + HK83*P(3,4) + HK84*P(4,4) + HK85*P(4,5) + HK86*P(4,6);
const float HK91 = HK80*P(0,2) + HK81*P(1,2) + HK82*P(2,2) + HK83*P(2,3) + HK84*P(2,4) + HK85*P(2,5) + HK86*P(2,6);
const float HK92 = 2*HK43;
const float HK93 = HK80*P(0,3) + HK81*P(1,3) + HK82*P(2,3) + HK83*P(3,3) + HK84*P(3,4) + HK85*P(3,5) + HK86*P(3,6);
const float HK94 = HK80*P(0,1) + HK81*P(1,1) + HK82*P(1,2) + HK83*P(1,3) + HK84*P(1,4) + HK85*P(1,5) + HK86*P(1,6);
const float HK95 = HK4/(HK43*HK74*HK90 + HK43*HK77*HK89 + HK43*HK79*HK88 + HK43*HK80*HK87 + HK66*HK92*HK94 + HK68*HK91*HK92 + HK70*HK92*HK93 + R_LOS);
const float HK17 = HK11 + HK13 + HK16;
const float HK18 = -HK10*Tbs(2,0) + HK17*Tbs(2,2) + HK6*Tbs(2,1);
const float HK19 = -Tbs(2,0)*q2 + Tbs(2,1)*q1 + Tbs(2,2)*q0;
const float HK20 = 2*Tbs(1,1);
const float HK21 = 2*Tbs(1,0);
const float HK22 = HK17*Tbs(1,2) + HK20*HK5 - HK21*HK9;
const float HK23 = q0*q3;
const float HK24 = q1*q2;
const float HK25 = HK23 + HK24;
const float HK26 = HK3 - HK4;
const float HK27 = 2*Tbs(1,2);
const float HK28 = -HK11;
const float HK29 = HK12 + HK16 + HK28;
const float HK30 = HK21*HK25 - HK26*HK27 + HK29*Tbs(1,1);
const float HK31 = HK7 + HK8;
const float HK32 = HK23 - HK24;
const float HK33 = HK13 + HK14 + HK15 + HK28;
const float HK34 = -HK20*HK32 + HK27*HK31 + HK33*Tbs(1,0);
const float HK35 = HK22*vd + HK30*ve + HK34*vn;
const float HK36 = HK18*(HK0*vd + HK1*ve + HK2*vn) + HK19*HK35;
const float HK37 = pd - ptd;
const float HK38 = 1.0F/HK37;
const float HK39 = 2*HK38;
const float HK40 = Tbs(1,0)*q1 + Tbs(1,1)*q2 + Tbs(1,2)*q3;
const float HK41 = Tbs(2,0)*q3 + Tbs(2,1)*q0 - Tbs(2,2)*q1;
const float HK42 = HK18*(-HK0*ve + HK1*vd + HK40*vn) + HK35*HK41;
const float HK43 = Tbs(2,0)*q0 - Tbs(2,1)*q3 + Tbs(2,2)*q2;
const float HK44 = -HK18*(HK0*vn - HK2*vd + HK40*ve) + HK35*HK43;
const float HK45 = Tbs(2,0)*q1 + Tbs(2,1)*q2 + Tbs(2,2)*q3;
const float HK46 = HK18*(-HK1*vn + HK2*ve + HK40*vd) + HK35*HK45;
const float HK47 = HK18*HK38;
const float HK48 = powf(HK37, -2);
const float HK49 = HK18*HK48;
const float HK50 = HK35*HK49;
const float HK51 = HK18*HK34;
const float HK52 = HK51*P(0,4);
const float HK53 = HK18*HK30;
const float HK54 = HK53*P(0,5);
const float HK55 = HK18*HK22;
const float HK56 = HK55*P(0,6);
const float HK57 = HK35*HK47;
const float HK58 = HK57*P(0,24);
const float HK59 = HK57*P(0,9);
const float HK60 = 2*HK46;
const float HK61 = HK60*P(0,3);
const float HK62 = 2*HK36;
const float HK63 = HK62*P(0,0);
const float HK64 = 2*HK42;
const float HK65 = HK64*P(0,1);
const float HK66 = 2*HK44;
const float HK67 = HK66*P(0,2);
const float HK68 = HK51*P(4,6);
const float HK69 = HK53*P(5,6);
const float HK70 = HK55*P(6,6);
const float HK71 = HK57*P(6,9);
const float HK72 = HK57*P(6,24);
const float HK73 = HK60*P(3,6);
const float HK74 = HK62*P(0,6);
const float HK75 = HK64*P(1,6);
const float HK76 = HK66*P(2,6);
const float HK77 = HK51*P(4,5);
const float HK78 = HK53*P(5,5);
const float HK79 = HK55*P(5,6);
const float HK80 = HK57*P(5,9);
const float HK81 = HK57*P(5,24);
const float HK82 = HK60*P(3,5);
const float HK83 = HK62*P(0,5);
const float HK84 = HK64*P(1,5);
const float HK85 = HK66*P(2,5);
const float HK86 = HK51*P(4,4);
const float HK87 = HK53*P(4,5);
const float HK88 = HK55*P(4,6);
const float HK89 = HK57*P(4,9);
const float HK90 = HK57*P(4,24);
const float HK91 = HK60*P(3,4);
const float HK92 = HK62*P(0,4);
const float HK93 = HK64*P(1,4);
const float HK94 = HK66*P(2,4);
const float HK95 = HK51*P(4,24);
const float HK96 = HK53*P(5,24);
const float HK97 = HK55*P(6,24);
const float HK98 = HK57*P(9,24);
const float HK99 = HK57*P(24,24);
const float HK100 = HK60*P(3,24);
const float HK101 = HK62*P(0,24);
const float HK102 = HK64*P(1,24);
const float HK103 = HK66*P(2,24);
const float HK104 = HK18/powf(HK37, 3);
const float HK105 = HK104*HK35;
const float HK106 = HK51*P(4,9);
const float HK107 = HK53*P(5,9);
const float HK108 = HK55*P(6,9);
const float HK109 = HK57*P(9,9);
const float HK110 = -HK98;
const float HK111 = HK60*P(3,9);
const float HK112 = HK62*P(0,9);
const float HK113 = HK64*P(1,9);
const float HK114 = HK66*P(2,9);
const float HK115 = HK51*P(3,4);
const float HK116 = HK53*P(3,5);
const float HK117 = HK55*P(3,6);
const float HK118 = HK57*P(3,9);
const float HK119 = HK57*P(3,24);
const float HK120 = HK60*P(3,3);
const float HK121 = HK62*P(0,3);
const float HK122 = HK64*P(1,3);
const float HK123 = HK66*P(2,3);
const float HK124 = HK51*P(1,4);
const float HK125 = HK53*P(1,5);
const float HK126 = HK55*P(1,6);
const float HK127 = HK57*P(1,9);
const float HK128 = HK57*P(1,24);
const float HK129 = HK60*P(1,3);
const float HK130 = HK62*P(0,1);
const float HK131 = HK64*P(1,1);
const float HK132 = HK66*P(1,2);
const float HK133 = HK51*P(2,4);
const float HK134 = HK53*P(2,5);
const float HK135 = HK55*P(2,6);
const float HK136 = HK57*P(2,9);
const float HK137 = HK57*P(2,24);
const float HK138 = HK60*P(2,3);
const float HK139 = HK62*P(0,2);
const float HK140 = HK64*P(1,2);
const float HK141 = HK66*P(2,2);
const float HK142 = HK38/(HK105*(-HK100 - HK101 - HK102 + HK103 - HK95 - HK96 - HK97 + HK98 - HK99) - HK105*(-HK106 - HK107 - HK108 + HK109 + HK110 - HK111 - HK112 - HK113 + HK114) + HK22*HK49*(-HK68 - HK69 - HK70 + HK71 - HK72 - HK73 - HK74 - HK75 + HK76) + HK30*HK49*(-HK77 - HK78 - HK79 + HK80 - HK81 - HK82 - HK83 - HK84 + HK85) + HK34*HK49*(-HK86 - HK87 - HK88 + HK89 - HK90 - HK91 - HK92 - HK93 + HK94) + HK48*HK60*(-HK115 - HK116 - HK117 + HK118 - HK119 - HK120 - HK121 - HK122 + HK123) + HK48*HK62*(-HK52 - HK54 - HK56 - HK58 + HK59 - HK61 - HK63 - HK65 + HK67) + HK48*HK64*(-HK124 - HK125 - HK126 + HK127 - HK128 - HK129 - HK130 - HK131 + HK132) - HK48*HK66*(-HK133 - HK134 - HK135 + HK136 - HK137 - HK138 - HK139 - HK140 + HK141) - R_LOS);
const float HK143 = -Tbs(0,0)*q2 + Tbs(0,1)*q1 + Tbs(0,2)*q0;
const float HK144 = Tbs(0,0)*q3 + Tbs(0,1)*q0 - Tbs(0,2)*q1;
const float HK145 = Tbs(0,0)*q0;
const float HK146 = Tbs(0,2)*q2;
const float HK147 = Tbs(0,1)*q3;
const float HK148 = HK145 + HK146 - HK147;
const float HK149 = HK17*Tbs(0,2) + HK6*Tbs(0,1);
const float HK150 = -HK10*Tbs(0,0) + HK149;
const float HK151 = 2*Tbs(0,2);
const float HK152 = 2*Tbs(0,0);
const float HK153 = HK152*HK25 + HK29*Tbs(0,1);
const float HK154 = -HK151*HK26 + HK153;
const float HK155 = 2*Tbs(0,1);
const float HK156 = HK151*HK31 + HK33*Tbs(0,0);
const float HK157 = -HK155*HK32 + HK156;
const float HK158 = HK150*vd + HK154*ve + HK157*vn;
const float HK159 = HK158*HK19 + HK18*(HK143*vd + HK144*ve + HK148*vn);
const float HK160 = Tbs(0,0)*q1 + Tbs(0,1)*q2 + Tbs(0,2)*q3;
const float HK161 = HK158*HK41 + HK18*(-HK143*ve + HK144*vd + HK160*vn);
const float HK162 = HK18*(HK143*vn + HK160*ve + vd*(-HK145 - HK146 + HK147));
const float HK163 = HK43*(vd*(HK149 + HK152*(-HK7 + HK8)) + ve*(HK151*(-HK3 + HK4) + HK153) + vn*(HK155*(-HK23 + HK24) + HK156));
const float HK164 = HK158*HK45 + HK18*(-HK144*vn + HK148*ve + HK160*vd);
const float HK165 = HK158*HK49;
const float HK166 = HK157*HK18;
const float HK167 = HK154*HK18;
const float HK168 = HK150*HK18;
const float HK169 = HK158*HK47;
const float HK170 = 2*HK164;
const float HK171 = 2*HK159;
const float HK172 = 2*HK161;
const float HK173 = -2*HK162 + 2*HK163;
const float HK174 = HK166*P(0,4) + HK167*P(0,5) + HK168*P(0,6) + HK169*P(0,24) - HK169*P(0,9) + HK170*P(0,3) + HK171*P(0,0) + HK172*P(0,1) - HK173*P(0,2);
const float HK175 = HK166*P(4,6) + HK167*P(5,6) + HK168*P(6,6) + HK169*P(6,24) - HK169*P(6,9) + HK170*P(3,6) + HK171*P(0,6) + HK172*P(1,6) - HK173*P(2,6);
const float HK176 = HK166*P(4,5) + HK167*P(5,5) + HK168*P(5,6) + HK169*P(5,24) - HK169*P(5,9) + HK170*P(3,5) + HK171*P(0,5) + HK172*P(1,5) - HK173*P(2,5);
const float HK177 = HK166*P(4,4) + HK167*P(4,5) + HK168*P(4,6) + HK169*P(4,24) - HK169*P(4,9) + HK170*P(3,4) + HK171*P(0,4) + HK172*P(1,4) - HK173*P(2,4);
const float HK178 = HK169*P(9,24);
const float HK179 = HK166*P(4,24) + HK167*P(5,24) + HK168*P(6,24) + HK169*P(24,24) + HK170*P(3,24) + HK171*P(0,24) + HK172*P(1,24) - HK173*P(2,24) - HK178;
const float HK180 = HK104*HK158;
const float HK181 = HK166*P(4,9) + HK167*P(5,9) + HK168*P(6,9) - HK169*P(9,9) + HK170*P(3,9) + HK171*P(0,9) + HK172*P(1,9) - HK173*P(2,9) + HK178;
const float HK182 = HK166*P(3,4) + HK167*P(3,5) + HK168*P(3,6) + HK169*P(3,24) - HK169*P(3,9) + HK170*P(3,3) + HK171*P(0,3) + HK172*P(1,3) - HK173*P(2,3);
const float HK183 = HK166*P(1,4) + HK167*P(1,5) + HK168*P(1,6) + HK169*P(1,24) - HK169*P(1,9) + HK170*P(1,3) + HK171*P(0,1) + HK172*P(1,1) - HK173*P(1,2);
const float HK184 = HK166*P(2,4) + HK167*P(2,5) + HK168*P(2,6) + HK169*P(2,24) - HK169*P(2,9) + HK170*P(2,3) + HK171*P(0,2) + HK172*P(1,2) - HK173*P(2,2);
const float HK185 = HK38/(HK150*HK175*HK49 + HK154*HK176*HK49 + HK157*HK177*HK49 + HK170*HK182*HK48 + HK171*HK174*HK48 + HK172*HK183*HK48 - HK173*HK184*HK48 + HK179*HK180 - HK180*HK181 + R_LOS);
// Observation Jacobians - axis 0
Hfusion.at<0>() = HK3*HK5;
Hfusion.at<1>() = HK5*HK7;
Hfusion.at<2>() = HK5*HK8;
Hfusion.at<3>() = HK5*HK9;
Hfusion.at<4>() = HK25*HK4;
Hfusion.at<5>() = HK33*HK4;
Hfusion.at<6>() = HK37*HK4;
Hfusion.at<0>() = -HK36*HK39;
Hfusion.at<1>() = -HK39*HK42;
Hfusion.at<2>() = HK39*HK44;
Hfusion.at<3>() = -HK39*HK46;
Hfusion.at<4>() = -HK34*HK47;
Hfusion.at<5>() = -HK30*HK47;
Hfusion.at<6>() = -HK22*HK47;
Hfusion.at<7>() = 0;
Hfusion.at<8>() = 0;
Hfusion.at<9>() = 0;
Hfusion.at<9>() = HK50;
Hfusion.at<10>() = 0;
Hfusion.at<11>() = 0;
Hfusion.at<12>() = 0;
@ -122,48 +212,48 @@ Hfusion.at<20>() = 0;
Hfusion.at<21>() = 0;
Hfusion.at<22>() = 0;
Hfusion.at<23>() = 0;
Hfusion.at<24>() = 0;
Hfusion.at<24>() = -HK50;
// Kalman gains - axis 0
Kfusion(0) = HK42*HK50;
Kfusion(1) = HK49*HK50;
Kfusion(2) = HK47*HK50;
Kfusion(3) = HK48*HK50;
Kfusion(4) = HK46*HK50;
Kfusion(5) = HK45*HK50;
Kfusion(6) = HK44*HK50;
Kfusion(7) = HK50*(HK25*P(4,7) + HK33*P(5,7) + HK37*P(6,7) + HK38*P(0,7) + HK39*P(1,7) + HK40*P(2,7) + HK41*P(3,7));
Kfusion(8) = HK50*(HK25*P(4,8) + HK33*P(5,8) + HK37*P(6,8) + HK38*P(0,8) + HK39*P(1,8) + HK40*P(2,8) + HK41*P(3,8));
Kfusion(9) = HK50*(HK25*P(4,9) + HK33*P(5,9) + HK37*P(6,9) + HK38*P(0,9) + HK39*P(1,9) + HK40*P(2,9) + HK41*P(3,9));
Kfusion(10) = HK50*(HK25*P(4,10) + HK33*P(5,10) + HK37*P(6,10) + HK38*P(0,10) + HK39*P(1,10) + HK40*P(2,10) + HK41*P(3,10));
Kfusion(11) = HK50*(HK25*P(4,11) + HK33*P(5,11) + HK37*P(6,11) + HK38*P(0,11) + HK39*P(1,11) + HK40*P(2,11) + HK41*P(3,11));
Kfusion(12) = HK50*(HK25*P(4,12) + HK33*P(5,12) + HK37*P(6,12) + HK38*P(0,12) + HK39*P(1,12) + HK40*P(2,12) + HK41*P(3,12));
Kfusion(13) = HK50*(HK25*P(4,13) + HK33*P(5,13) + HK37*P(6,13) + HK38*P(0,13) + HK39*P(1,13) + HK40*P(2,13) + HK41*P(3,13));
Kfusion(14) = HK50*(HK25*P(4,14) + HK33*P(5,14) + HK37*P(6,14) + HK38*P(0,14) + HK39*P(1,14) + HK40*P(2,14) + HK41*P(3,14));
Kfusion(15) = HK50*(HK25*P(4,15) + HK33*P(5,15) + HK37*P(6,15) + HK38*P(0,15) + HK39*P(1,15) + HK40*P(2,15) + HK41*P(3,15));
Kfusion(16) = HK50*(HK25*P(4,16) + HK33*P(5,16) + HK37*P(6,16) + HK38*P(0,16) + HK39*P(1,16) + HK40*P(2,16) + HK41*P(3,16));
Kfusion(17) = HK50*(HK25*P(4,17) + HK33*P(5,17) + HK37*P(6,17) + HK38*P(0,17) + HK39*P(1,17) + HK40*P(2,17) + HK41*P(3,17));
Kfusion(18) = HK50*(HK25*P(4,18) + HK33*P(5,18) + HK37*P(6,18) + HK38*P(0,18) + HK39*P(1,18) + HK40*P(2,18) + HK41*P(3,18));
Kfusion(19) = HK50*(HK25*P(4,19) + HK33*P(5,19) + HK37*P(6,19) + HK38*P(0,19) + HK39*P(1,19) + HK40*P(2,19) + HK41*P(3,19));
Kfusion(20) = HK50*(HK25*P(4,20) + HK33*P(5,20) + HK37*P(6,20) + HK38*P(0,20) + HK39*P(1,20) + HK40*P(2,20) + HK41*P(3,20));
Kfusion(21) = HK50*(HK25*P(4,21) + HK33*P(5,21) + HK37*P(6,21) + HK38*P(0,21) + HK39*P(1,21) + HK40*P(2,21) + HK41*P(3,21));
Kfusion(22) = HK50*(HK25*P(4,22) + HK33*P(5,22) + HK37*P(6,22) + HK38*P(0,22) + HK39*P(1,22) + HK40*P(2,22) + HK41*P(3,22));
Kfusion(23) = HK50*(HK25*P(4,23) + HK33*P(5,23) + HK37*P(6,23) + HK38*P(0,23) + HK39*P(1,23) + HK40*P(2,23) + HK41*P(3,23));
Kfusion(24) = HK50*(HK25*P(4,24) + HK33*P(5,24) + HK37*P(6,24) + HK38*P(0,24) + HK39*P(1,24) + HK40*P(2,24) + HK41*P(3,24));
Kfusion(0) = HK142*(HK52 + HK54 + HK56 + HK58 - HK59 + HK61 + HK63 + HK65 - HK67);
Kfusion(1) = HK142*(HK124 + HK125 + HK126 - HK127 + HK128 + HK129 + HK130 + HK131 - HK132);
Kfusion(2) = HK142*(HK133 + HK134 + HK135 - HK136 + HK137 + HK138 + HK139 + HK140 - HK141);
Kfusion(3) = HK142*(HK115 + HK116 + HK117 - HK118 + HK119 + HK120 + HK121 + HK122 - HK123);
Kfusion(4) = HK142*(HK86 + HK87 + HK88 - HK89 + HK90 + HK91 + HK92 + HK93 - HK94);
Kfusion(5) = HK142*(HK77 + HK78 + HK79 - HK80 + HK81 + HK82 + HK83 + HK84 - HK85);
Kfusion(6) = HK142*(HK68 + HK69 + HK70 - HK71 + HK72 + HK73 + HK74 + HK75 - HK76);
Kfusion(7) = HK142*(HK51*P(4,7) + HK53*P(5,7) + HK55*P(6,7) + HK57*P(7,24) - HK57*P(7,9) + HK60*P(3,7) + HK62*P(0,7) + HK64*P(1,7) - HK66*P(2,7));
Kfusion(8) = HK142*(HK51*P(4,8) + HK53*P(5,8) + HK55*P(6,8) + HK57*P(8,24) - HK57*P(8,9) + HK60*P(3,8) + HK62*P(0,8) + HK64*P(1,8) - HK66*P(2,8));
Kfusion(9) = HK142*(HK106 + HK107 + HK108 - HK109 + HK111 + HK112 + HK113 - HK114 + HK98);
Kfusion(10) = HK142*(HK51*P(4,10) + HK53*P(5,10) + HK55*P(6,10) + HK57*P(10,24) - HK57*P(9,10) + HK60*P(3,10) + HK62*P(0,10) + HK64*P(1,10) - HK66*P(2,10));
Kfusion(11) = HK142*(HK51*P(4,11) + HK53*P(5,11) + HK55*P(6,11) + HK57*P(11,24) - HK57*P(9,11) + HK60*P(3,11) + HK62*P(0,11) + HK64*P(1,11) - HK66*P(2,11));
Kfusion(12) = HK142*(HK51*P(4,12) + HK53*P(5,12) + HK55*P(6,12) + HK57*P(12,24) - HK57*P(9,12) + HK60*P(3,12) + HK62*P(0,12) + HK64*P(1,12) - HK66*P(2,12));
Kfusion(13) = HK142*(HK51*P(4,13) + HK53*P(5,13) + HK55*P(6,13) + HK57*P(13,24) - HK57*P(9,13) + HK60*P(3,13) + HK62*P(0,13) + HK64*P(1,13) - HK66*P(2,13));
Kfusion(14) = HK142*(HK51*P(4,14) + HK53*P(5,14) + HK55*P(6,14) + HK57*P(14,24) - HK57*P(9,14) + HK60*P(3,14) + HK62*P(0,14) + HK64*P(1,14) - HK66*P(2,14));
Kfusion(15) = HK142*(HK51*P(4,15) + HK53*P(5,15) + HK55*P(6,15) + HK57*P(15,24) - HK57*P(9,15) + HK60*P(3,15) + HK62*P(0,15) + HK64*P(1,15) - HK66*P(2,15));
Kfusion(16) = HK142*(HK51*P(4,16) + HK53*P(5,16) + HK55*P(6,16) + HK57*P(16,24) - HK57*P(9,16) + HK60*P(3,16) + HK62*P(0,16) + HK64*P(1,16) - HK66*P(2,16));
Kfusion(17) = HK142*(HK51*P(4,17) + HK53*P(5,17) + HK55*P(6,17) + HK57*P(17,24) - HK57*P(9,17) + HK60*P(3,17) + HK62*P(0,17) + HK64*P(1,17) - HK66*P(2,17));
Kfusion(18) = HK142*(HK51*P(4,18) + HK53*P(5,18) + HK55*P(6,18) + HK57*P(18,24) - HK57*P(9,18) + HK60*P(3,18) + HK62*P(0,18) + HK64*P(1,18) - HK66*P(2,18));
Kfusion(19) = HK142*(HK51*P(4,19) + HK53*P(5,19) + HK55*P(6,19) + HK57*P(19,24) - HK57*P(9,19) + HK60*P(3,19) + HK62*P(0,19) + HK64*P(1,19) - HK66*P(2,19));
Kfusion(20) = HK142*(HK51*P(4,20) + HK53*P(5,20) + HK55*P(6,20) + HK57*P(20,24) - HK57*P(9,20) + HK60*P(3,20) + HK62*P(0,20) + HK64*P(1,20) - HK66*P(2,20));
Kfusion(21) = HK142*(HK51*P(4,21) + HK53*P(5,21) + HK55*P(6,21) + HK57*P(21,24) - HK57*P(9,21) + HK60*P(3,21) + HK62*P(0,21) + HK64*P(1,21) - HK66*P(2,21));
Kfusion(22) = HK142*(HK51*P(4,22) + HK53*P(5,22) + HK55*P(6,22) + HK57*P(22,24) - HK57*P(9,22) + HK60*P(3,22) + HK62*P(0,22) + HK64*P(1,22) - HK66*P(2,22));
Kfusion(23) = HK142*(HK51*P(4,23) + HK53*P(5,23) + HK55*P(6,23) + HK57*P(23,24) - HK57*P(9,23) + HK60*P(3,23) + HK62*P(0,23) + HK64*P(1,23) - HK66*P(2,23));
Kfusion(24) = HK142*(HK100 + HK101 + HK102 - HK103 + HK110 + HK95 + HK96 + HK97 + HK99);
// Observation Jacobians - axis 1
Hfusion.at<0>() = -HK5*HK63;
Hfusion.at<1>() = -HK5*HK66;
Hfusion.at<2>() = -HK5*HK68;
Hfusion.at<3>() = -HK5*HK70;
Hfusion.at<4>() = -HK4*HK74;
Hfusion.at<5>() = -HK4*HK77;
Hfusion.at<6>() = -HK4*HK79;
Hfusion.at<0>() = HK159*HK39;
Hfusion.at<1>() = HK161*HK39;
Hfusion.at<2>() = HK39*(HK162 - HK163);
Hfusion.at<3>() = HK164*HK39;
Hfusion.at<4>() = HK157*HK47;
Hfusion.at<5>() = HK154*HK47;
Hfusion.at<6>() = HK150*HK47;
Hfusion.at<7>() = 0;
Hfusion.at<8>() = 0;
Hfusion.at<9>() = 0;
Hfusion.at<9>() = -HK165;
Hfusion.at<10>() = 0;
Hfusion.at<11>() = 0;
Hfusion.at<12>() = 0;
@ -178,32 +268,32 @@ Hfusion.at<20>() = 0;
Hfusion.at<21>() = 0;
Hfusion.at<22>() = 0;
Hfusion.at<23>() = 0;
Hfusion.at<24>() = 0;
Hfusion.at<24>() = HK165;
// Kalman gains - axis 1
Kfusion(0) = -HK87*HK95;
Kfusion(1) = -HK94*HK95;
Kfusion(2) = -HK91*HK95;
Kfusion(3) = -HK93*HK95;
Kfusion(4) = -HK90*HK95;
Kfusion(5) = -HK89*HK95;
Kfusion(6) = -HK88*HK95;
Kfusion(7) = -HK95*(HK80*P(0,7) + HK81*P(1,7) + HK82*P(2,7) + HK83*P(3,7) + HK84*P(4,7) + HK85*P(5,7) + HK86*P(6,7));
Kfusion(8) = -HK95*(HK80*P(0,8) + HK81*P(1,8) + HK82*P(2,8) + HK83*P(3,8) + HK84*P(4,8) + HK85*P(5,8) + HK86*P(6,8));
Kfusion(9) = -HK95*(HK80*P(0,9) + HK81*P(1,9) + HK82*P(2,9) + HK83*P(3,9) + HK84*P(4,9) + HK85*P(5,9) + HK86*P(6,9));
Kfusion(10) = -HK95*(HK80*P(0,10) + HK81*P(1,10) + HK82*P(2,10) + HK83*P(3,10) + HK84*P(4,10) + HK85*P(5,10) + HK86*P(6,10));
Kfusion(11) = -HK95*(HK80*P(0,11) + HK81*P(1,11) + HK82*P(2,11) + HK83*P(3,11) + HK84*P(4,11) + HK85*P(5,11) + HK86*P(6,11));
Kfusion(12) = -HK95*(HK80*P(0,12) + HK81*P(1,12) + HK82*P(2,12) + HK83*P(3,12) + HK84*P(4,12) + HK85*P(5,12) + HK86*P(6,12));
Kfusion(13) = -HK95*(HK80*P(0,13) + HK81*P(1,13) + HK82*P(2,13) + HK83*P(3,13) + HK84*P(4,13) + HK85*P(5,13) + HK86*P(6,13));
Kfusion(14) = -HK95*(HK80*P(0,14) + HK81*P(1,14) + HK82*P(2,14) + HK83*P(3,14) + HK84*P(4,14) + HK85*P(5,14) + HK86*P(6,14));
Kfusion(15) = -HK95*(HK80*P(0,15) + HK81*P(1,15) + HK82*P(2,15) + HK83*P(3,15) + HK84*P(4,15) + HK85*P(5,15) + HK86*P(6,15));
Kfusion(16) = -HK95*(HK80*P(0,16) + HK81*P(1,16) + HK82*P(2,16) + HK83*P(3,16) + HK84*P(4,16) + HK85*P(5,16) + HK86*P(6,16));
Kfusion(17) = -HK95*(HK80*P(0,17) + HK81*P(1,17) + HK82*P(2,17) + HK83*P(3,17) + HK84*P(4,17) + HK85*P(5,17) + HK86*P(6,17));
Kfusion(18) = -HK95*(HK80*P(0,18) + HK81*P(1,18) + HK82*P(2,18) + HK83*P(3,18) + HK84*P(4,18) + HK85*P(5,18) + HK86*P(6,18));
Kfusion(19) = -HK95*(HK80*P(0,19) + HK81*P(1,19) + HK82*P(2,19) + HK83*P(3,19) + HK84*P(4,19) + HK85*P(5,19) + HK86*P(6,19));
Kfusion(20) = -HK95*(HK80*P(0,20) + HK81*P(1,20) + HK82*P(2,20) + HK83*P(3,20) + HK84*P(4,20) + HK85*P(5,20) + HK86*P(6,20));
Kfusion(21) = -HK95*(HK80*P(0,21) + HK81*P(1,21) + HK82*P(2,21) + HK83*P(3,21) + HK84*P(4,21) + HK85*P(5,21) + HK86*P(6,21));
Kfusion(22) = -HK95*(HK80*P(0,22) + HK81*P(1,22) + HK82*P(2,22) + HK83*P(3,22) + HK84*P(4,22) + HK85*P(5,22) + HK86*P(6,22));
Kfusion(23) = -HK95*(HK80*P(0,23) + HK81*P(1,23) + HK82*P(2,23) + HK83*P(3,23) + HK84*P(4,23) + HK85*P(5,23) + HK86*P(6,23));
Kfusion(24) = -HK95*(HK80*P(0,24) + HK81*P(1,24) + HK82*P(2,24) + HK83*P(3,24) + HK84*P(4,24) + HK85*P(5,24) + HK86*P(6,24));
Kfusion(0) = HK174*HK185;
Kfusion(1) = HK183*HK185;
Kfusion(2) = HK184*HK185;
Kfusion(3) = HK182*HK185;
Kfusion(4) = HK177*HK185;
Kfusion(5) = HK176*HK185;
Kfusion(6) = HK175*HK185;
Kfusion(7) = HK185*(HK166*P(4,7) + HK167*P(5,7) + HK168*P(6,7) + HK169*P(7,24) - HK169*P(7,9) + HK170*P(3,7) + HK171*P(0,7) + HK172*P(1,7) - HK173*P(2,7));
Kfusion(8) = HK185*(HK166*P(4,8) + HK167*P(5,8) + HK168*P(6,8) + HK169*P(8,24) - HK169*P(8,9) + HK170*P(3,8) + HK171*P(0,8) + HK172*P(1,8) - HK173*P(2,8));
Kfusion(9) = HK181*HK185;
Kfusion(10) = HK185*(HK166*P(4,10) + HK167*P(5,10) + HK168*P(6,10) + HK169*P(10,24) - HK169*P(9,10) + HK170*P(3,10) + HK171*P(0,10) + HK172*P(1,10) - HK173*P(2,10));
Kfusion(11) = HK185*(HK166*P(4,11) + HK167*P(5,11) + HK168*P(6,11) + HK169*P(11,24) - HK169*P(9,11) + HK170*P(3,11) + HK171*P(0,11) + HK172*P(1,11) - HK173*P(2,11));
Kfusion(12) = HK185*(HK166*P(4,12) + HK167*P(5,12) + HK168*P(6,12) + HK169*P(12,24) - HK169*P(9,12) + HK170*P(3,12) + HK171*P(0,12) + HK172*P(1,12) - HK173*P(2,12));
Kfusion(13) = HK185*(HK166*P(4,13) + HK167*P(5,13) + HK168*P(6,13) + HK169*P(13,24) - HK169*P(9,13) + HK170*P(3,13) + HK171*P(0,13) + HK172*P(1,13) - HK173*P(2,13));
Kfusion(14) = HK185*(HK166*P(4,14) + HK167*P(5,14) + HK168*P(6,14) + HK169*P(14,24) - HK169*P(9,14) + HK170*P(3,14) + HK171*P(0,14) + HK172*P(1,14) - HK173*P(2,14));
Kfusion(15) = HK185*(HK166*P(4,15) + HK167*P(5,15) + HK168*P(6,15) + HK169*P(15,24) - HK169*P(9,15) + HK170*P(3,15) + HK171*P(0,15) + HK172*P(1,15) - HK173*P(2,15));
Kfusion(16) = HK185*(HK166*P(4,16) + HK167*P(5,16) + HK168*P(6,16) + HK169*P(16,24) - HK169*P(9,16) + HK170*P(3,16) + HK171*P(0,16) + HK172*P(1,16) - HK173*P(2,16));
Kfusion(17) = HK185*(HK166*P(4,17) + HK167*P(5,17) + HK168*P(6,17) + HK169*P(17,24) - HK169*P(9,17) + HK170*P(3,17) + HK171*P(0,17) + HK172*P(1,17) - HK173*P(2,17));
Kfusion(18) = HK185*(HK166*P(4,18) + HK167*P(5,18) + HK168*P(6,18) + HK169*P(18,24) - HK169*P(9,18) + HK170*P(3,18) + HK171*P(0,18) + HK172*P(1,18) - HK173*P(2,18));
Kfusion(19) = HK185*(HK166*P(4,19) + HK167*P(5,19) + HK168*P(6,19) + HK169*P(19,24) - HK169*P(9,19) + HK170*P(3,19) + HK171*P(0,19) + HK172*P(1,19) - HK173*P(2,19));
Kfusion(20) = HK185*(HK166*P(4,20) + HK167*P(5,20) + HK168*P(6,20) + HK169*P(20,24) - HK169*P(9,20) + HK170*P(3,20) + HK171*P(0,20) + HK172*P(1,20) - HK173*P(2,20));
Kfusion(21) = HK185*(HK166*P(4,21) + HK167*P(5,21) + HK168*P(6,21) + HK169*P(21,24) - HK169*P(9,21) + HK170*P(3,21) + HK171*P(0,21) + HK172*P(1,21) - HK173*P(2,21));
Kfusion(22) = HK185*(HK166*P(4,22) + HK167*P(5,22) + HK168*P(6,22) + HK169*P(22,24) - HK169*P(9,22) + HK170*P(3,22) + HK171*P(0,22) + HK172*P(1,22) - HK173*P(2,22));
Kfusion(23) = HK185*(HK166*P(4,23) + HK167*P(5,23) + HK168*P(6,23) + HK169*P(23,24) - HK169*P(9,23) + HK170*P(3,23) + HK171*P(0,23) + HK172*P(1,23) - HK173*P(2,23));
Kfusion(24) = HK179*HK185;

10
src/modules/ekf2/EKF/python/ekf_derivation/main.py
View File

@ -143,10 +143,16 @@ def optical_flow_observation(P,state,R_to_body,vx,vy,vz):
obs_var = symbols("R_LOS", real=True) # optical flow line of sight rate measurement noise variance
# Define rotation matrix from body to sensor frame
Tbs = Matrix(3,3,create_Tbs_matrix)
Tsb = Matrix(3,3,create_Tbs_matrix)
# define rotation from nav to sensor frame
Tsn = Tsb * R_to_body
hagl = state[24] - state[9]
range = hagl / Tsn[2,2]
# Calculate earth relative velocity in a non-rotating sensor frame
relVelSensor = Tbs * R_to_body * Matrix([vx,vy,vz])
relVelSensor = Tsn * Matrix([vx,vy,vz])
# Divide by range to get predicted angular LOS rates relative to X and Y
# axes. Note these are rates in a non-rotating sensor frame