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

python generation add shebang, mark executable, ignore cache

Browse Source 
- commit all current generated files
This commit is contained in:
Daniel Agar
2020-09-06 20:27:30 -04:00
committed by Mathieu Bresciani
parent a204c59990
commit fab242a81f
7 changed files with 1044 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/python/ekf_derivation/.gitignore
+1
View File
@@ -0,0 +1 @@
__pycache__
EKF/python/ekf_derivation/generated/acc_bf_generated_alt.cpp
+182
View File
@@ -0,0 +1,182 @@
// Sub Expressions
const float HK0 = vn - vwn;
const float HK1 = ve - vwe;
const float HK2 = HK0*q0 + HK1*q3 - q2*vd;
const float HK3 = 2*Kaccx;
const float HK4 = HK0*q1 + HK1*q2 + q3*vd;
const float HK5 = HK0*q2 - HK1*q1 + q0*vd;
const float HK6 = -HK0*q3 + HK1*q0 + q1*vd;
const float HK7 = powf(q1, 2);
const float HK8 = powf(q2, 2);
const float HK9 = powf(q0, 2) - powf(q3, 2);
const float HK10 = HK7 - HK8 + HK9;
const float HK11 = HK10*Kaccx;
const float HK12 = q0*q3;
const float HK13 = q1*q2;
const float HK14 = HK12 + HK13;
const float HK15 = HK14*HK3;
const float HK16 = q0*q2 - q1*q3;
const float HK17 = 2*HK14;
const float HK18 = 2*HK16;
const float HK19 = 2*HK4;
const float HK20 = 2*HK2;
const float HK21 = 2*HK5;
const float HK22 = 2*HK6;
const float HK23 = HK22*P(0,3);
const float HK24 = -HK10*P(0,22) + HK10*P(0,4) - HK17*P(0,23) + HK17*P(0,5) - HK18*P(0,6) + HK19*P(0,1) + HK20*P(0,0) - HK21*P(0,2) + HK23;
const float HK25 = HK17*P(5,23);
const float HK26 = -HK10*P(22,23) + HK10*P(4,23) - HK17*P(23,23) - HK18*P(6,23) + HK19*P(1,23) + HK20*P(0,23) - HK21*P(2,23) + HK22*P(3,23) + HK25;
const float HK27 = powf(Kaccx, 2);
const float HK28 = HK17*HK27;
const float HK29 = HK10*P(4,5) - HK10*P(5,22) + HK17*P(5,5) - HK18*P(5,6) + HK19*P(1,5) + HK20*P(0,5) - HK21*P(2,5) + HK22*P(3,5) - HK25;
const float HK30 = HK10*P(4,6) - HK10*P(6,22) + HK17*P(5,6) - HK17*P(6,23) - HK18*P(6,6) + HK19*P(1,6) + HK20*P(0,6) - HK21*P(2,6) + HK22*P(3,6);
const float HK31 = HK10*P(4,22);
const float HK32 = HK10*P(4,4) - HK17*P(4,23) + HK17*P(4,5) - HK18*P(4,6) + HK19*P(1,4) + HK20*P(0,4) - HK21*P(2,4) + HK22*P(3,4) - HK31;
const float HK33 = HK10*HK27;
const float HK34 = -HK10*P(22,22) - HK17*P(22,23) + HK17*P(5,22) - HK18*P(6,22) + HK19*P(1,22) + HK20*P(0,22) - HK21*P(2,22) + HK22*P(3,22) + HK31;
const float HK35 = HK21*P(1,2);
const float HK36 = -HK10*P(1,22) + HK10*P(1,4) - HK17*P(1,23) + HK17*P(1,5) - HK18*P(1,6) + HK19*P(1,1) + HK20*P(0,1) + HK22*P(1,3) - HK35;
const float HK37 = HK19*P(1,2);
const float HK38 = -HK10*P(2,22) + HK10*P(2,4) - HK17*P(2,23) + HK17*P(2,5) - HK18*P(2,6) + HK20*P(0,2) - HK21*P(2,2) + HK22*P(2,3) + HK37;
const float HK39 = HK20*P(0,3);
const float HK40 = -HK10*P(3,22) + HK10*P(3,4) - HK17*P(3,23) + HK17*P(3,5) - HK18*P(3,6) + HK19*P(1,3) - HK21*P(2,3) + HK22*P(3,3) + HK39;
const float HK41 = Kaccx/(-HK18*HK27*HK30 + HK19*HK27*HK36 + HK20*HK24*HK27 - HK21*HK27*HK38 + HK22*HK27*HK40 - HK26*HK28 + HK28*HK29 + HK32*HK33 - HK33*HK34 + R_ACC);
const float HK42 = HK12 - HK13;
const float HK43 = 2*Kaccy;
const float HK44 = HK42*HK43;
const float HK45 = -HK7 + HK8 + HK9;
const float HK46 = HK45*Kaccy;
const float HK47 = q0*q1 + q2*q3;
const float HK48 = 2*HK47;
const float HK49 = 2*HK42;
const float HK50 = HK19*P(0,2) + HK21*P(0,1) + HK22*P(0,0) - HK39 - HK45*P(0,23) + HK45*P(0,5) + HK48*P(0,6) + HK49*P(0,22) - HK49*P(0,4);
const float HK51 = powf(Kaccy, 2);
const float HK52 = HK19*P(2,6) - HK20*P(3,6) + HK21*P(1,6) + HK22*P(0,6) + HK45*P(5,6) - HK45*P(6,23) + HK48*P(6,6) - HK49*P(4,6) + HK49*P(6,22);
const float HK53 = HK49*P(4,22);
const float HK54 = HK19*P(2,22) - HK20*P(3,22) + HK21*P(1,22) + HK22*P(0,22) - HK45*P(22,23) + HK45*P(5,22) + HK48*P(6,22) + HK49*P(22,22) - HK53;
const float HK55 = HK49*HK51;
const float HK56 = HK19*P(2,4) - HK20*P(3,4) + HK21*P(1,4) + HK22*P(0,4) - HK45*P(4,23) + HK45*P(4,5) + HK48*P(4,6) - HK49*P(4,4) + HK53;
const float HK57 = HK45*P(5,23);
const float HK58 = HK19*P(2,5) - HK20*P(3,5) + HK21*P(1,5) + HK22*P(0,5) + HK45*P(5,5) + HK48*P(5,6) - HK49*P(4,5) + HK49*P(5,22) - HK57;
const float HK59 = HK45*HK51;
const float HK60 = HK19*P(2,23) - HK20*P(3,23) + HK21*P(1,23) + HK22*P(0,23) - HK45*P(23,23) + HK48*P(6,23) + HK49*P(22,23) - HK49*P(4,23) + HK57;
const float HK61 = HK19*P(2,2) - HK20*P(2,3) + HK22*P(0,2) + HK35 - HK45*P(2,23) + HK45*P(2,5) + HK48*P(2,6) + HK49*P(2,22) - HK49*P(2,4);
const float HK62 = -HK20*P(1,3) + HK21*P(1,1) + HK22*P(0,1) + HK37 - HK45*P(1,23) + HK45*P(1,5) + HK48*P(1,6) + HK49*P(1,22) - HK49*P(1,4);
const float HK63 = HK19*P(2,3) - HK20*P(3,3) + HK21*P(1,3) + HK23 - HK45*P(3,23) + HK45*P(3,5) + HK48*P(3,6) + HK49*P(3,22) - HK49*P(3,4);
const float HK64 = Kaccy/(HK19*HK51*HK61 - HK20*HK51*HK63 + HK21*HK51*HK62 + HK22*HK50*HK51 + HK48*HK51*HK52 + HK54*HK55 - HK55*HK56 + HK58*HK59 - HK59*HK60 + R_ACC);
// Observation Jacobians - axis 0
Hfusion.at<0>() = -HK2*HK3;
Hfusion.at<1>() = -HK3*HK4;
Hfusion.at<2>() = HK3*HK5;
Hfusion.at<3>() = -HK3*HK6;
Hfusion.at<4>() = -HK11;
Hfusion.at<5>() = -HK15;
Hfusion.at<6>() = HK16*HK3;
Hfusion.at<7>() = 0;
Hfusion.at<8>() = 0;
Hfusion.at<9>() = 0;
Hfusion.at<10>() = 0;
Hfusion.at<11>() = 0;
Hfusion.at<12>() = 0;
Hfusion.at<13>() = 0;
Hfusion.at<14>() = 0;
Hfusion.at<15>() = 0;
Hfusion.at<16>() = 0;
Hfusion.at<17>() = 0;
Hfusion.at<18>() = 0;
Hfusion.at<19>() = 0;
Hfusion.at<20>() = 0;
Hfusion.at<21>() = 0;
Hfusion.at<22>() = HK11;
Hfusion.at<23>() = HK15;
// Kalman gains - axis 0
Kfusion(0) = -HK24*HK41;
Kfusion(1) = -HK36*HK41;
Kfusion(2) = -HK38*HK41;
Kfusion(3) = -HK40*HK41;
Kfusion(4) = -HK32*HK41;
Kfusion(5) = -HK29*HK41;
Kfusion(6) = -HK30*HK41;
Kfusion(7) = -HK41*(HK10*P(4,7) - HK10*P(7,22) + HK17*P(5,7) - HK17*P(7,23) - HK18*P(6,7) + HK19*P(1,7) + HK20*P(0,7) - HK21*P(2,7) + HK22*P(3,7));
Kfusion(8) = -HK41*(HK10*P(4,8) - HK10*P(8,22) + HK17*P(5,8) - HK17*P(8,23) - HK18*P(6,8) + HK19*P(1,8) + HK20*P(0,8) - HK21*P(2,8) + HK22*P(3,8));
Kfusion(9) = -HK41*(HK10*P(4,9) - HK10*P(9,22) + HK17*P(5,9) - HK17*P(9,23) - HK18*P(6,9) + HK19*P(1,9) + HK20*P(0,9) - HK21*P(2,9) + HK22*P(3,9));
Kfusion(10) = -HK41*(-HK10*P(10,22) + HK10*P(4,10) - HK17*P(10,23) + HK17*P(5,10) - HK18*P(6,10) + HK19*P(1,10) + HK20*P(0,10) - HK21*P(2,10) + HK22*P(3,10));
Kfusion(11) = -HK41*(-HK10*P(11,22) + HK10*P(4,11) - HK17*P(11,23) + HK17*P(5,11) - HK18*P(6,11) + HK19*P(1,11) + HK20*P(0,11) - HK21*P(2,11) + HK22*P(3,11));
Kfusion(12) = -HK41*(-HK10*P(12,22) + HK10*P(4,12) - HK17*P(12,23) + HK17*P(5,12) - HK18*P(6,12) + HK19*P(1,12) + HK20*P(0,12) - HK21*P(2,12) + HK22*P(3,12));
Kfusion(13) = -HK41*(-HK10*P(13,22) + HK10*P(4,13) - HK17*P(13,23) + HK17*P(5,13) - HK18*P(6,13) + HK19*P(1,13) + HK20*P(0,13) - HK21*P(2,13) + HK22*P(3,13));
Kfusion(14) = -HK41*(-HK10*P(14,22) + HK10*P(4,14) - HK17*P(14,23) + HK17*P(5,14) - HK18*P(6,14) + HK19*P(1,14) + HK20*P(0,14) - HK21*P(2,14) + HK22*P(3,14));
Kfusion(15) = -HK41*(-HK10*P(15,22) + HK10*P(4,15) - HK17*P(15,23) + HK17*P(5,15) - HK18*P(6,15) + HK19*P(1,15) + HK20*P(0,15) - HK21*P(2,15) + HK22*P(3,15));
Kfusion(16) = -HK41*(-HK10*P(16,22) + HK10*P(4,16) - HK17*P(16,23) + HK17*P(5,16) - HK18*P(6,16) + HK19*P(1,16) + HK20*P(0,16) - HK21*P(2,16) + HK22*P(3,16));
Kfusion(17) = -HK41*(-HK10*P(17,22) + HK10*P(4,17) - HK17*P(17,23) + HK17*P(5,17) - HK18*P(6,17) + HK19*P(1,17) + HK20*P(0,17) - HK21*P(2,17) + HK22*P(3,17));
Kfusion(18) = -HK41*(-HK10*P(18,22) + HK10*P(4,18) - HK17*P(18,23) + HK17*P(5,18) - HK18*P(6,18) + HK19*P(1,18) + HK20*P(0,18) - HK21*P(2,18) + HK22*P(3,18));
Kfusion(19) = -HK41*(-HK10*P(19,22) + HK10*P(4,19) - HK17*P(19,23) + HK17*P(5,19) - HK18*P(6,19) + HK19*P(1,19) + HK20*P(0,19) - HK21*P(2,19) + HK22*P(3,19));
Kfusion(20) = -HK41*(-HK10*P(20,22) + HK10*P(4,20) - HK17*P(20,23) + HK17*P(5,20) - HK18*P(6,20) + HK19*P(1,20) + HK20*P(0,20) - HK21*P(2,20) + HK22*P(3,20));
Kfusion(21) = -HK41*(-HK10*P(21,22) + HK10*P(4,21) - HK17*P(21,23) + HK17*P(5,21) - HK18*P(6,21) + HK19*P(1,21) + HK20*P(0,21) - HK21*P(2,21) + HK22*P(3,21));
Kfusion(22) = -HK34*HK41;
Kfusion(23) = -HK26*HK41;
// Observation Jacobians - axis 1
Hfusion.at<0>() = -HK22*Kaccy;
Hfusion.at<1>() = -HK21*Kaccy;
Hfusion.at<2>() = -HK19*Kaccy;
Hfusion.at<3>() = HK20*Kaccy;
Hfusion.at<4>() = HK44;
Hfusion.at<5>() = -HK46;
Hfusion.at<6>() = -HK43*HK47;
Hfusion.at<7>() = 0;
Hfusion.at<8>() = 0;
Hfusion.at<9>() = 0;
Hfusion.at<10>() = 0;
Hfusion.at<11>() = 0;
Hfusion.at<12>() = 0;
Hfusion.at<13>() = 0;
Hfusion.at<14>() = 0;
Hfusion.at<15>() = 0;
Hfusion.at<16>() = 0;
Hfusion.at<17>() = 0;
Hfusion.at<18>() = 0;
Hfusion.at<19>() = 0;
Hfusion.at<20>() = 0;
Hfusion.at<21>() = 0;
Hfusion.at<22>() = -HK44;
Hfusion.at<23>() = HK46;
// Kalman gains - axis 1
Kfusion(0) = -HK50*HK64;
Kfusion(1) = -HK62*HK64;
Kfusion(2) = -HK61*HK64;
Kfusion(3) = -HK63*HK64;
Kfusion(4) = -HK56*HK64;
Kfusion(5) = -HK58*HK64;
Kfusion(6) = -HK52*HK64;
Kfusion(7) = -HK64*(HK19*P(2,7) - HK20*P(3,7) + HK21*P(1,7) + HK22*P(0,7) + HK45*P(5,7) - HK45*P(7,23) + HK48*P(6,7) - HK49*P(4,7) + HK49*P(7,22));
Kfusion(8) = -HK64*(HK19*P(2,8) - HK20*P(3,8) + HK21*P(1,8) + HK22*P(0,8) + HK45*P(5,8) - HK45*P(8,23) + HK48*P(6,8) - HK49*P(4,8) + HK49*P(8,22));
Kfusion(9) = -HK64*(HK19*P(2,9) - HK20*P(3,9) + HK21*P(1,9) + HK22*P(0,9) + HK45*P(5,9) - HK45*P(9,23) + HK48*P(6,9) - HK49*P(4,9) + HK49*P(9,22));
Kfusion(10) = -HK64*(HK19*P(2,10) - HK20*P(3,10) + HK21*P(1,10) + HK22*P(0,10) - HK45*P(10,23) + HK45*P(5,10) + HK48*P(6,10) + HK49*P(10,22) - HK49*P(4,10));
Kfusion(11) = -HK64*(HK19*P(2,11) - HK20*P(3,11) + HK21*P(1,11) + HK22*P(0,11) - HK45*P(11,23) + HK45*P(5,11) + HK48*P(6,11) + HK49*P(11,22) - HK49*P(4,11));
Kfusion(12) = -HK64*(HK19*P(2,12) - HK20*P(3,12) + HK21*P(1,12) + HK22*P(0,12) - HK45*P(12,23) + HK45*P(5,12) + HK48*P(6,12) + HK49*P(12,22) - HK49*P(4,12));
Kfusion(13) = -HK64*(HK19*P(2,13) - HK20*P(3,13) + HK21*P(1,13) + HK22*P(0,13) - HK45*P(13,23) + HK45*P(5,13) + HK48*P(6,13) + HK49*P(13,22) - HK49*P(4,13));
Kfusion(14) = -HK64*(HK19*P(2,14) - HK20*P(3,14) + HK21*P(1,14) + HK22*P(0,14) - HK45*P(14,23) + HK45*P(5,14) + HK48*P(6,14) + HK49*P(14,22) - HK49*P(4,14));
Kfusion(15) = -HK64*(HK19*P(2,15) - HK20*P(3,15) + HK21*P(1,15) + HK22*P(0,15) - HK45*P(15,23) + HK45*P(5,15) + HK48*P(6,15) + HK49*P(15,22) - HK49*P(4,15));
Kfusion(16) = -HK64*(HK19*P(2,16) - HK20*P(3,16) + HK21*P(1,16) + HK22*P(0,16) - HK45*P(16,23) + HK45*P(5,16) + HK48*P(6,16) + HK49*P(16,22) - HK49*P(4,16));
Kfusion(17) = -HK64*(HK19*P(2,17) - HK20*P(3,17) + HK21*P(1,17) + HK22*P(0,17) - HK45*P(17,23) + HK45*P(5,17) + HK48*P(6,17) + HK49*P(17,22) - HK49*P(4,17));
Kfusion(18) = -HK64*(HK19*P(2,18) - HK20*P(3,18) + HK21*P(1,18) + HK22*P(0,18) - HK45*P(18,23) + HK45*P(5,18) + HK48*P(6,18) + HK49*P(18,22) - HK49*P(4,18));
Kfusion(19) = -HK64*(HK19*P(2,19) - HK20*P(3,19) + HK21*P(1,19) + HK22*P(0,19) - HK45*P(19,23) + HK45*P(5,19) + HK48*P(6,19) + HK49*P(19,22) - HK49*P(4,19));
Kfusion(20) = -HK64*(HK19*P(2,20) - HK20*P(3,20) + HK21*P(1,20) + HK22*P(0,20) - HK45*P(20,23) + HK45*P(5,20) + HK48*P(6,20) + HK49*P(20,22) - HK49*P(4,20));
Kfusion(21) = -HK64*(HK19*P(2,21) - HK20*P(3,21) + HK21*P(1,21) + HK22*P(0,21) - HK45*P(21,23) + HK45*P(5,21) + HK48*P(6,21) + HK49*P(21,22) - HK49*P(4,21));
Kfusion(22) = -HK54*HK64;
Kfusion(23) = -HK60*HK64;
// Observation Jacobians - axis 2
// Kalman gains - axis 2
EKF/python/ekf_derivation/generated/flow_generated.cpp
+219
View File
@@ -0,0 +1,219 @@
// X Axis Equations
// Sub Expressions
const float HK0 = -Tbs(1,0)*q2 + Tbs(1,1)*q1 + Tbs(1,2)*q0;
const float HK1 = Tbs(1,0)*q3 + Tbs(1,1)*q0 - Tbs(1,2)*q1;
const float HK2 = Tbs(1,0)*q0 - Tbs(1,1)*q3 + Tbs(1,2)*q2;
const float HK3 = HK0*vd + HK1*ve + HK2*vn;
const float HK4 = 1.0F/range;
const float HK5 = 2*HK4;
const float HK6 = Tbs(1,0)*q1 + Tbs(1,1)*q2 + Tbs(1,2)*q3;
const float HK7 = -HK0*ve + HK1*vd + HK6*vn;
const float HK8 = HK0*vn - HK2*vd + HK6*ve;
const float HK9 = -HK1*vn + HK2*ve + HK6*vd;
const float HK10 = q0*q2;
const float HK11 = q1*q3;
const float HK12 = 2*Tbs(1,2);
const float HK13 = q0*q3;
const float HK14 = q1*q2;
const float HK15 = 2*Tbs(1,1);
const float HK16 = powf(q1, 2);
const float HK17 = powf(q2, 2);
const float HK18 = -HK17;
const float HK19 = powf(q0, 2);
const float HK20 = powf(q3, 2);
const float HK21 = HK19 - HK20;
const float HK22 = HK12*(HK10 + HK11) - HK15*(HK13 - HK14) + Tbs(1,0)*(HK16 + HK18 + HK21);
const float HK23 = 2*Tbs(1,0);
const float HK24 = q0*q1;
const float HK25 = q2*q3;
const float HK26 = -HK16;
const float HK27 = -HK12*(HK24 - HK25) + HK23*(HK13 + HK14) + Tbs(1,1)*(HK17 + HK21 + HK26);
const float HK28 = HK15*(HK24 + HK25) - HK23*(HK10 - HK11) + Tbs(1,2)*(HK18 + HK19 + HK20 + HK26);
const float HK29 = 2*HK3;
const float HK30 = 2*HK7;
const float HK31 = 2*HK8;
const float HK32 = 2*HK9;
const float HK33 = HK22*P(0,4) + HK27*P(0,5) + HK28*P(0,6) + HK29*P(0,0) + HK30*P(0,1) + HK31*P(0,2) + HK32*P(0,3);
const float HK34 = powf(range, -2);
const float HK35 = HK22*P(4,6) + HK27*P(5,6) + HK28*P(6,6) + HK29*P(0,6) + HK30*P(1,6) + HK31*P(2,6) + HK32*P(3,6);
const float HK36 = HK22*P(4,5) + HK27*P(5,5) + HK28*P(5,6) + HK29*P(0,5) + HK30*P(1,5) + HK31*P(2,5) + HK32*P(3,5);
const float HK37 = HK22*P(4,4) + HK27*P(4,5) + HK28*P(4,6) + HK29*P(0,4) + HK30*P(1,4) + HK31*P(2,4) + HK32*P(3,4);
const float HK38 = HK22*P(2,4) + HK27*P(2,5) + HK28*P(2,6) + HK29*P(0,2) + HK30*P(1,2) + HK31*P(2,2) + HK32*P(2,3);
const float HK39 = HK22*P(3,4) + HK27*P(3,5) + HK28*P(3,6) + HK29*P(0,3) + HK30*P(1,3) + HK31*P(2,3) + HK32*P(3,3);
const float HK40 = HK22*P(1,4) + HK27*P(1,5) + HK28*P(1,6) + HK29*P(0,1) + HK30*P(1,1) + HK31*P(1,2) + HK32*P(1,3);
const float HK41 = HK4/(HK22*HK34*HK37 + HK27*HK34*HK36 + HK28*HK34*HK35 + HK29*HK33*HK34 + HK30*HK34*HK40 + HK31*HK34*HK38 + HK32*HK34*HK39 + R_LOS);
// 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<7>() = 0;
Hfusion.at<8>() = 0;
Hfusion.at<9>() = 0;
Hfusion.at<10>() = 0;
Hfusion.at<11>() = 0;
Hfusion.at<12>() = 0;
Hfusion.at<13>() = 0;
Hfusion.at<14>() = 0;
Hfusion.at<15>() = 0;
Hfusion.at<16>() = 0;
Hfusion.at<17>() = 0;
Hfusion.at<18>() = 0;
Hfusion.at<19>() = 0;
Hfusion.at<20>() = 0;
Hfusion.at<21>() = 0;
Hfusion.at<22>() = 0;
Hfusion.at<23>() = 0;
// Kalman gains
Kfusion(0) = HK33*HK41;
Kfusion(1) = HK40*HK41;
Kfusion(2) = HK38*HK41;
Kfusion(3) = HK39*HK41;
Kfusion(4) = HK37*HK41;
Kfusion(5) = HK36*HK41;
Kfusion(6) = HK35*HK41;
Kfusion(7) = HK41*(HK22*P(4,7) + HK27*P(5,7) + HK28*P(6,7) + HK29*P(0,7) + HK30*P(1,7) + HK31*P(2,7) + HK32*P(3,7));
Kfusion(8) = HK41*(HK22*P(4,8) + HK27*P(5,8) + HK28*P(6,8) + HK29*P(0,8) + HK30*P(1,8) + HK31*P(2,8) + HK32*P(3,8));
Kfusion(9) = HK41*(HK22*P(4,9) + HK27*P(5,9) + HK28*P(6,9) + HK29*P(0,9) + HK30*P(1,9) + HK31*P(2,9) + HK32*P(3,9));
Kfusion(10) = HK41*(HK22*P(4,10) + HK27*P(5,10) + HK28*P(6,10) + HK29*P(0,10) + HK30*P(1,10) + HK31*P(2,10) + HK32*P(3,10));
Kfusion(11) = HK41*(HK22*P(4,11) + HK27*P(5,11) + HK28*P(6,11) + HK29*P(0,11) + HK30*P(1,11) + HK31*P(2,11) + HK32*P(3,11));
Kfusion(12) = HK41*(HK22*P(4,12) + HK27*P(5,12) + HK28*P(6,12) + HK29*P(0,12) + HK30*P(1,12) + HK31*P(2,12) + HK32*P(3,12));
Kfusion(13) = HK41*(HK22*P(4,13) + HK27*P(5,13) + HK28*P(6,13) + HK29*P(0,13) + HK30*P(1,13) + HK31*P(2,13) + HK32*P(3,13));
Kfusion(14) = HK41*(HK22*P(4,14) + HK27*P(5,14) + HK28*P(6,14) + HK29*P(0,14) + HK30*P(1,14) + HK31*P(2,14) + HK32*P(3,14));
Kfusion(15) = HK41*(HK22*P(4,15) + HK27*P(5,15) + HK28*P(6,15) + HK29*P(0,15) + HK30*P(1,15) + HK31*P(2,15) + HK32*P(3,15));
Kfusion(16) = HK41*(HK22*P(4,16) + HK27*P(5,16) + HK28*P(6,16) + HK29*P(0,16) + HK30*P(1,16) + HK31*P(2,16) + HK32*P(3,16));
Kfusion(17) = HK41*(HK22*P(4,17) + HK27*P(5,17) + HK28*P(6,17) + HK29*P(0,17) + HK30*P(1,17) + HK31*P(2,17) + HK32*P(3,17));
Kfusion(18) = HK41*(HK22*P(4,18) + HK27*P(5,18) + HK28*P(6,18) + HK29*P(0,18) + HK30*P(1,18) + HK31*P(2,18) + HK32*P(3,18));
Kfusion(19) = HK41*(HK22*P(4,19) + HK27*P(5,19) + HK28*P(6,19) + HK29*P(0,19) + HK30*P(1,19) + HK31*P(2,19) + HK32*P(3,19));
Kfusion(20) = HK41*(HK22*P(4,20) + HK27*P(5,20) + HK28*P(6,20) + HK29*P(0,20) + HK30*P(1,20) + HK31*P(2,20) + HK32*P(3,20));
Kfusion(21) = HK41*(HK22*P(4,21) + HK27*P(5,21) + HK28*P(6,21) + HK29*P(0,21) + HK30*P(1,21) + HK31*P(2,21) + HK32*P(3,21));
Kfusion(22) = HK41*(HK22*P(4,22) + HK27*P(5,22) + HK28*P(6,22) + HK29*P(0,22) + HK30*P(1,22) + HK31*P(2,22) + HK32*P(3,22));
Kfusion(23) = HK41*(HK22*P(4,23) + HK27*P(5,23) + HK28*P(6,23) + HK29*P(0,23) + HK30*P(1,23) + HK31*P(2,23) + HK32*P(3,23));
// 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);
// 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<7>() = 0;
Hfusion.at<8>() = 0;
Hfusion.at<9>() = 0;
Hfusion.at<10>() = 0;
Hfusion.at<11>() = 0;
Hfusion.at<12>() = 0;
Hfusion.at<13>() = 0;
Hfusion.at<14>() = 0;
Hfusion.at<15>() = 0;
Hfusion.at<16>() = 0;
Hfusion.at<17>() = 0;
Hfusion.at<18>() = 0;
Hfusion.at<19>() = 0;
Hfusion.at<20>() = 0;
Hfusion.at<21>() = 0;
Hfusion.at<22>() = 0;
Hfusion.at<23>() = 0;
// Kalman gains
Kfusion(0) = -HK51*HK60;
Kfusion(1) = -HK59*HK60;
Kfusion(2) = -HK56*HK60;
Kfusion(3) = -HK58*HK60;
Kfusion(4) = -HK55*HK60;
Kfusion(5) = -HK54*HK60;
Kfusion(6) = -HK53*HK60;
Kfusion(7) = -HK60*(HK44*P(0,7) + HK45*P(1,7) + HK46*P(2,7) + HK47*P(3,7) + HK48*P(4,7) + HK49*P(5,7) + HK50*P(6,7));
Kfusion(8) = -HK60*(HK44*P(0,8) + HK45*P(1,8) + HK46*P(2,8) + HK47*P(3,8) + HK48*P(4,8) + HK49*P(5,8) + HK50*P(6,8));
Kfusion(9) = -HK60*(HK44*P(0,9) + HK45*P(1,9) + HK46*P(2,9) + HK47*P(3,9) + HK48*P(4,9) + HK49*P(5,9) + HK50*P(6,9));
Kfusion(10) = -HK60*(HK44*P(0,10) + HK45*P(1,10) + HK46*P(2,10) + HK47*P(3,10) + HK48*P(4,10) + HK49*P(5,10) + HK50*P(6,10));
Kfusion(11) = -HK60*(HK44*P(0,11) + HK45*P(1,11) + HK46*P(2,11) + HK47*P(3,11) + HK48*P(4,11) + HK49*P(5,11) + HK50*P(6,11));
Kfusion(12) = -HK60*(HK44*P(0,12) + HK45*P(1,12) + HK46*P(2,12) + HK47*P(3,12) + HK48*P(4,12) + HK49*P(5,12) + HK50*P(6,12));
Kfusion(13) = -HK60*(HK44*P(0,13) + HK45*P(1,13) + HK46*P(2,13) + HK47*P(3,13) + HK48*P(4,13) + HK49*P(5,13) + HK50*P(6,13));
Kfusion(14) = -HK60*(HK44*P(0,14) + HK45*P(1,14) + HK46*P(2,14) + HK47*P(3,14) + HK48*P(4,14) + HK49*P(5,14) + HK50*P(6,14));
Kfusion(15) = -HK60*(HK44*P(0,15) + HK45*P(1,15) + HK46*P(2,15) + HK47*P(3,15) + HK48*P(4,15) + HK49*P(5,15) + HK50*P(6,15));
Kfusion(16) = -HK60*(HK44*P(0,16) + HK45*P(1,16) + HK46*P(2,16) + HK47*P(3,16) + HK48*P(4,16) + HK49*P(5,16) + HK50*P(6,16));
Kfusion(17) = -HK60*(HK44*P(0,17) + HK45*P(1,17) + HK46*P(2,17) + HK47*P(3,17) + HK48*P(4,17) + HK49*P(5,17) + HK50*P(6,17));
Kfusion(18) = -HK60*(HK44*P(0,18) + HK45*P(1,18) + HK46*P(2,18) + HK47*P(3,18) + HK48*P(4,18) + HK49*P(5,18) + HK50*P(6,18));
Kfusion(19) = -HK60*(HK44*P(0,19) + HK45*P(1,19) + HK46*P(2,19) + HK47*P(3,19) + HK48*P(4,19) + HK49*P(5,19) + HK50*P(6,19));
Kfusion(20) = -HK60*(HK44*P(0,20) + HK45*P(1,20) + HK46*P(2,20) + HK47*P(3,20) + HK48*P(4,20) + HK49*P(5,20) + HK50*P(6,20));
Kfusion(21) = -HK60*(HK44*P(0,21) + HK45*P(1,21) + HK46*P(2,21) + HK47*P(3,21) + HK48*P(4,21) + HK49*P(5,21) + HK50*P(6,21));
Kfusion(22) = -HK60*(HK44*P(0,22) + HK45*P(1,22) + HK46*P(2,22) + HK47*P(3,22) + HK48*P(4,22) + HK49*P(5,22) + HK50*P(6,22));
Kfusion(23) = -HK60*(HK44*P(0,23) + HK45*P(1,23) + HK46*P(2,23) + HK47*P(3,23) + HK48*P(4,23) + HK49*P(5,23) + HK50*P(6,23));
EKF/python/ekf_derivation/generated/vel_bf_generated.cpp
+237
View File
@@ -0,0 +1,237 @@
// axis 0
const float HK0 = q0*vn - q2*vd + q3*ve;
const float HK1 = q1*vn + q2*ve + q3*vd;
const float HK2 = q1*ve;
const float HK3 = q0*vd;
const float HK4 = q2*vn;
const float HK5 = q0*ve + q1*vd - q3*vn;
const float HK6 = powf(q0, 2) + powf(q1, 2) - powf(q2, 2) - powf(q3, 2);
const float HK7 = q0*q3 + q1*q2;
const float HK8 = q1*q3;
const float HK9 = q0*q2;
const float HK10 = 2*HK7;
const float HK11 = -2*HK8 + 2*HK9;
const float HK12 = 2*HK1;
const float HK13 = 2*HK0;
const float HK14 = -2*HK2 + 2*HK3 + 2*HK4;
const float HK15 = 2*HK5;
const float HK16 = HK10*P(0,5) - HK11*P(0,6) + HK12*P(0,1) + HK13*P(0,0) - HK14*P(0,2) + HK15*P(0,3) + HK6*P(0,4);
const float HK17 = HK10*P(5,5) - HK11*P(5,6) + HK12*P(1,5) + HK13*P(0,5) - HK14*P(2,5) + HK15*P(3,5) + HK6*P(4,5);
const float HK18 = HK10*P(5,6) - HK11*P(6,6) + HK12*P(1,6) + HK13*P(0,6) - HK14*P(2,6) + HK15*P(3,6) + HK6*P(4,6);
const float HK19 = HK10*P(1,5) - HK11*P(1,6) + HK12*P(1,1) + HK13*P(0,1) - HK14*P(1,2) + HK15*P(1,3) + HK6*P(1,4);
const float HK20 = HK10*P(2,5) - HK11*P(2,6) + HK12*P(1,2) + HK13*P(0,2) - HK14*P(2,2) + HK15*P(2,3) + HK6*P(2,4);
const float HK21 = HK10*P(3,5) - HK11*P(3,6) + HK12*P(1,3) + HK13*P(0,3) - HK14*P(2,3) + HK15*P(3,3) + HK6*P(3,4);
const float HK22 = HK10*P(4,5) - HK11*P(4,6) + HK12*P(1,4) + HK13*P(0,4) - HK14*P(2,4) + HK15*P(3,4) + HK6*P(4,4);
const float HK23 = 1.0F/(HK10*HK17 - HK11*HK18 + HK12*HK19 + HK13*HK16 - HK14*HK20 + HK15*HK21 + HK22*HK6 + R_VEL);
H_VEL(0) = 2*HK0;
H_VEL(1) = 2*HK1;
H_VEL(2) = 2*HK2 - 2*HK3 - 2*HK4;
H_VEL(3) = 2*HK5;
H_VEL(4) = HK6;
H_VEL(5) = 2*HK7;
H_VEL(6) = 2*HK8 - 2*HK9;
H_VEL(7) = 0;
H_VEL(8) = 0;
H_VEL(9) = 0;
H_VEL(10) = 0;
H_VEL(11) = 0;
H_VEL(12) = 0;
H_VEL(13) = 0;
H_VEL(14) = 0;
H_VEL(15) = 0;
H_VEL(16) = 0;
H_VEL(17) = 0;
H_VEL(18) = 0;
H_VEL(19) = 0;
H_VEL(20) = 0;
H_VEL(21) = 0;
H_VEL(22) = 0;
H_VEL(23) = 0;
Kfusion(0) = HK16*HK23;
Kfusion(1) = HK19*HK23;
Kfusion(2) = HK20*HK23;
Kfusion(3) = HK21*HK23;
Kfusion(4) = HK22*HK23;
Kfusion(5) = HK17*HK23;
Kfusion(6) = HK18*HK23;
Kfusion(7) = HK23*(HK10*P(5,7) - HK11*P(6,7) + HK12*P(1,7) + HK13*P(0,7) - HK14*P(2,7) + HK15*P(3,7) + HK6*P(4,7));
Kfusion(8) = HK23*(HK10*P(5,8) - HK11*P(6,8) + HK12*P(1,8) + HK13*P(0,8) - HK14*P(2,8) + HK15*P(3,8) + HK6*P(4,8));
Kfusion(9) = HK23*(HK10*P(5,9) - HK11*P(6,9) + HK12*P(1,9) + HK13*P(0,9) - HK14*P(2,9) + HK15*P(3,9) + HK6*P(4,9));
Kfusion(10) = HK23*(HK10*P(5,10) - HK11*P(6,10) + HK12*P(1,10) + HK13*P(0,10) - HK14*P(2,10) + HK15*P(3,10) + HK6*P(4,10));
Kfusion(11) = HK23*(HK10*P(5,11) - HK11*P(6,11) + HK12*P(1,11) + HK13*P(0,11) - HK14*P(2,11) + HK15*P(3,11) + HK6*P(4,11));
Kfusion(12) = HK23*(HK10*P(5,12) - HK11*P(6,12) + HK12*P(1,12) + HK13*P(0,12) - HK14*P(2,12) + HK15*P(3,12) + HK6*P(4,12));
Kfusion(13) = HK23*(HK10*P(5,13) - HK11*P(6,13) + HK12*P(1,13) + HK13*P(0,13) - HK14*P(2,13) + HK15*P(3,13) + HK6*P(4,13));
Kfusion(14) = HK23*(HK10*P(5,14) - HK11*P(6,14) + HK12*P(1,14) + HK13*P(0,14) - HK14*P(2,14) + HK15*P(3,14) + HK6*P(4,14));
Kfusion(15) = HK23*(HK10*P(5,15) - HK11*P(6,15) + HK12*P(1,15) + HK13*P(0,15) - HK14*P(2,15) + HK15*P(3,15) + HK6*P(4,15));
Kfusion(16) = HK23*(HK10*P(5,16) - HK11*P(6,16) + HK12*P(1,16) + HK13*P(0,16) - HK14*P(2,16) + HK15*P(3,16) + HK6*P(4,16));
Kfusion(17) = HK23*(HK10*P(5,17) - HK11*P(6,17) + HK12*P(1,17) + HK13*P(0,17) - HK14*P(2,17) + HK15*P(3,17) + HK6*P(4,17));
Kfusion(18) = HK23*(HK10*P(5,18) - HK11*P(6,18) + HK12*P(1,18) + HK13*P(0,18) - HK14*P(2,18) + HK15*P(3,18) + HK6*P(4,18));
Kfusion(19) = HK23*(HK10*P(5,19) - HK11*P(6,19) + HK12*P(1,19) + HK13*P(0,19) - HK14*P(2,19) + HK15*P(3,19) + HK6*P(4,19));
Kfusion(20) = HK23*(HK10*P(5,20) - HK11*P(6,20) + HK12*P(1,20) + HK13*P(0,20) - HK14*P(2,20) + HK15*P(3,20) + HK6*P(4,20));
Kfusion(21) = HK23*(HK10*P(5,21) - HK11*P(6,21) + HK12*P(1,21) + HK13*P(0,21) - HK14*P(2,21) + HK15*P(3,21) + HK6*P(4,21));
Kfusion(22) = HK23*(HK10*P(5,22) - HK11*P(6,22) + HK12*P(1,22) + HK13*P(0,22) - HK14*P(2,22) + HK15*P(3,22) + HK6*P(4,22));
Kfusion(23) = HK23*(HK10*P(5,23) - HK11*P(6,23) + HK12*P(1,23) + HK13*P(0,23) - HK14*P(2,23) + HK15*P(3,23) + HK6*P(4,23));
// axis 1
const float HK0 = q0*ve + q1*vd - q3*vn;
const float HK1 = q0*vd - q1*ve + q2*vn;
const float HK2 = q1*vn + q2*ve + q3*vd;
const float HK3 = q2*vd;
const float HK4 = q0*vn;
const float HK5 = q3*ve;
const float HK6 = q1*q2;
const float HK7 = q0*q3;
const float HK8 = powf(q0, 2) - powf(q1, 2) + powf(q2, 2) - powf(q3, 2);
const float HK9 = q0*q1 + q2*q3;
const float HK10 = 2*HK9;
const float HK11 = -2*HK6 + 2*HK7;
const float HK12 = 2*HK2;
const float HK13 = 2*HK0;
const float HK14 = 2*HK1;
const float HK15 = -2*HK3 + 2*HK4 + 2*HK5;
const float HK16 = HK10*P(0,6) - HK11*P(0,4) + HK12*P(0,2) + HK13*P(0,0) + HK14*P(0,1) - HK15*P(0,3) + HK8*P(0,5);
const float HK17 = HK10*P(6,6) - HK11*P(4,6) + HK12*P(2,6) + HK13*P(0,6) + HK14*P(1,6) - HK15*P(3,6) + HK8*P(5,6);
const float HK18 = HK10*P(4,6) - HK11*P(4,4) + HK12*P(2,4) + HK13*P(0,4) + HK14*P(1,4) - HK15*P(3,4) + HK8*P(4,5);
const float HK19 = HK10*P(2,6) - HK11*P(2,4) + HK12*P(2,2) + HK13*P(0,2) + HK14*P(1,2) - HK15*P(2,3) + HK8*P(2,5);
const float HK20 = HK10*P(1,6) - HK11*P(1,4) + HK12*P(1,2) + HK13*P(0,1) + HK14*P(1,1) - HK15*P(1,3) + HK8*P(1,5);
const float HK21 = HK10*P(3,6) - HK11*P(3,4) + HK12*P(2,3) + HK13*P(0,3) + HK14*P(1,3) - HK15*P(3,3) + HK8*P(3,5);
const float HK22 = HK10*P(5,6) - HK11*P(4,5) + HK12*P(2,5) + HK13*P(0,5) + HK14*P(1,5) - HK15*P(3,5) + HK8*P(5,5);
const float HK23 = 1.0F/(HK10*HK17 - HK11*HK18 + HK12*HK19 + HK13*HK16 + HK14*HK20 - HK15*HK21 + HK22*HK8 + R_VEL);
H_VEL(0) = 2*HK0;
H_VEL(1) = 2*HK1;
H_VEL(2) = 2*HK2;
H_VEL(3) = 2*HK3 - 2*HK4 - 2*HK5;
H_VEL(4) = 2*HK6 - 2*HK7;
H_VEL(5) = HK8;
H_VEL(6) = 2*HK9;
H_VEL(7) = 0;
H_VEL(8) = 0;
H_VEL(9) = 0;
H_VEL(10) = 0;
H_VEL(11) = 0;
H_VEL(12) = 0;
H_VEL(13) = 0;
H_VEL(14) = 0;
H_VEL(15) = 0;
H_VEL(16) = 0;
H_VEL(17) = 0;
H_VEL(18) = 0;
H_VEL(19) = 0;
H_VEL(20) = 0;
H_VEL(21) = 0;
H_VEL(22) = 0;
H_VEL(23) = 0;
Kfusion(0) = HK16*HK23;
Kfusion(1) = HK20*HK23;
Kfusion(2) = HK19*HK23;
Kfusion(3) = HK21*HK23;
Kfusion(4) = HK18*HK23;
Kfusion(5) = HK22*HK23;
Kfusion(6) = HK17*HK23;
Kfusion(7) = HK23*(HK10*P(6,7) - HK11*P(4,7) + HK12*P(2,7) + HK13*P(0,7) + HK14*P(1,7) - HK15*P(3,7) + HK8*P(5,7));
Kfusion(8) = HK23*(HK10*P(6,8) - HK11*P(4,8) + HK12*P(2,8) + HK13*P(0,8) + HK14*P(1,8) - HK15*P(3,8) + HK8*P(5,8));
Kfusion(9) = HK23*(HK10*P(6,9) - HK11*P(4,9) + HK12*P(2,9) + HK13*P(0,9) + HK14*P(1,9) - HK15*P(3,9) + HK8*P(5,9));
Kfusion(10) = HK23*(HK10*P(6,10) - HK11*P(4,10) + HK12*P(2,10) + HK13*P(0,10) + HK14*P(1,10) - HK15*P(3,10) + HK8*P(5,10));
Kfusion(11) = HK23*(HK10*P(6,11) - HK11*P(4,11) + HK12*P(2,11) + HK13*P(0,11) + HK14*P(1,11) - HK15*P(3,11) + HK8*P(5,11));
Kfusion(12) = HK23*(HK10*P(6,12) - HK11*P(4,12) + HK12*P(2,12) + HK13*P(0,12) + HK14*P(1,12) - HK15*P(3,12) + HK8*P(5,12));
Kfusion(13) = HK23*(HK10*P(6,13) - HK11*P(4,13) + HK12*P(2,13) + HK13*P(0,13) + HK14*P(1,13) - HK15*P(3,13) + HK8*P(5,13));
Kfusion(14) = HK23*(HK10*P(6,14) - HK11*P(4,14) + HK12*P(2,14) + HK13*P(0,14) + HK14*P(1,14) - HK15*P(3,14) + HK8*P(5,14));
Kfusion(15) = HK23*(HK10*P(6,15) - HK11*P(4,15) + HK12*P(2,15) + HK13*P(0,15) + HK14*P(1,15) - HK15*P(3,15) + HK8*P(5,15));
Kfusion(16) = HK23*(HK10*P(6,16) - HK11*P(4,16) + HK12*P(2,16) + HK13*P(0,16) + HK14*P(1,16) - HK15*P(3,16) + HK8*P(5,16));
Kfusion(17) = HK23*(HK10*P(6,17) - HK11*P(4,17) + HK12*P(2,17) + HK13*P(0,17) + HK14*P(1,17) - HK15*P(3,17) + HK8*P(5,17));
Kfusion(18) = HK23*(HK10*P(6,18) - HK11*P(4,18) + HK12*P(2,18) + HK13*P(0,18) + HK14*P(1,18) - HK15*P(3,18) + HK8*P(5,18));
Kfusion(19) = HK23*(HK10*P(6,19) - HK11*P(4,19) + HK12*P(2,19) + HK13*P(0,19) + HK14*P(1,19) - HK15*P(3,19) + HK8*P(5,19));
Kfusion(20) = HK23*(HK10*P(6,20) - HK11*P(4,20) + HK12*P(2,20) + HK13*P(0,20) + HK14*P(1,20) - HK15*P(3,20) + HK8*P(5,20));
Kfusion(21) = HK23*(HK10*P(6,21) - HK11*P(4,21) + HK12*P(2,21) + HK13*P(0,21) + HK14*P(1,21) - HK15*P(3,21) + HK8*P(5,21));
Kfusion(22) = HK23*(HK10*P(6,22) - HK11*P(4,22) + HK12*P(2,22) + HK13*P(0,22) + HK14*P(1,22) - HK15*P(3,22) + HK8*P(5,22));
Kfusion(23) = HK23*(HK10*P(6,23) - HK11*P(4,23) + HK12*P(2,23) + HK13*P(0,23) + HK14*P(1,23) - HK15*P(3,23) + HK8*P(5,23));
// axis 2
const float HK0 = q0*vd - q1*ve + q2*vn;
const float HK1 = q3*vn;
const float HK2 = q0*ve;
const float HK3 = q1*vd;
const float HK4 = q0*vn - q2*vd + q3*ve;
const float HK5 = q1*vn + q2*ve + q3*vd;
const float HK6 = q0*q2 + q1*q3;
const float HK7 = q2*q3;
const float HK8 = q0*q1;
const float HK9 = powf(q0, 2) - powf(q1, 2) - powf(q2, 2) + powf(q3, 2);
const float HK10 = 2*HK6;
const float HK11 = -2*HK7 + 2*HK8;
const float HK12 = 2*HK5;
const float HK13 = 2*HK0;
const float HK14 = -2*HK1 + 2*HK2 + 2*HK3;
const float HK15 = 2*HK4;
const float HK16 = HK10*P(0,4) - HK11*P(0,5) + HK12*P(0,3) + HK13*P(0,0) - HK14*P(0,1) + HK15*P(0,2) + HK9*P(0,6);
const float HK17 = HK10*P(4,4) - HK11*P(4,5) + HK12*P(3,4) + HK13*P(0,4) - HK14*P(1,4) + HK15*P(2,4) + HK9*P(4,6);
const float HK18 = HK10*P(4,5) - HK11*P(5,5) + HK12*P(3,5) + HK13*P(0,5) - HK14*P(1,5) + HK15*P(2,5) + HK9*P(5,6);
const float HK19 = HK10*P(3,4) - HK11*P(3,5) + HK12*P(3,3) + HK13*P(0,3) - HK14*P(1,3) + HK15*P(2,3) + HK9*P(3,6);
const float HK20 = HK10*P(1,4) - HK11*P(1,5) + HK12*P(1,3) + HK13*P(0,1) - HK14*P(1,1) + HK15*P(1,2) + HK9*P(1,6);
const float HK21 = HK10*P(2,4) - HK11*P(2,5) + HK12*P(2,3) + HK13*P(0,2) - HK14*P(1,2) + HK15*P(2,2) + HK9*P(2,6);
const float HK22 = HK10*P(4,6) - HK11*P(5,6) + HK12*P(3,6) + HK13*P(0,6) - HK14*P(1,6) + HK15*P(2,6) + HK9*P(6,6);
const float HK23 = 1.0F/(HK10*HK17 - HK11*HK18 + HK12*HK19 + HK13*HK16 - HK14*HK20 + HK15*HK21 + HK22*HK9 + R_VEL);
H_VEL(0) = 2*HK0;
H_VEL(1) = 2*HK1 - 2*HK2 - 2*HK3;
H_VEL(2) = 2*HK4;
H_VEL(3) = 2*HK5;
H_VEL(4) = 2*HK6;
H_VEL(5) = 2*HK7 - 2*HK8;
H_VEL(6) = HK9;
H_VEL(7) = 0;
H_VEL(8) = 0;
H_VEL(9) = 0;
H_VEL(10) = 0;
H_VEL(11) = 0;
H_VEL(12) = 0;
H_VEL(13) = 0;
H_VEL(14) = 0;
H_VEL(15) = 0;
H_VEL(16) = 0;
H_VEL(17) = 0;
H_VEL(18) = 0;
H_VEL(19) = 0;
H_VEL(20) = 0;
H_VEL(21) = 0;
H_VEL(22) = 0;
H_VEL(23) = 0;
Kfusion(0) = HK16*HK23;
Kfusion(1) = HK20*HK23;
Kfusion(2) = HK21*HK23;
Kfusion(3) = HK19*HK23;
Kfusion(4) = HK17*HK23;
Kfusion(5) = HK18*HK23;
Kfusion(6) = HK22*HK23;
Kfusion(7) = HK23*(HK10*P(4,7) - HK11*P(5,7) + HK12*P(3,7) + HK13*P(0,7) - HK14*P(1,7) + HK15*P(2,7) + HK9*P(6,7));
Kfusion(8) = HK23*(HK10*P(4,8) - HK11*P(5,8) + HK12*P(3,8) + HK13*P(0,8) - HK14*P(1,8) + HK15*P(2,8) + HK9*P(6,8));
Kfusion(9) = HK23*(HK10*P(4,9) - HK11*P(5,9) + HK12*P(3,9) + HK13*P(0,9) - HK14*P(1,9) + HK15*P(2,9) + HK9*P(6,9));
Kfusion(10) = HK23*(HK10*P(4,10) - HK11*P(5,10) + HK12*P(3,10) + HK13*P(0,10) - HK14*P(1,10) + HK15*P(2,10) + HK9*P(6,10));
Kfusion(11) = HK23*(HK10*P(4,11) - HK11*P(5,11) + HK12*P(3,11) + HK13*P(0,11) - HK14*P(1,11) + HK15*P(2,11) + HK9*P(6,11));
Kfusion(12) = HK23*(HK10*P(4,12) - HK11*P(5,12) + HK12*P(3,12) + HK13*P(0,12) - HK14*P(1,12) + HK15*P(2,12) + HK9*P(6,12));
Kfusion(13) = HK23*(HK10*P(4,13) - HK11*P(5,13) + HK12*P(3,13) + HK13*P(0,13) - HK14*P(1,13) + HK15*P(2,13) + HK9*P(6,13));
Kfusion(14) = HK23*(HK10*P(4,14) - HK11*P(5,14) + HK12*P(3,14) + HK13*P(0,14) - HK14*P(1,14) + HK15*P(2,14) + HK9*P(6,14));
Kfusion(15) = HK23*(HK10*P(4,15) - HK11*P(5,15) + HK12*P(3,15) + HK13*P(0,15) - HK14*P(1,15) + HK15*P(2,15) + HK9*P(6,15));
Kfusion(16) = HK23*(HK10*P(4,16) - HK11*P(5,16) + HK12*P(3,16) + HK13*P(0,16) - HK14*P(1,16) + HK15*P(2,16) + HK9*P(6,16));
Kfusion(17) = HK23*(HK10*P(4,17) - HK11*P(5,17) + HK12*P(3,17) + HK13*P(0,17) - HK14*P(1,17) + HK15*P(2,17) + HK9*P(6,17));
Kfusion(18) = HK23*(HK10*P(4,18) - HK11*P(5,18) + HK12*P(3,18) + HK13*P(0,18) - HK14*P(1,18) + HK15*P(2,18) + HK9*P(6,18));
Kfusion(19) = HK23*(HK10*P(4,19) - HK11*P(5,19) + HK12*P(3,19) + HK13*P(0,19) - HK14*P(1,19) + HK15*P(2,19) + HK9*P(6,19));
Kfusion(20) = HK23*(HK10*P(4,20) - HK11*P(5,20) + HK12*P(3,20) + HK13*P(0,20) - HK14*P(1,20) + HK15*P(2,20) + HK9*P(6,20));
Kfusion(21) = HK23*(HK10*P(4,21) - HK11*P(5,21) + HK12*P(3,21) + HK13*P(0,21) - HK14*P(1,21) + HK15*P(2,21) + HK9*P(6,21));
Kfusion(22) = HK23*(HK10*P(4,22) - HK11*P(5,22) + HK12*P(3,22) + HK13*P(0,22) - HK14*P(1,22) + HK15*P(2,22) + HK9*P(6,22));
Kfusion(23) = HK23*(HK10*P(4,23) - HK11*P(5,23) + HK12*P(3,23) + HK13*P(0,23) - HK14*P(1,23) + HK15*P(2,23) + HK9*P(6,23));
EKF/python/ekf_derivation/generated/vel_bf_generated_alt.cpp
+247
View File
@@ -0,0 +1,247 @@
// Sub Expressions
const float HK0 = q0*vn;
const float HK1 = q3*ve;
const float HK2 = q2*vd;
const float HK3 = HK0 + HK1 - HK2;
const float HK4 = 2*HK3;
const float HK5 = q1*vn + q2*ve + q3*vd;
const float HK6 = 2*HK5;
const float HK7 = q1*ve;
const float HK8 = q0*vd;
const float HK9 = q2*vn;
const float HK10 = q0*ve;
const float HK11 = q1*vd;
const float HK12 = q3*vn;
const float HK13 = HK10 + HK11 - HK12;
const float HK14 = 2*HK13;
const float HK15 = powf(q1, 2);
const float HK16 = powf(q2, 2);
const float HK17 = -HK16;
const float HK18 = powf(q0, 2);
const float HK19 = powf(q3, 2);
const float HK20 = HK18 - HK19;
const float HK21 = HK15 + HK17 + HK20;
const float HK22 = q0*q3;
const float HK23 = q1*q2;
const float HK24 = HK22 + HK23;
const float HK25 = q1*q3;
const float HK26 = q0*q2;
const float HK27 = 2*HK24;
const float HK28 = -2*HK25 + 2*HK26;
const float HK29 = 2*HK5;
const float HK30 = 2*HK3;
const float HK31 = -HK7 + HK8 + HK9;
const float HK32 = 2*HK31;
const float HK33 = HK32*P(0,2);
const float HK34 = 2*HK13;
const float HK35 = HK34*P(0,3);
const float HK36 = HK21*P(0,4) + HK27*P(0,5) - HK28*P(0,6) + HK29*P(0,1) + HK30*P(0,0) - HK33 + HK35;
const float HK37 = HK21*P(4,5) + HK27*P(5,5) - HK28*P(5,6) + HK29*P(1,5) + HK30*P(0,5) - HK32*P(2,5) + HK34*P(3,5);
const float HK38 = HK21*P(4,6) + HK27*P(5,6) - HK28*P(6,6) + HK29*P(1,6) + HK30*P(0,6) - HK32*P(2,6) + HK34*P(3,6);
const float HK39 = HK32*P(1,2);
const float HK40 = HK34*P(1,3);
const float HK41 = HK21*P(1,4) + HK27*P(1,5) - HK28*P(1,6) + HK29*P(1,1) + HK30*P(0,1) - HK39 + HK40;
const float HK42 = HK29*P(1,2);
const float HK43 = HK30*P(0,2);
const float HK44 = HK21*P(2,4) + HK27*P(2,5) - HK28*P(2,6) - HK32*P(2,2) + HK34*P(2,3) + HK42 + HK43;
const float HK45 = HK29*P(1,3);
const float HK46 = HK30*P(0,3);
const float HK47 = HK21*P(3,4) + HK27*P(3,5) - HK28*P(3,6) - HK32*P(2,3) + HK34*P(3,3) + HK45 + HK46;
const float HK48 = HK21*P(4,4) + HK27*P(4,5) - HK28*P(4,6) + HK29*P(1,4) + HK30*P(0,4) - HK32*P(2,4) + HK34*P(3,4);
const float HK49 = 1.0F/(HK21*HK48 + HK27*HK37 - HK28*HK38 + HK29*HK41 + HK30*HK36 - HK32*HK44 + HK34*HK47 + R_VEL);
const float HK50 = 2*HK31;
const float HK51 = -HK15;
const float HK52 = HK16 + HK20 + HK51;
const float HK53 = q0*q1;
const float HK54 = q2*q3;
const float HK55 = HK53 + HK54;
const float HK56 = 2*HK55;
const float HK57 = 2*HK22 - 2*HK23;
const float HK58 = HK32*P(0,1);
const float HK59 = HK29*P(0,2) + HK34*P(0,0) - HK46 + HK52*P(0,5) + HK56*P(0,6) - HK57*P(0,4) + HK58;
const float HK60 = HK29*P(2,6) - HK30*P(3,6) + HK32*P(1,6) + HK34*P(0,6) + HK52*P(5,6) + HK56*P(6,6) - HK57*P(4,6);
const float HK61 = HK29*P(2,4) - HK30*P(3,4) + HK32*P(1,4) + HK34*P(0,4) + HK52*P(4,5) + HK56*P(4,6) - HK57*P(4,4);
const float HK62 = HK30*P(2,3);
const float HK63 = HK29*P(2,2) + HK34*P(0,2) + HK39 + HK52*P(2,5) + HK56*P(2,6) - HK57*P(2,4) - HK62;
const float HK64 = HK34*P(0,1);
const float HK65 = -HK30*P(1,3) + HK32*P(1,1) + HK42 + HK52*P(1,5) + HK56*P(1,6) - HK57*P(1,4) + HK64;
const float HK66 = HK29*P(2,3);
const float HK67 = -HK30*P(3,3) + HK32*P(1,3) + HK35 + HK52*P(3,5) + HK56*P(3,6) - HK57*P(3,4) + HK66;
const float HK68 = HK29*P(2,5) - HK30*P(3,5) + HK32*P(1,5) + HK34*P(0,5) + HK52*P(5,5) + HK56*P(5,6) - HK57*P(4,5);
const float HK69 = 1.0F/(HK29*HK63 - HK30*HK67 + HK32*HK65 + HK34*HK59 + HK52*HK68 + HK56*HK60 - HK57*HK61 + R_VEL);
const float HK70 = HK25 + HK26;
const float HK71 = HK17 + HK18 + HK19 + HK51;
const float HK72 = 2*HK70;
const float HK73 = 2*HK53 - 2*HK54;
const float HK74 = HK29*P(0,3) + HK32*P(0,0) + HK43 - HK64 + HK71*P(0,6) + HK72*P(0,4) - HK73*P(0,5);
const float HK75 = HK29*P(3,4) + HK30*P(2,4) + HK32*P(0,4) - HK34*P(1,4) + HK71*P(4,6) + HK72*P(4,4) - HK73*P(4,5);
const float HK76 = HK29*P(3,5) + HK30*P(2,5) + HK32*P(0,5) - HK34*P(1,5) + HK71*P(5,6) + HK72*P(4,5) - HK73*P(5,5);
const float HK77 = HK29*P(3,3) + HK32*P(0,3) - HK40 + HK62 + HK71*P(3,6) + HK72*P(3,4) - HK73*P(3,5);
const float HK78 = HK30*P(1,2) - HK34*P(1,1) + HK45 + HK58 + HK71*P(1,6) + HK72*P(1,4) - HK73*P(1,5);
const float HK79 = HK30*P(2,2) + HK33 - HK34*P(1,2) + HK66 + HK71*P(2,6) + HK72*P(2,4) - HK73*P(2,5);
const float HK80 = HK29*P(3,6) + HK30*P(2,6) + HK32*P(0,6) - HK34*P(1,6) + HK71*P(6,6) + HK72*P(4,6) - HK73*P(5,6);
const float HK81 = 1.0F/(HK29*HK77 + HK30*HK79 + HK32*HK74 - HK34*HK78 + HK71*HK80 + HK72*HK75 - HK73*HK76 + R_VEL);
// Observation Jacobians - axis 0
Hfusion.at<0>() = HK4;
Hfusion.at<1>() = HK6;
Hfusion.at<2>() = 2*HK7 - 2*HK8 - 2*HK9;
Hfusion.at<3>() = HK14;
Hfusion.at<4>() = HK21;
Hfusion.at<5>() = 2*HK24;
Hfusion.at<6>() = 2*HK25 - 2*HK26;
Hfusion.at<7>() = 0;
Hfusion.at<8>() = 0;
Hfusion.at<9>() = 0;
Hfusion.at<10>() = 0;
Hfusion.at<11>() = 0;
Hfusion.at<12>() = 0;
Hfusion.at<13>() = 0;
Hfusion.at<14>() = 0;
Hfusion.at<15>() = 0;
Hfusion.at<16>() = 0;
Hfusion.at<17>() = 0;
Hfusion.at<18>() = 0;
Hfusion.at<19>() = 0;
Hfusion.at<20>() = 0;
Hfusion.at<21>() = 0;
Hfusion.at<22>() = 0;
Hfusion.at<23>() = 0;
// Kalman gains - axis 0
Kfusion(0) = HK36*HK49;
Kfusion(1) = HK41*HK49;
Kfusion(2) = HK44*HK49;
Kfusion(3) = HK47*HK49;
Kfusion(4) = HK48*HK49;
Kfusion(5) = HK37*HK49;
Kfusion(6) = HK38*HK49;
Kfusion(7) = HK49*(HK21*P(4,7) + HK27*P(5,7) - HK28*P(6,7) + HK29*P(1,7) + HK30*P(0,7) - HK32*P(2,7) + HK34*P(3,7));
Kfusion(8) = HK49*(HK21*P(4,8) + HK27*P(5,8) - HK28*P(6,8) + HK29*P(1,8) + HK30*P(0,8) - HK32*P(2,8) + HK34*P(3,8));
Kfusion(9) = HK49*(HK21*P(4,9) + HK27*P(5,9) - HK28*P(6,9) + HK29*P(1,9) + HK30*P(0,9) - HK32*P(2,9) + HK34*P(3,9));
Kfusion(10) = HK49*(HK21*P(4,10) + HK27*P(5,10) - HK28*P(6,10) + HK29*P(1,10) + HK30*P(0,10) - HK32*P(2,10) + HK34*P(3,10));
Kfusion(11) = HK49*(HK21*P(4,11) + HK27*P(5,11) - HK28*P(6,11) + HK29*P(1,11) + HK30*P(0,11) - HK32*P(2,11) + HK34*P(3,11));
Kfusion(12) = HK49*(HK21*P(4,12) + HK27*P(5,12) - HK28*P(6,12) + HK29*P(1,12) + HK30*P(0,12) - HK32*P(2,12) + HK34*P(3,12));
Kfusion(13) = HK49*(HK21*P(4,13) + HK27*P(5,13) - HK28*P(6,13) + HK29*P(1,13) + HK30*P(0,13) - HK32*P(2,13) + HK34*P(3,13));
Kfusion(14) = HK49*(HK21*P(4,14) + HK27*P(5,14) - HK28*P(6,14) + HK29*P(1,14) + HK30*P(0,14) - HK32*P(2,14) + HK34*P(3,14));
Kfusion(15) = HK49*(HK21*P(4,15) + HK27*P(5,15) - HK28*P(6,15) + HK29*P(1,15) + HK30*P(0,15) - HK32*P(2,15) + HK34*P(3,15));
Kfusion(16) = HK49*(HK21*P(4,16) + HK27*P(5,16) - HK28*P(6,16) + HK29*P(1,16) + HK30*P(0,16) - HK32*P(2,16) + HK34*P(3,16));
Kfusion(17) = HK49*(HK21*P(4,17) + HK27*P(5,17) - HK28*P(6,17) + HK29*P(1,17) + HK30*P(0,17) - HK32*P(2,17) + HK34*P(3,17));
Kfusion(18) = HK49*(HK21*P(4,18) + HK27*P(5,18) - HK28*P(6,18) + HK29*P(1,18) + HK30*P(0,18) - HK32*P(2,18) + HK34*P(3,18));
Kfusion(19) = HK49*(HK21*P(4,19) + HK27*P(5,19) - HK28*P(6,19) + HK29*P(1,19) + HK30*P(0,19) - HK32*P(2,19) + HK34*P(3,19));
Kfusion(20) = HK49*(HK21*P(4,20) + HK27*P(5,20) - HK28*P(6,20) + HK29*P(1,20) + HK30*P(0,20) - HK32*P(2,20) + HK34*P(3,20));
Kfusion(21) = HK49*(HK21*P(4,21) + HK27*P(5,21) - HK28*P(6,21) + HK29*P(1,21) + HK30*P(0,21) - HK32*P(2,21) + HK34*P(3,21));
Kfusion(22) = HK49*(HK21*P(4,22) + HK27*P(5,22) - HK28*P(6,22) + HK29*P(1,22) + HK30*P(0,22) - HK32*P(2,22) + HK34*P(3,22));
Kfusion(23) = HK49*(HK21*P(4,23) + HK27*P(5,23) - HK28*P(6,23) + HK29*P(1,23) + HK30*P(0,23) - HK32*P(2,23) + HK34*P(3,23));
// Observation Jacobians - axis 1
Hfusion.at<0>() = HK14;
Hfusion.at<1>() = HK50;
Hfusion.at<2>() = HK6;
Hfusion.at<3>() = -2*HK0 - 2*HK1 + 2*HK2;
Hfusion.at<4>() = -2*HK22 + 2*HK23;
Hfusion.at<5>() = HK52;
Hfusion.at<6>() = 2*HK55;
Hfusion.at<7>() = 0;
Hfusion.at<8>() = 0;
Hfusion.at<9>() = 0;
Hfusion.at<10>() = 0;
Hfusion.at<11>() = 0;
Hfusion.at<12>() = 0;
Hfusion.at<13>() = 0;
Hfusion.at<14>() = 0;
Hfusion.at<15>() = 0;
Hfusion.at<16>() = 0;
Hfusion.at<17>() = 0;
Hfusion.at<18>() = 0;
Hfusion.at<19>() = 0;
Hfusion.at<20>() = 0;
Hfusion.at<21>() = 0;
Hfusion.at<22>() = 0;
Hfusion.at<23>() = 0;
// Kalman gains - axis 1
Kfusion(0) = HK59*HK69;
Kfusion(1) = HK65*HK69;
Kfusion(2) = HK63*HK69;
Kfusion(3) = HK67*HK69;
Kfusion(4) = HK61*HK69;
Kfusion(5) = HK68*HK69;
Kfusion(6) = HK60*HK69;
Kfusion(7) = HK69*(HK29*P(2,7) - HK30*P(3,7) + HK32*P(1,7) + HK34*P(0,7) + HK52*P(5,7) + HK56*P(6,7) - HK57*P(4,7));
Kfusion(8) = HK69*(HK29*P(2,8) - HK30*P(3,8) + HK32*P(1,8) + HK34*P(0,8) + HK52*P(5,8) + HK56*P(6,8) - HK57*P(4,8));
Kfusion(9) = HK69*(HK29*P(2,9) - HK30*P(3,9) + HK32*P(1,9) + HK34*P(0,9) + HK52*P(5,9) + HK56*P(6,9) - HK57*P(4,9));
Kfusion(10) = HK69*(HK29*P(2,10) - HK30*P(3,10) + HK32*P(1,10) + HK34*P(0,10) + HK52*P(5,10) + HK56*P(6,10) - HK57*P(4,10));
Kfusion(11) = HK69*(HK29*P(2,11) - HK30*P(3,11) + HK32*P(1,11) + HK34*P(0,11) + HK52*P(5,11) + HK56*P(6,11) - HK57*P(4,11));
Kfusion(12) = HK69*(HK29*P(2,12) - HK30*P(3,12) + HK32*P(1,12) + HK34*P(0,12) + HK52*P(5,12) + HK56*P(6,12) - HK57*P(4,12));
Kfusion(13) = HK69*(HK29*P(2,13) - HK30*P(3,13) + HK32*P(1,13) + HK34*P(0,13) + HK52*P(5,13) + HK56*P(6,13) - HK57*P(4,13));
Kfusion(14) = HK69*(HK29*P(2,14) - HK30*P(3,14) + HK32*P(1,14) + HK34*P(0,14) + HK52*P(5,14) + HK56*P(6,14) - HK57*P(4,14));
Kfusion(15) = HK69*(HK29*P(2,15) - HK30*P(3,15) + HK32*P(1,15) + HK34*P(0,15) + HK52*P(5,15) + HK56*P(6,15) - HK57*P(4,15));
Kfusion(16) = HK69*(HK29*P(2,16) - HK30*P(3,16) + HK32*P(1,16) + HK34*P(0,16) + HK52*P(5,16) + HK56*P(6,16) - HK57*P(4,16));
Kfusion(17) = HK69*(HK29*P(2,17) - HK30*P(3,17) + HK32*P(1,17) + HK34*P(0,17) + HK52*P(5,17) + HK56*P(6,17) - HK57*P(4,17));
Kfusion(18) = HK69*(HK29*P(2,18) - HK30*P(3,18) + HK32*P(1,18) + HK34*P(0,18) + HK52*P(5,18) + HK56*P(6,18) - HK57*P(4,18));
Kfusion(19) = HK69*(HK29*P(2,19) - HK30*P(3,19) + HK32*P(1,19) + HK34*P(0,19) + HK52*P(5,19) + HK56*P(6,19) - HK57*P(4,19));
Kfusion(20) = HK69*(HK29*P(2,20) - HK30*P(3,20) + HK32*P(1,20) + HK34*P(0,20) + HK52*P(5,20) + HK56*P(6,20) - HK57*P(4,20));
Kfusion(21) = HK69*(HK29*P(2,21) - HK30*P(3,21) + HK32*P(1,21) + HK34*P(0,21) + HK52*P(5,21) + HK56*P(6,21) - HK57*P(4,21));
Kfusion(22) = HK69*(HK29*P(2,22) - HK30*P(3,22) + HK32*P(1,22) + HK34*P(0,22) + HK52*P(5,22) + HK56*P(6,22) - HK57*P(4,22));
Kfusion(23) = HK69*(HK29*P(2,23) - HK30*P(3,23) + HK32*P(1,23) + HK34*P(0,23) + HK52*P(5,23) + HK56*P(6,23) - HK57*P(4,23));
// Observation Jacobians - axis 2
Hfusion.at<0>() = HK50;
Hfusion.at<1>() = -2*HK10 - 2*HK11 + 2*HK12;
Hfusion.at<2>() = HK4;
Hfusion.at<3>() = HK6;
Hfusion.at<4>() = 2*HK70;
Hfusion.at<5>() = -2*HK53 + 2*HK54;
Hfusion.at<6>() = HK71;
Hfusion.at<7>() = 0;
Hfusion.at<8>() = 0;
Hfusion.at<9>() = 0;
Hfusion.at<10>() = 0;
Hfusion.at<11>() = 0;
Hfusion.at<12>() = 0;
Hfusion.at<13>() = 0;
Hfusion.at<14>() = 0;
Hfusion.at<15>() = 0;
Hfusion.at<16>() = 0;
Hfusion.at<17>() = 0;
Hfusion.at<18>() = 0;
Hfusion.at<19>() = 0;
Hfusion.at<20>() = 0;
Hfusion.at<21>() = 0;
Hfusion.at<22>() = 0;
Hfusion.at<23>() = 0;
// Kalman gains - axis 2
Kfusion(0) = HK74*HK81;
Kfusion(1) = HK78*HK81;
Kfusion(2) = HK79*HK81;
Kfusion(3) = HK77*HK81;
Kfusion(4) = HK75*HK81;
Kfusion(5) = HK76*HK81;
Kfusion(6) = HK80*HK81;
Kfusion(7) = HK81*(HK29*P(3,7) + HK30*P(2,7) + HK32*P(0,7) - HK34*P(1,7) + HK71*P(6,7) + HK72*P(4,7) - HK73*P(5,7));
Kfusion(8) = HK81*(HK29*P(3,8) + HK30*P(2,8) + HK32*P(0,8) - HK34*P(1,8) + HK71*P(6,8) + HK72*P(4,8) - HK73*P(5,8));
Kfusion(9) = HK81*(HK29*P(3,9) + HK30*P(2,9) + HK32*P(0,9) - HK34*P(1,9) + HK71*P(6,9) + HK72*P(4,9) - HK73*P(5,9));
Kfusion(10) = HK81*(HK29*P(3,10) + HK30*P(2,10) + HK32*P(0,10) - HK34*P(1,10) + HK71*P(6,10) + HK72*P(4,10) - HK73*P(5,10));
Kfusion(11) = HK81*(HK29*P(3,11) + HK30*P(2,11) + HK32*P(0,11) - HK34*P(1,11) + HK71*P(6,11) + HK72*P(4,11) - HK73*P(5,11));
Kfusion(12) = HK81*(HK29*P(3,12) + HK30*P(2,12) + HK32*P(0,12) - HK34*P(1,12) + HK71*P(6,12) + HK72*P(4,12) - HK73*P(5,12));
Kfusion(13) = HK81*(HK29*P(3,13) + HK30*P(2,13) + HK32*P(0,13) - HK34*P(1,13) + HK71*P(6,13) + HK72*P(4,13) - HK73*P(5,13));
Kfusion(14) = HK81*(HK29*P(3,14) + HK30*P(2,14) + HK32*P(0,14) - HK34*P(1,14) + HK71*P(6,14) + HK72*P(4,14) - HK73*P(5,14));
Kfusion(15) = HK81*(HK29*P(3,15) + HK30*P(2,15) + HK32*P(0,15) - HK34*P(1,15) + HK71*P(6,15) + HK72*P(4,15) - HK73*P(5,15));
Kfusion(16) = HK81*(HK29*P(3,16) + HK30*P(2,16) + HK32*P(0,16) - HK34*P(1,16) + HK71*P(6,16) + HK72*P(4,16) - HK73*P(5,16));
Kfusion(17) = HK81*(HK29*P(3,17) + HK30*P(2,17) + HK32*P(0,17) - HK34*P(1,17) + HK71*P(6,17) + HK72*P(4,17) - HK73*P(5,17));
Kfusion(18) = HK81*(HK29*P(3,18) + HK30*P(2,18) + HK32*P(0,18) - HK34*P(1,18) + HK71*P(6,18) + HK72*P(4,18) - HK73*P(5,18));
Kfusion(19) = HK81*(HK29*P(3,19) + HK30*P(2,19) + HK32*P(0,19) - HK34*P(1,19) + HK71*P(6,19) + HK72*P(4,19) - HK73*P(5,19));
Kfusion(20) = HK81*(HK29*P(3,20) + HK30*P(2,20) + HK32*P(0,20) - HK34*P(1,20) + HK71*P(6,20) + HK72*P(4,20) - HK73*P(5,20));
Kfusion(21) = HK81*(HK29*P(3,21) + HK30*P(2,21) + HK32*P(0,21) - HK34*P(1,21) + HK71*P(6,21) + HK72*P(4,21) - HK73*P(5,21));
Kfusion(22) = HK81*(HK29*P(3,22) + HK30*P(2,22) + HK32*P(0,22) - HK34*P(1,22) + HK71*P(6,22) + HK72*P(4,22) - HK73*P(5,22));
Kfusion(23) = HK81*(HK29*P(3,23) + HK30*P(2,23) + HK32*P(0,23) - HK34*P(1,23) + HK71*P(6,23) + HK72*P(4,23) - HK73*P(5,23));
EKF/python/ekf_derivation/generated/yaw_generated.cpp
+156
View File
@@ -0,0 +1,156 @@
// calculate 321 yaw observation matrix - option A
const float SA0 = 2*q3;
const float SA1 = 2*q2;
const float SA2 = SA0*q0 + SA1*q1;
const float SA3 = powf(q0, 2) + powf(q1, 2) - powf(q2, 2) - powf(q3, 2);
const float SA4 = powf(SA3, -2);
const float SA5 = 1.0F/(powf(SA2, 2)*SA4 + 1);
const float SA6 = 1.0F/SA3;
const float SA7 = SA2*SA4;
const float SA8 = 2*SA7;
const float SA9 = 2*SA6;
H_YAW.at<0>() = SA5*(SA0*SA6 - SA8*q0);
H_YAW.at<1>() = SA5*(SA1*SA6 - SA8*q1);
H_YAW.at<2>() = SA5*(SA1*SA7 + SA9*q1);
H_YAW.at<3>() = SA5*(SA0*SA7 + SA9*q0);
H_YAW.at<4>() = 0;
H_YAW.at<5>() = 0;
H_YAW.at<6>() = 0;
H_YAW.at<7>() = 0;
H_YAW.at<8>() = 0;
H_YAW.at<9>() = 0;
H_YAW.at<10>() = 0;
H_YAW.at<11>() = 0;
H_YAW.at<12>() = 0;
H_YAW.at<13>() = 0;
H_YAW.at<14>() = 0;
H_YAW.at<15>() = 0;
H_YAW.at<16>() = 0;
H_YAW.at<17>() = 0;
H_YAW.at<18>() = 0;
H_YAW.at<19>() = 0;
H_YAW.at<20>() = 0;
H_YAW.at<21>() = 0;
H_YAW.at<22>() = 0;
H_YAW.at<23>() = 0;
// calculate 321 yaw observation matrix - option B
const float SB0 = 2*q0;
const float SB1 = 2*q1;
const float SB2 = SB0*q3 + SB1*q2;
const float SB3 = powf(SB2, -2);
const float SB4 = powf(q0, 2) + powf(q1, 2) - powf(q2, 2) - powf(q3, 2);
const float SB5 = 1.0F/(SB3*powf(SB4, 2) + 1);
const float SB6 = 1.0F/SB2;
const float SB7 = SB3*SB4;
const float SB8 = 2*SB7;
const float SB9 = 2*SB6;
H_YAW.at<0>() = -SB5*(SB0*SB6 - SB8*q3);
H_YAW.at<1>() = -SB5*(SB1*SB6 - SB8*q2);
H_YAW.at<2>() = -SB5*(-SB1*SB7 - SB9*q2);
H_YAW.at<3>() = -SB5*(-SB0*SB7 - SB9*q3);
H_YAW.at<4>() = 0;
H_YAW.at<5>() = 0;
H_YAW.at<6>() = 0;
H_YAW.at<7>() = 0;
H_YAW.at<8>() = 0;
H_YAW.at<9>() = 0;
H_YAW.at<10>() = 0;
H_YAW.at<11>() = 0;
H_YAW.at<12>() = 0;
H_YAW.at<13>() = 0;
H_YAW.at<14>() = 0;
H_YAW.at<15>() = 0;
H_YAW.at<16>() = 0;
H_YAW.at<17>() = 0;
H_YAW.at<18>() = 0;
H_YAW.at<19>() = 0;
H_YAW.at<20>() = 0;
H_YAW.at<21>() = 0;
H_YAW.at<22>() = 0;
H_YAW.at<23>() = 0;
// calculate 312 yaw observation matrix - option A
const float SA0 = 2*q3;
const float SA1 = 2*q2;
const float SA2 = SA0*q0 - SA1*q1;
const float SA3 = powf(q0, 2) - powf(q1, 2) + powf(q2, 2) - powf(q3, 2);
const float SA4 = powf(SA3, -2);
const float SA5 = 1.0F/(powf(SA2, 2)*SA4 + 1);
const float SA6 = 1.0F/SA3;
const float SA7 = SA2*SA4;
const float SA8 = 2*SA7;
const float SA9 = 2*SA6;
H_YAW.at<0>() = SA5*(SA0*SA6 - SA8*q0);
H_YAW.at<1>() = SA5*(-SA1*SA6 + SA8*q1);
H_YAW.at<2>() = SA5*(-SA1*SA7 - SA9*q1);
H_YAW.at<3>() = SA5*(SA0*SA7 + SA9*q0);
H_YAW.at<4>() = 0;
H_YAW.at<5>() = 0;
H_YAW.at<6>() = 0;
H_YAW.at<7>() = 0;
H_YAW.at<8>() = 0;
H_YAW.at<9>() = 0;
H_YAW.at<10>() = 0;
H_YAW.at<11>() = 0;
H_YAW.at<12>() = 0;
H_YAW.at<13>() = 0;
H_YAW.at<14>() = 0;
H_YAW.at<15>() = 0;
H_YAW.at<16>() = 0;
H_YAW.at<17>() = 0;
H_YAW.at<18>() = 0;
H_YAW.at<19>() = 0;
H_YAW.at<20>() = 0;
H_YAW.at<21>() = 0;
H_YAW.at<22>() = 0;
H_YAW.at<23>() = 0;
// calculate 312 yaw observation matrix - option B
const float SB0 = 2*q0;
const float SB1 = 2*q1;
const float SB2 = -SB0*q3 + SB1*q2;
const float SB3 = powf(SB2, -2);
const float SB4 = -powf(q0, 2) + powf(q1, 2) - powf(q2, 2) + powf(q3, 2);
const float SB5 = 1.0F/(SB3*powf(SB4, 2) + 1);
const float SB6 = 1.0F/SB2;
const float SB7 = SB3*SB4;
const float SB8 = 2*SB7;
const float SB9 = 2*SB6;
H_YAW.at<0>() = -SB5*(-SB0*SB6 + SB8*q3);
H_YAW.at<1>() = -SB5*(SB1*SB6 - SB8*q2);
H_YAW.at<2>() = -SB5*(-SB1*SB7 - SB9*q2);
H_YAW.at<3>() = -SB5*(SB0*SB7 + SB9*q3);
H_YAW.at<4>() = 0;
H_YAW.at<5>() = 0;
H_YAW.at<6>() = 0;
H_YAW.at<7>() = 0;
H_YAW.at<8>() = 0;
H_YAW.at<9>() = 0;
H_YAW.at<10>() = 0;
H_YAW.at<11>() = 0;
H_YAW.at<12>() = 0;
H_YAW.at<13>() = 0;
H_YAW.at<14>() = 0;
H_YAW.at<15>() = 0;
H_YAW.at<16>() = 0;
H_YAW.at<17>() = 0;
H_YAW.at<18>() = 0;
H_YAW.at<19>() = 0;
H_YAW.at<20>() = 0;
H_YAW.at<21>() = 0;
H_YAW.at<22>() = 0;
H_YAW.at<23>() = 0;
EKF/python/ekf_derivation/main.py
Regular → Executable
+2
View File
@@ -1,3 +1,5 @@
#!/usr/bin/env python3
from sympy import * from sympy import *
from code_gen import * from code_gen import *