fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-15 05:57:36 +08:00
Code Issues Packages Projects Releases Wiki Activity
Files
2d8e183aa3b85cffac172fe73d0f0ca1ea198c0e
PX4-Autopilot/EKF/python/ekf_derivation/generated/flow_generated_alt.cpp
T
bresch 2d8e183aa3 update EKF derivation auto-generated code
2021-07-13 10:49:32 +02:00

223 lines
11 KiB
C++
Raw Blame History

// Sub Expressions
const float HK0 = Tbs(1,0)*q2;
const float HK1 = Tbs(1,1)*q1;
const float HK2 = Tbs(1,1)*q3;
const float HK3 = Tbs(1,2)*q2;
const float HK4 = Tbs(1,0)*q3;
const float HK5 = Tbs(1,2)*q1;
const float HK6 = -HK5;
const float HK7 = vd*(HK0 - HK1) - ve*(HK4 + HK6) + vn*(HK2 - HK3);
const float HK8 = 1.0F/range;
const float HK9 = 2*HK8;
const float HK10 = Tbs(1,1)*q2;
const float HK11 = Tbs(1,2)*q3;
const float HK12 = Tbs(1,1)*q0;
const float HK13 = Tbs(1,2)*q0;
const float HK14 = vd*(HK12 + HK4 - 2*HK5) - ve*(-HK0 + 2*HK1 + HK13) + vn*(HK10 + HK11);
const float HK15 = Tbs(1,0)*q1;
const float HK16 = Tbs(1,0)*q0;
const float HK17 = -vd*(HK16 - HK2 + 2*HK3) + ve*(HK11 + HK15) + vn*(-2*HK0 + HK1 + HK13);
const float HK18 = vd*(HK10 + HK15) + ve*(HK16 - 2*HK2 + HK3) - vn*(HK12 + 2*HK4 + HK6);
const float HK19 = q0*q2;
const float HK20 = q1*q3;
const float HK21 = HK19 + HK20;
const float HK22 = 2*Tbs(1,2);
const float HK23 = q0*q3;
const float HK24 = q1*q2;
const float HK25 = HK23 - HK24;
const float HK26 = 2*Tbs(1,1);
const float HK27 = 2*powf(q2, 2);
const float HK28 = 2*powf(q3, 2);
const float HK29 = HK28 - 1;
const float HK30 = HK27 + HK29;
const float HK31 = -HK21*HK22 + HK25*HK26 + HK30*Tbs(1,0);
const float HK32 = HK23 + HK24;
const float HK33 = 2*Tbs(1,0);
const float HK34 = q0*q1;
const float HK35 = q2*q3;
const float HK36 = HK34 - HK35;
const float HK37 = 2*powf(q1, 2);
const float HK38 = HK29 + HK37;
const float HK39 = HK22*HK36 - HK32*HK33 + HK38*Tbs(1,1);
const float HK40 = HK34 + HK35;
const float HK41 = HK19 - HK20;
const float HK42 = HK27 + HK37 - 1;
const float HK43 = -HK26*HK40 + HK33*HK41 + HK42*Tbs(1,2);
const float HK44 = 2*HK7;
const float HK45 = 2*HK14;
const float HK46 = 2*HK17;
const float HK47 = 2*HK18;
const float HK48 = -HK31*P(0,4) - HK39*P(0,5) - HK43*P(0,6) - HK44*P(0,0) + HK45*P(0,1) + HK46*P(0,2) + HK47*P(0,3);
const float HK49 = powf(range, -2);
const float HK50 = -HK31*P(4,6) - HK39*P(5,6) - HK43*P(6,6) - HK44*P(0,6) + HK45*P(1,6) + HK46*P(2,6) + HK47*P(3,6);
const float HK51 = -HK31*P(4,5) - HK39*P(5,5) - HK43*P(5,6) - HK44*P(0,5) + HK45*P(1,5) + HK46*P(2,5) + HK47*P(3,5);
const float HK52 = -HK31*P(4,4) - HK39*P(4,5) - HK43*P(4,6) - HK44*P(0,4) + HK45*P(1,4) + HK46*P(2,4) + HK47*P(3,4);
const float HK53 = -HK31*P(3,4) - HK39*P(3,5) - HK43*P(3,6) - HK44*P(0,3) + HK45*P(1,3) + HK46*P(2,3) + HK47*P(3,3);
const float HK54 = -HK31*P(2,4) - HK39*P(2,5) - HK43*P(2,6) - HK44*P(0,2) + HK45*P(1,2) + HK46*P(2,2) + HK47*P(2,3);
const float HK55 = -HK31*P(1,4) - HK39*P(1,5) - HK43*P(1,6) - HK44*P(0,1) + HK45*P(1,1) + HK46*P(1,2) + HK47*P(1,3);
const float HK56 = HK8/(-HK31*HK49*HK52 - HK39*HK49*HK51 - HK43*HK49*HK50 - HK44*HK48*HK49 + HK45*HK49*HK55 + HK46*HK49*HK54 + HK47*HK49*HK53 + R_LOS);
const float HK57 = Tbs(0,0)*q3;
const float HK58 = Tbs(0,2)*q1;
const float HK59 = -HK58;
const float HK60 = ve*(HK57 + HK59);
const float HK61 = Tbs(0,0)*q2;
const float HK62 = Tbs(0,1)*q1;
const float HK63 = Tbs(0,1)*q3;
const float HK64 = Tbs(0,2)*q2;
const float HK65 = HK60 - vd*(HK61 - HK62) - vn*(HK63 - HK64);
const float HK66 = Tbs(0,2)*q0;
const float HK67 = -HK61;
const float HK68 = 2*HK62;
const float HK69 = Tbs(0,1)*q0;
const float HK70 = Tbs(0,1)*q2;
const float HK71 = Tbs(0,2)*q3;
const float HK72 = vd*(HK57 - 2*HK58 + HK69) + vn*(HK70 + HK71);
const float HK73 = HK72 - ve*(HK66 + HK67 + HK68);
const float HK74 = Tbs(0,0)*q0;
const float HK75 = -HK63;
const float HK76 = 2*HK64;
const float HK77 = Tbs(0,0)*q1;
const float HK78 = ve*(HK71 + HK77) + vn*(-2*HK61 + HK62 + HK66);
const float HK79 = HK78 - vd*(HK74 + HK75 + HK76);
const float HK80 = 2*HK57;
const float HK81 = vd*(HK70 + HK77) + ve*(-2*HK63 + HK64 + HK74);
const float HK82 = HK81 - vn*(HK59 + HK69 + HK80);
const float HK83 = 2*Tbs(0,2);
const float HK84 = HK21*HK83;
const float HK85 = 2*Tbs(0,1);
const float HK86 = -HK25*HK85 - HK30*Tbs(0,0) + HK84;
const float HK87 = 2*Tbs(0,0);
const float HK88 = HK32*HK87;
const float HK89 = -HK36*HK83 - HK38*Tbs(0,1) + HK88;
const float HK90 = HK40*HK85;
const float HK91 = -HK41*HK87 - HK42*Tbs(0,2) + HK90;
const float HK92 = 2*HK60 + 2*vd*(HK62 + HK67) + 2*vn*(HK64 + HK75);
const float HK93 = -HK27;
const float HK94 = 1 - HK28;
const float HK95 = HK84 + HK85*(-HK23 + HK24) + Tbs(0,0)*(HK93 + HK94);
const float HK96 = -HK37;
const float HK97 = HK83*(-HK34 + HK35) + HK88 + Tbs(0,1)*(HK94 + HK96);
const float HK98 = HK87*(-HK19 + HK20) + HK90 + Tbs(0,2)*(HK93 + HK96 + 1);
const float HK99 = 2*HK72 + 2*ve*(HK61 - HK66 - HK68);
const float HK100 = 2*HK78 + 2*vd*(HK63 - HK74 - HK76);
const float HK101 = 2*HK81 + 2*vn*(HK58 - HK69 - HK80);
const float HK102 = HK100*P(0,2) + HK101*P(0,3) + HK92*P(0,0) + HK95*P(0,4) + HK97*P(0,5) + HK98*P(0,6) + HK99*P(0,1);
const float HK103 = 2*HK49;
const float HK104 = HK100*P(2,6) + HK101*P(3,6) + HK92*P(0,6) + HK95*P(4,6) + HK97*P(5,6) + HK98*P(6,6) + HK99*P(1,6);
const float HK105 = HK100*P(2,5) + HK101*P(3,5) + HK92*P(0,5) + HK95*P(4,5) + HK97*P(5,5) + HK98*P(5,6) + HK99*P(1,5);
const float HK106 = HK100*P(2,4) + HK101*P(3,4) + HK92*P(0,4) + HK95*P(4,4) + HK97*P(4,5) + HK98*P(4,6) + HK99*P(1,4);
const float HK107 = HK100*P(2,3) + HK101*P(3,3) + HK92*P(0,3) + HK95*P(3,4) + HK97*P(3,5) + HK98*P(3,6) + HK99*P(1,3);
const float HK108 = HK100*P(2,2) + HK101*P(2,3) + HK92*P(0,2) + HK95*P(2,4) + HK97*P(2,5) + HK98*P(2,6) + HK99*P(1,2);
const float HK109 = HK100*P(1,2) + HK101*P(1,3) + HK92*P(0,1) + HK95*P(1,4) + HK97*P(1,5) + HK98*P(1,6) + HK99*P(1,1);
const float HK110 = HK8/(HK102*HK103*HK65 + HK103*HK107*HK82 + HK103*HK108*HK79 + HK103*HK109*HK73 + HK104*HK49*HK91 + HK105*HK49*HK89 + HK106*HK49*HK86 + R_LOS);
// Observation Jacobians - axis 0
Hfusion.at<0>() = -HK7*HK9;
Hfusion.at<1>() = HK14*HK9;
Hfusion.at<2>() = HK17*HK9;
Hfusion.at<3>() = HK18*HK9;
Hfusion.at<4>() = -HK31*HK8;
Hfusion.at<5>() = -HK39*HK8;
Hfusion.at<6>() = -HK43*HK8;
Hfusion.at<7>() = 0;
Hfusion.at<8>() = 0;
Hfusion.at<9>() = 0;
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) = HK48*HK56;
Kfusion(1) = HK55*HK56;
Kfusion(2) = HK54*HK56;
Kfusion(3) = HK53*HK56;
Kfusion(4) = HK52*HK56;
Kfusion(5) = HK51*HK56;
Kfusion(6) = HK50*HK56;
Kfusion(7) = HK56*(-HK31*P(4,7) - HK39*P(5,7) - HK43*P(6,7) - HK44*P(0,7) + HK45*P(1,7) + HK46*P(2,7) + HK47*P(3,7));
Kfusion(8) = HK56*(-HK31*P(4,8) - HK39*P(5,8) - HK43*P(6,8) - HK44*P(0,8) + HK45*P(1,8) + HK46*P(2,8) + HK47*P(3,8));
Kfusion(9) = HK56*(-HK31*P(4,9) - HK39*P(5,9) - HK43*P(6,9) - HK44*P(0,9) + HK45*P(1,9) + HK46*P(2,9) + HK47*P(3,9));
Kfusion(10) = HK56*(-HK31*P(4,10) - HK39*P(5,10) - HK43*P(6,10) - HK44*P(0,10) + HK45*P(1,10) + HK46*P(2,10) + HK47*P(3,10));
Kfusion(11) = HK56*(-HK31*P(4,11) - HK39*P(5,11) - HK43*P(6,11) - HK44*P(0,11) + HK45*P(1,11) + HK46*P(2,11) + HK47*P(3,11));
Kfusion(12) = HK56*(-HK31*P(4,12) - HK39*P(5,12) - HK43*P(6,12) - HK44*P(0,12) + HK45*P(1,12) + HK46*P(2,12) + HK47*P(3,12));
Kfusion(13) = HK56*(-HK31*P(4,13) - HK39*P(5,13) - HK43*P(6,13) - HK44*P(0,13) + HK45*P(1,13) + HK46*P(2,13) + HK47*P(3,13));
Kfusion(14) = HK56*(-HK31*P(4,14) - HK39*P(5,14) - HK43*P(6,14) - HK44*P(0,14) + HK45*P(1,14) + HK46*P(2,14) + HK47*P(3,14));
Kfusion(15) = HK56*(-HK31*P(4,15) - HK39*P(5,15) - HK43*P(6,15) - HK44*P(0,15) + HK45*P(1,15) + HK46*P(2,15) + HK47*P(3,15));
Kfusion(16) = HK56*(-HK31*P(4,16) - HK39*P(5,16) - HK43*P(6,16) - HK44*P(0,16) + HK45*P(1,16) + HK46*P(2,16) + HK47*P(3,16));
Kfusion(17) = HK56*(-HK31*P(4,17) - HK39*P(5,17) - HK43*P(6,17) - HK44*P(0,17) + HK45*P(1,17) + HK46*P(2,17) + HK47*P(3,17));
Kfusion(18) = HK56*(-HK31*P(4,18) - HK39*P(5,18) - HK43*P(6,18) - HK44*P(0,18) + HK45*P(1,18) + HK46*P(2,18) + HK47*P(3,18));
Kfusion(19) = HK56*(-HK31*P(4,19) - HK39*P(5,19) - HK43*P(6,19) - HK44*P(0,19) + HK45*P(1,19) + HK46*P(2,19) + HK47*P(3,19));
Kfusion(20) = HK56*(-HK31*P(4,20) - HK39*P(5,20) - HK43*P(6,20) - HK44*P(0,20) + HK45*P(1,20) + HK46*P(2,20) + HK47*P(3,20));
Kfusion(21) = HK56*(-HK31*P(4,21) - HK39*P(5,21) - HK43*P(6,21) - HK44*P(0,21) + HK45*P(1,21) + HK46*P(2,21) + HK47*P(3,21));
Kfusion(22) = HK56*(-HK31*P(4,22) - HK39*P(5,22) - HK43*P(6,22) - HK44*P(0,22) + HK45*P(1,22) + HK46*P(2,22) + HK47*P(3,22));
Kfusion(23) = HK56*(-HK31*P(4,23) - HK39*P(5,23) - HK43*P(6,23) - HK44*P(0,23) + HK45*P(1,23) + HK46*P(2,23) + HK47*P(3,23));
// Observation Jacobians - axis 1
Hfusion.at<0>() = -HK65*HK9;
Hfusion.at<1>() = -HK73*HK9;
Hfusion.at<2>() = -HK79*HK9;
Hfusion.at<3>() = -HK82*HK9;
Hfusion.at<4>() = -HK8*HK86;
Hfusion.at<5>() = -HK8*HK89;
Hfusion.at<6>() = -HK8*HK91;
Hfusion.at<7>() = 0;
Hfusion.at<8>() = 0;
Hfusion.at<9>() = 0;
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) = -HK102*HK110;
Kfusion(1) = -HK109*HK110;
Kfusion(2) = -HK108*HK110;
Kfusion(3) = -HK107*HK110;
Kfusion(4) = -HK106*HK110;
Kfusion(5) = -HK105*HK110;
Kfusion(6) = -HK104*HK110;
Kfusion(7) = -HK110*(HK100*P(2,7) + HK101*P(3,7) + HK92*P(0,7) + HK95*P(4,7) + HK97*P(5,7) + HK98*P(6,7) + HK99*P(1,7));
Kfusion(8) = -HK110*(HK100*P(2,8) + HK101*P(3,8) + HK92*P(0,8) + HK95*P(4,8) + HK97*P(5,8) + HK98*P(6,8) + HK99*P(1,8));
Kfusion(9) = -HK110*(HK100*P(2,9) + HK101*P(3,9) + HK92*P(0,9) + HK95*P(4,9) + HK97*P(5,9) + HK98*P(6,9) + HK99*P(1,9));
Kfusion(10) = -HK110*(HK100*P(2,10) + HK101*P(3,10) + HK92*P(0,10) + HK95*P(4,10) + HK97*P(5,10) + HK98*P(6,10) + HK99*P(1,10));
Kfusion(11) = -HK110*(HK100*P(2,11) + HK101*P(3,11) + HK92*P(0,11) + HK95*P(4,11) + HK97*P(5,11) + HK98*P(6,11) + HK99*P(1,11));
Kfusion(12) = -HK110*(HK100*P(2,12) + HK101*P(3,12) + HK92*P(0,12) + HK95*P(4,12) + HK97*P(5,12) + HK98*P(6,12) + HK99*P(1,12));
Kfusion(13) = -HK110*(HK100*P(2,13) + HK101*P(3,13) + HK92*P(0,13) + HK95*P(4,13) + HK97*P(5,13) + HK98*P(6,13) + HK99*P(1,13));
Kfusion(14) = -HK110*(HK100*P(2,14) + HK101*P(3,14) + HK92*P(0,14) + HK95*P(4,14) + HK97*P(5,14) + HK98*P(6,14) + HK99*P(1,14));
Kfusion(15) = -HK110*(HK100*P(2,15) + HK101*P(3,15) + HK92*P(0,15) + HK95*P(4,15) + HK97*P(5,15) + HK98*P(6,15) + HK99*P(1,15));
Kfusion(16) = -HK110*(HK100*P(2,16) + HK101*P(3,16) + HK92*P(0,16) + HK95*P(4,16) + HK97*P(5,16) + HK98*P(6,16) + HK99*P(1,16));
Kfusion(17) = -HK110*(HK100*P(2,17) + HK101*P(3,17) + HK92*P(0,17) + HK95*P(4,17) + HK97*P(5,17) + HK98*P(6,17) + HK99*P(1,17));
Kfusion(18) = -HK110*(HK100*P(2,18) + HK101*P(3,18) + HK92*P(0,18) + HK95*P(4,18) + HK97*P(5,18) + HK98*P(6,18) + HK99*P(1,18));
Kfusion(19) = -HK110*(HK100*P(2,19) + HK101*P(3,19) + HK92*P(0,19) + HK95*P(4,19) + HK97*P(5,19) + HK98*P(6,19) + HK99*P(1,19));
Kfusion(20) = -HK110*(HK100*P(2,20) + HK101*P(3,20) + HK92*P(0,20) + HK95*P(4,20) + HK97*P(5,20) + HK98*P(6,20) + HK99*P(1,20));
Kfusion(21) = -HK110*(HK100*P(2,21) + HK101*P(3,21) + HK92*P(0,21) + HK95*P(4,21) + HK97*P(5,21) + HK98*P(6,21) + HK99*P(1,21));
Kfusion(22) = -HK110*(HK100*P(2,22) + HK101*P(3,22) + HK92*P(0,22) + HK95*P(4,22) + HK97*P(5,22) + HK98*P(6,22) + HK99*P(1,22));
Kfusion(23) = -HK110*(HK100*P(2,23) + HK101*P(3,23) + HK92*P(0,23) + HK95*P(4,23) + HK97*P(5,23) + HK98*P(6,23) + HK99*P(1,23));
Reference in New Issue View Git Blame Copy Permalink