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

231 lines
11 KiB
C++
Raw Blame History

// X Axis Equations
// 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 = 2*Tbs(1,2);
const float HK22 = q0*q3;
const float HK23 = q1*q2;
const float HK24 = 2*Tbs(1,1);
const float HK25 = 2*powf(q2, 2);
const float HK26 = 2*powf(q3, 2) - 1;
const float HK27 = -HK21*(HK19 + HK20) + HK24*(HK22 - HK23) + Tbs(1,0)*(HK25 + HK26);
const float HK28 = 2*Tbs(1,0);
const float HK29 = q0*q1;
const float HK30 = q2*q3;
const float HK31 = 2*powf(q1, 2);
const float HK32 = HK21*(HK29 - HK30) - HK28*(HK22 + HK23) + Tbs(1,1)*(HK26 + HK31);
const float HK33 = -HK24*(HK29 + HK30) + HK28*(HK19 - HK20) + Tbs(1,2)*(HK25 + HK31 - 1);
const float HK34 = 2*HK7;
const float HK35 = 2*HK14;
const float HK36 = 2*HK17;
const float HK37 = 2*HK18;
const float HK38 = -HK27*P(0,4) - HK32*P(0,5) - HK33*P(0,6) - HK34*P(0,0) + HK35*P(0,1) + HK36*P(0,2) + HK37*P(0,3);
const float HK39 = powf(range, -2);
const float HK40 = -HK27*P(4,6) - HK32*P(5,6) - HK33*P(6,6) - HK34*P(0,6) + HK35*P(1,6) + HK36*P(2,6) + HK37*P(3,6);
const float HK41 = -HK27*P(4,5) - HK32*P(5,5) - HK33*P(5,6) - HK34*P(0,5) + HK35*P(1,5) + HK36*P(2,5) + HK37*P(3,5);
const float HK42 = -HK27*P(4,4) - HK32*P(4,5) - HK33*P(4,6) - HK34*P(0,4) + HK35*P(1,4) + HK36*P(2,4) + HK37*P(3,4);
const float HK43 = -HK27*P(3,4) - HK32*P(3,5) - HK33*P(3,6) - HK34*P(0,3) + HK35*P(1,3) + HK36*P(2,3) + HK37*P(3,3);
const float HK44 = -HK27*P(2,4) - HK32*P(2,5) - HK33*P(2,6) - HK34*P(0,2) + HK35*P(1,2) + HK36*P(2,2) + HK37*P(2,3);
const float HK45 = -HK27*P(1,4) - HK32*P(1,5) - HK33*P(1,6) - HK34*P(0,1) + HK35*P(1,1) + HK36*P(1,2) + HK37*P(1,3);
const float HK46 = HK8/(-HK27*HK39*HK42 - HK32*HK39*HK41 - HK33*HK39*HK40 - HK34*HK38*HK39 + HK35*HK39*HK45 + HK36*HK39*HK44 + HK37*HK39*HK43 + R_LOS);
// Observation Jacobians
Hfusion.at<0>() = -HK7*HK9;
Hfusion.at<1>() = HK14*HK9;
Hfusion.at<2>() = HK17*HK9;
Hfusion.at<3>() = HK18*HK9;
Hfusion.at<4>() = -HK27*HK8;
Hfusion.at<5>() = -HK32*HK8;
Hfusion.at<6>() = -HK33*HK8;
Hfusion.at<7>() = 0;
Hfusion.at<8>() = 0;
Hfusion.at<9>() = 0;
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) = HK38*HK46;
Kfusion(1) = HK45*HK46;
Kfusion(2) = HK44*HK46;
Kfusion(3) = HK43*HK46;
Kfusion(4) = HK42*HK46;
Kfusion(5) = HK41*HK46;
Kfusion(6) = HK40*HK46;
Kfusion(7) = HK46*(-HK27*P(4,7) - HK32*P(5,7) - HK33*P(6,7) - HK34*P(0,7) + HK35*P(1,7) + HK36*P(2,7) + HK37*P(3,7));
Kfusion(8) = HK46*(-HK27*P(4,8) - HK32*P(5,8) - HK33*P(6,8) - HK34*P(0,8) + HK35*P(1,8) + HK36*P(2,8) + HK37*P(3,8));
Kfusion(9) = HK46*(-HK27*P(4,9) - HK32*P(5,9) - HK33*P(6,9) - HK34*P(0,9) + HK35*P(1,9) + HK36*P(2,9) + HK37*P(3,9));
Kfusion(10) = HK46*(-HK27*P(4,10) - HK32*P(5,10) - HK33*P(6,10) - HK34*P(0,10) + HK35*P(1,10) + HK36*P(2,10) + HK37*P(3,10));
Kfusion(11) = HK46*(-HK27*P(4,11) - HK32*P(5,11) - HK33*P(6,11) - HK34*P(0,11) + HK35*P(1,11) + HK36*P(2,11) + HK37*P(3,11));
Kfusion(12) = HK46*(-HK27*P(4,12) - HK32*P(5,12) - HK33*P(6,12) - HK34*P(0,12) + HK35*P(1,12) + HK36*P(2,12) + HK37*P(3,12));
Kfusion(13) = HK46*(-HK27*P(4,13) - HK32*P(5,13) - HK33*P(6,13) - HK34*P(0,13) + HK35*P(1,13) + HK36*P(2,13) + HK37*P(3,13));
Kfusion(14) = HK46*(-HK27*P(4,14) - HK32*P(5,14) - HK33*P(6,14) - HK34*P(0,14) + HK35*P(1,14) + HK36*P(2,14) + HK37*P(3,14));
Kfusion(15) = HK46*(-HK27*P(4,15) - HK32*P(5,15) - HK33*P(6,15) - HK34*P(0,15) + HK35*P(1,15) + HK36*P(2,15) + HK37*P(3,15));
Kfusion(16) = HK46*(-HK27*P(4,16) - HK32*P(5,16) - HK33*P(6,16) - HK34*P(0,16) + HK35*P(1,16) + HK36*P(2,16) + HK37*P(3,16));
Kfusion(17) = HK46*(-HK27*P(4,17) - HK32*P(5,17) - HK33*P(6,17) - HK34*P(0,17) + HK35*P(1,17) + HK36*P(2,17) + HK37*P(3,17));
Kfusion(18) = HK46*(-HK27*P(4,18) - HK32*P(5,18) - HK33*P(6,18) - HK34*P(0,18) + HK35*P(1,18) + HK36*P(2,18) + HK37*P(3,18));
Kfusion(19) = HK46*(-HK27*P(4,19) - HK32*P(5,19) - HK33*P(6,19) - HK34*P(0,19) + HK35*P(1,19) + HK36*P(2,19) + HK37*P(3,19));
Kfusion(20) = HK46*(-HK27*P(4,20) - HK32*P(5,20) - HK33*P(6,20) - HK34*P(0,20) + HK35*P(1,20) + HK36*P(2,20) + HK37*P(3,20));
Kfusion(21) = HK46*(-HK27*P(4,21) - HK32*P(5,21) - HK33*P(6,21) - HK34*P(0,21) + HK35*P(1,21) + HK36*P(2,21) + HK37*P(3,21));
Kfusion(22) = HK46*(-HK27*P(4,22) - HK32*P(5,22) - HK33*P(6,22) - HK34*P(0,22) + HK35*P(1,22) + HK36*P(2,22) + HK37*P(3,22));
Kfusion(23) = HK46*(-HK27*P(4,23) - HK32*P(5,23) - HK33*P(6,23) - HK34*P(0,23) + HK35*P(1,23) + HK36*P(2,23) + HK37*P(3,23));
// Y Axis Equations
// Sub Expressions
const float HK0 = Tbs(0,0)*q3;
const float HK1 = Tbs(0,2)*q1;
const float HK2 = -HK1;
const float HK3 = ve*(HK0 + HK2);
const float HK4 = Tbs(0,0)*q2;
const float HK5 = Tbs(0,1)*q1;
const float HK6 = Tbs(0,1)*q3;
const float HK7 = Tbs(0,2)*q2;
const float HK8 = HK3 - vd*(HK4 - HK5) - vn*(HK6 - HK7);
const float HK9 = 1.0F/range;
const float HK10 = 2*HK9;
const float HK11 = Tbs(0,2)*q0;
const float HK12 = -HK4;
const float HK13 = 2*HK5;
const float HK14 = Tbs(0,1)*q0;
const float HK15 = Tbs(0,1)*q2;
const float HK16 = Tbs(0,2)*q3;
const float HK17 = vd*(HK0 - 2*HK1 + HK14) + vn*(HK15 + HK16);
const float HK18 = HK17 - ve*(HK11 + HK12 + HK13);
const float HK19 = Tbs(0,0)*q0;
const float HK20 = -HK6;
const float HK21 = 2*HK7;
const float HK22 = Tbs(0,0)*q1;
const float HK23 = ve*(HK16 + HK22) + vn*(HK11 - 2*HK4 + HK5);
const float HK24 = HK23 - vd*(HK19 + HK20 + HK21);
const float HK25 = 2*HK0;
const float HK26 = vd*(HK15 + HK22) + ve*(HK19 - 2*HK6 + HK7);
const float HK27 = HK26 - vn*(HK14 + HK2 + HK25);
const float HK28 = q0*q2;
const float HK29 = q1*q3;
const float HK30 = 2*Tbs(0,2);
const float HK31 = HK30*(HK28 + HK29);
const float HK32 = q0*q3;
const float HK33 = q1*q2;
const float HK34 = 2*Tbs(0,1);
const float HK35 = 2*powf(q2, 2);
const float HK36 = 2*powf(q3, 2);
const float HK37 = HK36 - 1;
const float HK38 = HK31 - HK34*(HK32 - HK33) - Tbs(0,0)*(HK35 + HK37);
const float HK39 = 2*Tbs(0,0);
const float HK40 = HK39*(HK32 + HK33);
const float HK41 = q0*q1;
const float HK42 = q2*q3;
const float HK43 = 2*powf(q1, 2);
const float HK44 = -HK30*(HK41 - HK42) + HK40 - Tbs(0,1)*(HK37 + HK43);
const float HK45 = HK34*(HK41 + HK42);
const float HK46 = -HK39*(HK28 - HK29) + HK45 - Tbs(0,2)*(HK35 + HK43 - 1);
const float HK47 = 2*HK3 + 2*vd*(HK12 + HK5) + 2*vn*(HK20 + HK7);
const float HK48 = -HK35;
const float HK49 = 1 - HK36;
const float HK50 = HK31 + HK34*(-HK32 + HK33) + Tbs(0,0)*(HK48 + HK49);
const float HK51 = -HK43;
const float HK52 = HK30*(-HK41 + HK42) + HK40 + Tbs(0,1)*(HK49 + HK51);
const float HK53 = HK39*(-HK28 + HK29) + HK45 + Tbs(0,2)*(HK48 + HK51 + 1);
const float HK54 = 2*HK17 + 2*ve*(-HK11 - HK13 + HK4);
const float HK55 = 2*HK23 + 2*vd*(-HK19 - HK21 + HK6);
const float HK56 = 2*HK26 + 2*vn*(HK1 - HK14 - HK25);
const float HK57 = HK47*P(0,0) + HK50*P(0,4) + HK52*P(0,5) + HK53*P(0,6) + HK54*P(0,1) + HK55*P(0,2) + HK56*P(0,3);
const float HK58 = powf(range, -2);
const float HK59 = 2*HK58;
const float HK60 = HK47*P(0,6) + HK50*P(4,6) + HK52*P(5,6) + HK53*P(6,6) + HK54*P(1,6) + HK55*P(2,6) + HK56*P(3,6);
const float HK61 = HK47*P(0,5) + HK50*P(4,5) + HK52*P(5,5) + HK53*P(5,6) + HK54*P(1,5) + HK55*P(2,5) + HK56*P(3,5);
const float HK62 = HK47*P(0,4) + HK50*P(4,4) + HK52*P(4,5) + HK53*P(4,6) + HK54*P(1,4) + HK55*P(2,4) + HK56*P(3,4);
const float HK63 = HK47*P(0,3) + HK50*P(3,4) + HK52*P(3,5) + HK53*P(3,6) + HK54*P(1,3) + HK55*P(2,3) + HK56*P(3,3);
const float HK64 = HK47*P(0,2) + HK50*P(2,4) + HK52*P(2,5) + HK53*P(2,6) + HK54*P(1,2) + HK55*P(2,2) + HK56*P(2,3);
const float HK65 = HK47*P(0,1) + HK50*P(1,4) + HK52*P(1,5) + HK53*P(1,6) + HK54*P(1,1) + HK55*P(1,2) + HK56*P(1,3);
const float HK66 = HK9/(HK18*HK59*HK65 + HK24*HK59*HK64 + HK27*HK59*HK63 + HK38*HK58*HK62 + HK44*HK58*HK61 + HK46*HK58*HK60 + HK57*HK59*HK8 + R_LOS);
// Observation Jacobians
Hfusion.at<0>() = -HK10*HK8;
Hfusion.at<1>() = -HK10*HK18;
Hfusion.at<2>() = -HK10*HK24;
Hfusion.at<3>() = -HK10*HK27;
Hfusion.at<4>() = -HK38*HK9;
Hfusion.at<5>() = -HK44*HK9;
Hfusion.at<6>() = -HK46*HK9;
Hfusion.at<7>() = 0;
Hfusion.at<8>() = 0;
Hfusion.at<9>() = 0;
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) = -HK57*HK66;
Kfusion(1) = -HK65*HK66;
Kfusion(2) = -HK64*HK66;
Kfusion(3) = -HK63*HK66;
Kfusion(4) = -HK62*HK66;
Kfusion(5) = -HK61*HK66;
Kfusion(6) = -HK60*HK66;
Kfusion(7) = -HK66*(HK47*P(0,7) + HK50*P(4,7) + HK52*P(5,7) + HK53*P(6,7) + HK54*P(1,7) + HK55*P(2,7) + HK56*P(3,7));
Kfusion(8) = -HK66*(HK47*P(0,8) + HK50*P(4,8) + HK52*P(5,8) + HK53*P(6,8) + HK54*P(1,8) + HK55*P(2,8) + HK56*P(3,8));
Kfusion(9) = -HK66*(HK47*P(0,9) + HK50*P(4,9) + HK52*P(5,9) + HK53*P(6,9) + HK54*P(1,9) + HK55*P(2,9) + HK56*P(3,9));
Kfusion(10) = -HK66*(HK47*P(0,10) + HK50*P(4,10) + HK52*P(5,10) + HK53*P(6,10) + HK54*P(1,10) + HK55*P(2,10) + HK56*P(3,10));
Kfusion(11) = -HK66*(HK47*P(0,11) + HK50*P(4,11) + HK52*P(5,11) + HK53*P(6,11) + HK54*P(1,11) + HK55*P(2,11) + HK56*P(3,11));
Kfusion(12) = -HK66*(HK47*P(0,12) + HK50*P(4,12) + HK52*P(5,12) + HK53*P(6,12) + HK54*P(1,12) + HK55*P(2,12) + HK56*P(3,12));
Kfusion(13) = -HK66*(HK47*P(0,13) + HK50*P(4,13) + HK52*P(5,13) + HK53*P(6,13) + HK54*P(1,13) + HK55*P(2,13) + HK56*P(3,13));
Kfusion(14) = -HK66*(HK47*P(0,14) + HK50*P(4,14) + HK52*P(5,14) + HK53*P(6,14) + HK54*P(1,14) + HK55*P(2,14) + HK56*P(3,14));
Kfusion(15) = -HK66*(HK47*P(0,15) + HK50*P(4,15) + HK52*P(5,15) + HK53*P(6,15) + HK54*P(1,15) + HK55*P(2,15) + HK56*P(3,15));
Kfusion(16) = -HK66*(HK47*P(0,16) + HK50*P(4,16) + HK52*P(5,16) + HK53*P(6,16) + HK54*P(1,16) + HK55*P(2,16) + HK56*P(3,16));
Kfusion(17) = -HK66*(HK47*P(0,17) + HK50*P(4,17) + HK52*P(5,17) + HK53*P(6,17) + HK54*P(1,17) + HK55*P(2,17) + HK56*P(3,17));
Kfusion(18) = -HK66*(HK47*P(0,18) + HK50*P(4,18) + HK52*P(5,18) + HK53*P(6,18) + HK54*P(1,18) + HK55*P(2,18) + HK56*P(3,18));
Kfusion(19) = -HK66*(HK47*P(0,19) + HK50*P(4,19) + HK52*P(5,19) + HK53*P(6,19) + HK54*P(1,19) + HK55*P(2,19) + HK56*P(3,19));
Kfusion(20) = -HK66*(HK47*P(0,20) + HK50*P(4,20) + HK52*P(5,20) + HK53*P(6,20) + HK54*P(1,20) + HK55*P(2,20) + HK56*P(3,20));
Kfusion(21) = -HK66*(HK47*P(0,21) + HK50*P(4,21) + HK52*P(5,21) + HK53*P(6,21) + HK54*P(1,21) + HK55*P(2,21) + HK56*P(3,21));
Kfusion(22) = -HK66*(HK47*P(0,22) + HK50*P(4,22) + HK52*P(5,22) + HK53*P(6,22) + HK54*P(1,22) + HK55*P(2,22) + HK56*P(3,22));
Kfusion(23) = -HK66*(HK47*P(0,23) + HK50*P(4,23) + HK52*P(5,23) + HK53*P(6,23) + HK54*P(1,23) + HK55*P(2,23) + HK56*P(3,23));
Reference in New Issue View Git Blame Copy Permalink