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

196 lines
11 KiB
C++
Raw Blame History

// Axis 0 equations
// Sub Expressions
const float HK0 = q2*vd;
const float HK1 = ve - vwe;
const float HK2 = HK1*q3;
const float HK3 = HK0 - HK2;
const float HK4 = 2*Kaccx;
const float HK5 = HK1*q2 + q3*vd;
const float HK6 = q0*vd;
const float HK7 = HK1*q1;
const float HK8 = 2*vn - 2*vwn;
const float HK9 = HK8*q2;
const float HK10 = HK6 - HK7 + HK9;
const float HK11 = HK1*q0 - HK8*q3 + q1*vd;
const float HK12 = 2*powf(q2, 2);
const float HK13 = 2*powf(q3, 2);
const float HK14 = HK12 + HK13 - 1;
const float HK15 = HK14*Kaccx;
const float HK16 = q0*q3 + q1*q2;
const float HK17 = HK16*HK4;
const float HK18 = q0*q2;
const float HK19 = q1*q3;
const float HK20 = HK18 - HK19;
const float HK21 = 2*HK3;
const float HK22 = 2*HK10;
const float HK23 = 2*HK20;
const float HK24 = 2*HK16;
const float HK25 = 2*HK5;
const float HK26 = 2*HK11;
const float HK27 = HK14*P(0,22) - HK24*P(0,23) + HK24*P(0,5) + HK25*P(0,1) + HK26*P(0,3);
const float HK28 = -HK12 - HK13 + 1;
const float HK29 = -2*HK0 + 2*HK2;
const float HK30 = -2*HK6 + 2*HK7 - 2*HK9;
const float HK31 = -2*HK18 + 2*HK19;
const float HK32 = HK24*P(5,23);
const float HK33 = HK14*P(22,23) - HK24*P(23,23) + HK25*P(1,23) + HK26*P(3,23) + HK32;
const float HK34 = powf(Kaccx, 2);
const float HK35 = HK24*HK34;
const float HK36 = HK14*P(5,22) + HK24*P(5,5) + HK25*P(1,5) + HK26*P(3,5) - HK32;
const float HK37 = HK14*P(6,22) + HK24*P(5,6) - HK24*P(6,23) + HK25*P(1,6) + HK26*P(3,6);
const float HK38 = HK14*P(1,22) - HK24*P(1,23) + HK24*P(1,5) + HK25*P(1,1) + HK26*P(1,3);
const float HK39 = HK14*P(4,22);
const float HK40 = -HK24*P(4,23) + HK24*P(4,5) + HK25*P(1,4) + HK26*P(3,4) + HK39;
const float HK41 = HK14*HK34;
const float HK42 = HK14*P(22,22) - HK24*P(22,23) + HK24*P(5,22) + HK25*P(1,22) + HK26*P(3,22);
const float HK43 = HK14*P(3,22) - HK24*P(3,23) + HK24*P(3,5) + HK25*P(1,3) + HK26*P(3,3);
const float HK44 = HK14*P(2,22) - HK24*P(2,23) + HK24*P(2,5) + HK25*P(1,2) + HK26*P(2,3);
const float HK45 = Kaccx/(HK21*HK34*(HK27 + HK28*P(0,4) + HK29*P(0,0) + HK30*P(0,2) + HK31*P(0,6)) + HK22*HK34*(HK28*P(2,4) + HK29*P(0,2) + HK30*P(2,2) + HK31*P(2,6) + HK44) + HK23*HK34*(HK28*P(4,6) + HK29*P(0,6) + HK30*P(2,6) + HK31*P(6,6) + HK37) - HK25*HK34*(HK28*P(1,4) + HK29*P(0,1) + HK30*P(1,2) + HK31*P(1,6) + HK38) - HK26*HK34*(HK28*P(3,4) + HK29*P(0,3) + HK30*P(2,3) + HK31*P(3,6) + HK43) + HK35*(HK28*P(4,23) + HK29*P(0,23) + HK30*P(2,23) + HK31*P(6,23) + HK33) - HK35*(HK28*P(4,5) + HK29*P(0,5) + HK30*P(2,5) + HK31*P(5,6) + HK36) - HK41*(HK28*P(4,22) + HK29*P(0,22) + HK30*P(2,22) + HK31*P(6,22) + HK42) + HK41*(HK28*P(4,4) + HK29*P(0,4) + HK30*P(2,4) + HK31*P(4,6) + HK40) - R_ACC);
// Observation Jacobians
Hfusion.at<0>() = HK3*HK4;
Hfusion.at<1>() = -HK4*HK5;
Hfusion.at<2>() = HK10*HK4;
Hfusion.at<3>() = -HK11*HK4;
Hfusion.at<4>() = HK15;
Hfusion.at<5>() = -HK17;
Hfusion.at<6>() = HK20*HK4;
Hfusion.at<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>() = -HK15;
Hfusion.at<23>() = HK17;
// Kalman gains
Kfusion(0) = HK45*(-HK14*P(0,4) - HK21*P(0,0) - HK22*P(0,2) - HK23*P(0,6) + HK27);
Kfusion(1) = HK45*(-HK14*P(1,4) - HK21*P(0,1) - HK22*P(1,2) - HK23*P(1,6) + HK38);
Kfusion(2) = HK45*(-HK14*P(2,4) - HK21*P(0,2) - HK22*P(2,2) - HK23*P(2,6) + HK44);
Kfusion(3) = HK45*(-HK14*P(3,4) - HK21*P(0,3) - HK22*P(2,3) - HK23*P(3,6) + HK43);
Kfusion(4) = HK45*(-HK14*P(4,4) - HK21*P(0,4) - HK22*P(2,4) - HK23*P(4,6) + HK40);
Kfusion(5) = HK45*(-HK14*P(4,5) - HK21*P(0,5) - HK22*P(2,5) - HK23*P(5,6) + HK36);
Kfusion(6) = HK45*(-HK14*P(4,6) - HK21*P(0,6) - HK22*P(2,6) - HK23*P(6,6) + HK37);
Kfusion(7) = HK45*(-HK14*P(4,7) + HK14*P(7,22) - HK21*P(0,7) - HK22*P(2,7) - HK23*P(6,7) + HK24*P(5,7) - HK24*P(7,23) + HK25*P(1,7) + HK26*P(3,7));
Kfusion(8) = HK45*(-HK14*P(4,8) + HK14*P(8,22) - HK21*P(0,8) - HK22*P(2,8) - HK23*P(6,8) + HK24*P(5,8) - HK24*P(8,23) + HK25*P(1,8) + HK26*P(3,8));
Kfusion(9) = HK45*(-HK14*P(4,9) + HK14*P(9,22) - HK21*P(0,9) - HK22*P(2,9) - HK23*P(6,9) + HK24*P(5,9) - HK24*P(9,23) + HK25*P(1,9) + HK26*P(3,9));
Kfusion(10) = HK45*(HK14*P(10,22) - HK14*P(4,10) - HK21*P(0,10) - HK22*P(2,10) - HK23*P(6,10) - HK24*P(10,23) + HK24*P(5,10) + HK25*P(1,10) + HK26*P(3,10));
Kfusion(11) = HK45*(HK14*P(11,22) - HK14*P(4,11) - HK21*P(0,11) - HK22*P(2,11) - HK23*P(6,11) - HK24*P(11,23) + HK24*P(5,11) + HK25*P(1,11) + HK26*P(3,11));
Kfusion(12) = HK45*(HK14*P(12,22) - HK14*P(4,12) - HK21*P(0,12) - HK22*P(2,12) - HK23*P(6,12) - HK24*P(12,23) + HK24*P(5,12) + HK25*P(1,12) + HK26*P(3,12));
Kfusion(13) = HK45*(HK14*P(13,22) - HK14*P(4,13) - HK21*P(0,13) - HK22*P(2,13) - HK23*P(6,13) - HK24*P(13,23) + HK24*P(5,13) + HK25*P(1,13) + HK26*P(3,13));
Kfusion(14) = HK45*(HK14*P(14,22) - HK14*P(4,14) - HK21*P(0,14) - HK22*P(2,14) - HK23*P(6,14) - HK24*P(14,23) + HK24*P(5,14) + HK25*P(1,14) + HK26*P(3,14));
Kfusion(15) = HK45*(HK14*P(15,22) - HK14*P(4,15) - HK21*P(0,15) - HK22*P(2,15) - HK23*P(6,15) - HK24*P(15,23) + HK24*P(5,15) + HK25*P(1,15) + HK26*P(3,15));
Kfusion(16) = HK45*(HK14*P(16,22) - HK14*P(4,16) - HK21*P(0,16) - HK22*P(2,16) - HK23*P(6,16) - HK24*P(16,23) + HK24*P(5,16) + HK25*P(1,16) + HK26*P(3,16));
Kfusion(17) = HK45*(HK14*P(17,22) - HK14*P(4,17) - HK21*P(0,17) - HK22*P(2,17) - HK23*P(6,17) - HK24*P(17,23) + HK24*P(5,17) + HK25*P(1,17) + HK26*P(3,17));
Kfusion(18) = HK45*(HK14*P(18,22) - HK14*P(4,18) - HK21*P(0,18) - HK22*P(2,18) - HK23*P(6,18) - HK24*P(18,23) + HK24*P(5,18) + HK25*P(1,18) + HK26*P(3,18));
Kfusion(19) = HK45*(HK14*P(19,22) - HK14*P(4,19) - HK21*P(0,19) - HK22*P(2,19) - HK23*P(6,19) - HK24*P(19,23) + HK24*P(5,19) + HK25*P(1,19) + HK26*P(3,19));
Kfusion(20) = HK45*(HK14*P(20,22) - HK14*P(4,20) - HK21*P(0,20) - HK22*P(2,20) - HK23*P(6,20) - HK24*P(20,23) + HK24*P(5,20) + HK25*P(1,20) + HK26*P(3,20));
Kfusion(21) = HK45*(HK14*P(21,22) - HK14*P(4,21) - HK21*P(0,21) - HK22*P(2,21) - HK23*P(6,21) - HK24*P(21,23) + HK24*P(5,21) + HK25*P(1,21) + HK26*P(3,21));
Kfusion(22) = HK45*(-HK21*P(0,22) - HK22*P(2,22) - HK23*P(6,22) - HK39 + HK42);
Kfusion(23) = HK45*(-HK14*P(4,23) - HK21*P(0,23) - HK22*P(2,23) - HK23*P(6,23) + HK33);
// Axis 1 equations
// Sub Expressions
const float HK0 = vn - vwn;
const float HK1 = -HK0*q3 + q1*vd;
const float HK2 = 2*Kaccy;
const float HK3 = 2*ve - 2*vwe;
const float HK4 = HK0*q2 - HK3*q1 + q0*vd;
const float HK5 = HK0*q1 + q3*vd;
const float HK6 = HK0*q0 + HK3*q3 - q2*vd;
const float HK7 = q0*q3 - q1*q2;
const float HK8 = HK2*HK7;
const float HK9 = 2*powf(q1, 2) + 2*powf(q3, 2) - 1;
const float HK10 = HK9*Kaccy;
const float HK11 = q0*q1 + q2*q3;
const float HK12 = 2*HK11;
const float HK13 = 2*HK7;
const float HK14 = 2*HK5;
const float HK15 = 2*HK1;
const float HK16 = 2*HK4;
const float HK17 = 2*HK6;
const float HK18 = HK12*P(0,6) + HK13*P(0,22) - HK13*P(0,4) + HK14*P(0,2) + HK15*P(0,0) + HK16*P(0,1) - HK17*P(0,3) + HK9*P(0,23) - HK9*P(0,5);
const float HK19 = powf(Kaccy, 2);
const float HK20 = HK12*P(6,6) - HK13*P(4,6) + HK13*P(6,22) + HK14*P(2,6) + HK15*P(0,6) + HK16*P(1,6) - HK17*P(3,6) - HK9*P(5,6) + HK9*P(6,23);
const float HK21 = HK13*P(4,22);
const float HK22 = HK12*P(6,22) + HK13*P(22,22) + HK14*P(2,22) + HK15*P(0,22) + HK16*P(1,22) - HK17*P(3,22) - HK21 + HK9*P(22,23) - HK9*P(5,22);
const float HK23 = HK13*HK19;
const float HK24 = HK12*P(4,6) - HK13*P(4,4) + HK14*P(2,4) + HK15*P(0,4) + HK16*P(1,4) - HK17*P(3,4) + HK21 + HK9*P(4,23) - HK9*P(4,5);
const float HK25 = HK12*P(2,6) + HK13*P(2,22) - HK13*P(2,4) + HK14*P(2,2) + HK15*P(0,2) + HK16*P(1,2) - HK17*P(2,3) + HK9*P(2,23) - HK9*P(2,5);
const float HK26 = HK9*P(5,23);
const float HK27 = HK12*P(6,23) + HK13*P(22,23) - HK13*P(4,23) + HK14*P(2,23) + HK15*P(0,23) + HK16*P(1,23) - HK17*P(3,23) - HK26 + HK9*P(23,23);
const float HK28 = HK19*HK9;
const float HK29 = HK12*P(5,6) - HK13*P(4,5) + HK13*P(5,22) + HK14*P(2,5) + HK15*P(0,5) + HK16*P(1,5) - HK17*P(3,5) + HK26 - HK9*P(5,5);
const float HK30 = HK12*P(1,6) + HK13*P(1,22) - HK13*P(1,4) + HK14*P(1,2) + HK15*P(0,1) + HK16*P(1,1) - HK17*P(1,3) + HK9*P(1,23) - HK9*P(1,5);
const float HK31 = HK12*P(3,6) + HK13*P(3,22) - HK13*P(3,4) + HK14*P(2,3) + HK15*P(0,3) + HK16*P(1,3) - HK17*P(3,3) + HK9*P(3,23) - HK9*P(3,5);
const float HK32 = Kaccy/(HK12*HK19*HK20 + HK14*HK19*HK25 + HK15*HK18*HK19 + HK16*HK19*HK30 - HK17*HK19*HK31 + HK22*HK23 - HK23*HK24 + HK27*HK28 - HK28*HK29 + R_ACC);
// Observation Jacobians
Hfusion.at<0>() = -HK1*HK2;
Hfusion.at<1>() = -HK2*HK4;
Hfusion.at<2>() = -HK2*HK5;
Hfusion.at<3>() = HK2*HK6;
Hfusion.at<4>() = HK8;
Hfusion.at<5>() = HK10;
Hfusion.at<6>() = -HK11*HK2;
Hfusion.at<7>() = 0;
Hfusion.at<8>() = 0;
Hfusion.at<9>() = 0;
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>() = -HK8;
Hfusion.at<23>() = -HK10;
// Kalman gains
Kfusion(0) = -HK18*HK32;
Kfusion(1) = -HK30*HK32;
Kfusion(2) = -HK25*HK32;
Kfusion(3) = -HK31*HK32;
Kfusion(4) = -HK24*HK32;
Kfusion(5) = -HK29*HK32;
Kfusion(6) = -HK20*HK32;
Kfusion(7) = -HK32*(HK12*P(6,7) - HK13*P(4,7) + HK13*P(7,22) + HK14*P(2,7) + HK15*P(0,7) + HK16*P(1,7) - HK17*P(3,7) - HK9*P(5,7) + HK9*P(7,23));
Kfusion(8) = -HK32*(HK12*P(6,8) - HK13*P(4,8) + HK13*P(8,22) + HK14*P(2,8) + HK15*P(0,8) + HK16*P(1,8) - HK17*P(3,8) - HK9*P(5,8) + HK9*P(8,23));
Kfusion(9) = -HK32*(HK12*P(6,9) - HK13*P(4,9) + HK13*P(9,22) + HK14*P(2,9) + HK15*P(0,9) + HK16*P(1,9) - HK17*P(3,9) - HK9*P(5,9) + HK9*P(9,23));
Kfusion(10) = -HK32*(HK12*P(6,10) + HK13*P(10,22) - HK13*P(4,10) + HK14*P(2,10) + HK15*P(0,10) + HK16*P(1,10) - HK17*P(3,10) + HK9*P(10,23) - HK9*P(5,10));
Kfusion(11) = -HK32*(HK12*P(6,11) + HK13*P(11,22) - HK13*P(4,11) + HK14*P(2,11) + HK15*P(0,11) + HK16*P(1,11) - HK17*P(3,11) + HK9*P(11,23) - HK9*P(5,11));
Kfusion(12) = -HK32*(HK12*P(6,12) + HK13*P(12,22) - HK13*P(4,12) + HK14*P(2,12) + HK15*P(0,12) + HK16*P(1,12) - HK17*P(3,12) + HK9*P(12,23) - HK9*P(5,12));
Kfusion(13) = -HK32*(HK12*P(6,13) + HK13*P(13,22) - HK13*P(4,13) + HK14*P(2,13) + HK15*P(0,13) + HK16*P(1,13) - HK17*P(3,13) + HK9*P(13,23) - HK9*P(5,13));
Kfusion(14) = -HK32*(HK12*P(6,14) + HK13*P(14,22) - HK13*P(4,14) + HK14*P(2,14) + HK15*P(0,14) + HK16*P(1,14) - HK17*P(3,14) + HK9*P(14,23) - HK9*P(5,14));
Kfusion(15) = -HK32*(HK12*P(6,15) + HK13*P(15,22) - HK13*P(4,15) + HK14*P(2,15) + HK15*P(0,15) + HK16*P(1,15) - HK17*P(3,15) + HK9*P(15,23) - HK9*P(5,15));
Kfusion(16) = -HK32*(HK12*P(6,16) + HK13*P(16,22) - HK13*P(4,16) + HK14*P(2,16) + HK15*P(0,16) + HK16*P(1,16) - HK17*P(3,16) + HK9*P(16,23) - HK9*P(5,16));
Kfusion(17) = -HK32*(HK12*P(6,17) + HK13*P(17,22) - HK13*P(4,17) + HK14*P(2,17) + HK15*P(0,17) + HK16*P(1,17) - HK17*P(3,17) + HK9*P(17,23) - HK9*P(5,17));
Kfusion(18) = -HK32*(HK12*P(6,18) + HK13*P(18,22) - HK13*P(4,18) + HK14*P(2,18) + HK15*P(0,18) + HK16*P(1,18) - HK17*P(3,18) + HK9*P(18,23) - HK9*P(5,18));
Kfusion(19) = -HK32*(HK12*P(6,19) + HK13*P(19,22) - HK13*P(4,19) + HK14*P(2,19) + HK15*P(0,19) + HK16*P(1,19) - HK17*P(3,19) + HK9*P(19,23) - HK9*P(5,19));
Kfusion(20) = -HK32*(HK12*P(6,20) + HK13*P(20,22) - HK13*P(4,20) + HK14*P(2,20) + HK15*P(0,20) + HK16*P(1,20) - HK17*P(3,20) + HK9*P(20,23) - HK9*P(5,20));
Kfusion(21) = -HK32*(HK12*P(6,21) + HK13*P(21,22) - HK13*P(4,21) + HK14*P(2,21) + HK15*P(0,21) + HK16*P(1,21) - HK17*P(3,21) + HK9*P(21,23) - HK9*P(5,21));
Kfusion(22) = -HK22*HK32;
Kfusion(23) = -HK27*HK32;
Reference in New Issue View Git Blame Copy Permalink