Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-22 06:14:14 +08:00
Code Issues Actions 8 Packages Projects Releases Wiki Activity

ekf2_derivation: do not print null equations

Browse Source
This commit is contained in:
bresch
2022-07-01 15:12:18 +02:00
committed by Mathieu Bresciani
parent 78dd557b26
commit 28b34a634b
14 changed files with 6 additions and 483 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/python/ekf_derivation/code_gen.py
+6 -1
View File
@@ -42,15 +42,20 @@ class CodeGenerator:
write_string = ""
if matrix.shape[0] * matrix.shape[1] == 1:
write_string = write_string + variable_name + " = " + self.get_ccode(matrix[0]) + ";\n"
if matrix[0] != 0:
write_string = write_string + variable_name + " = " + self.get_ccode(matrix[0]) + ";\n"
elif matrix.shape[0] == 1 or matrix.shape[1] == 1:
for i in range(0,len(matrix)):
if matrix[i] == 0:
continue
write_string = write_string + variable_name + pre_bracket + str(i) + post_bracket + " = " + self.get_ccode(matrix[i]) + ";\n"
else:
for j in range(0, matrix.shape[1]):
for i in range(0, matrix.shape[0]):
if j >= i or not is_symmetric:
if matrix[i,j] == 0:
continue
write_string = write_string + variable_name + pre_bracket + str(i) + "," + str(j) + post_bracket + " = " + self.get_ccode(matrix[i,j]) + ";\n"
write_string = write_string + "\n\n"
src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated.cpp
-48
View File
@@ -30,26 +30,10 @@ Hfusion.at<0>() = -2*HKX0;
Hfusion.at<1>() = 2*HKX1;
Hfusion.at<2>() = -2*HKX3;
Hfusion.at<3>() = 2*HKX4;
Hfusion.at<4>() = 0;
Hfusion.at<5>() = 0;
Hfusion.at<6>() = 0;
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>() = -HKX5;
Hfusion.at<17>() = 2*HKX6;
Hfusion.at<18>() = -2*HKX7;
Hfusion.at<19>() = 1;
Hfusion.at<20>() = 0;
Hfusion.at<21>() = 0;
Hfusion.at<22>() = 0;
Hfusion.at<23>() = 0;
// Kalman gains
@@ -111,26 +95,10 @@ Hfusion.at<0>() = 2*HKY0;
Hfusion.at<1>() = 2*HKY2;
Hfusion.at<2>() = 2*HKY3;
Hfusion.at<3>() = -2*HKY4;
Hfusion.at<4>() = 0;
Hfusion.at<5>() = 0;
Hfusion.at<6>() = 0;
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>() = -2*HKY5;
Hfusion.at<17>() = -HKY6;
Hfusion.at<18>() = 2*HKY7;
Hfusion.at<19>() = 0;
Hfusion.at<20>() = 1;
Hfusion.at<21>() = 0;
Hfusion.at<22>() = 0;
Hfusion.at<23>() = 0;
// Kalman gains
@@ -192,26 +160,10 @@ Hfusion.at<0>() = -2*HKZ0;
Hfusion.at<1>() = -2*HKZ2;
Hfusion.at<2>() = 2*HKZ3;
Hfusion.at<3>() = 2*HKZ4;
Hfusion.at<4>() = 0;
Hfusion.at<5>() = 0;
Hfusion.at<6>() = 0;
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>() = 2*HKZ5;
Hfusion.at<17>() = -2*HKZ6;
Hfusion.at<18>() = -HKZ7;
Hfusion.at<19>() = 0;
Hfusion.at<20>() = 0;
Hfusion.at<21>() = 1;
Hfusion.at<22>() = 0;
Hfusion.at<23>() = 0;
// Kalman gains
src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated_alt.cpp
-48
View File
@@ -93,26 +93,10 @@ Hfusion.at<0>() = -2*HK1;
Hfusion.at<1>() = 2*HK4;
Hfusion.at<2>() = -2*HK8;
Hfusion.at<3>() = 2*HK12;
Hfusion.at<4>() = 0;
Hfusion.at<5>() = 0;
Hfusion.at<6>() = 0;
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>() = -HK15;
Hfusion.at<17>() = 2*HK17;
Hfusion.at<18>() = -2*HK19;
Hfusion.at<19>() = 1;
Hfusion.at<20>() = 0;
Hfusion.at<21>() = 0;
Hfusion.at<22>() = 0;
Hfusion.at<23>() = 0;
// Kalman gains - axis 0
@@ -147,26 +131,10 @@ Hfusion.at<0>() = 2*HK38;
Hfusion.at<1>() = 2*HK39;
Hfusion.at<2>() = 2*HK41;
Hfusion.at<3>() = -2*HK44;
Hfusion.at<4>() = 0;
Hfusion.at<5>() = 0;
Hfusion.at<6>() = 0;
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>() = -2*HK45;
Hfusion.at<17>() = -HK47;
Hfusion.at<18>() = 2*HK49;
Hfusion.at<19>() = 0;
Hfusion.at<20>() = 1;
Hfusion.at<21>() = 0;
Hfusion.at<22>() = 0;
Hfusion.at<23>() = 0;
// Kalman gains - axis 1
@@ -201,26 +169,10 @@ Hfusion.at<0>() = -2*HK65;
Hfusion.at<1>() = -2*HK66;
Hfusion.at<2>() = 2*HK67;
Hfusion.at<3>() = 2*HK68;
Hfusion.at<4>() = 0;
Hfusion.at<5>() = 0;
Hfusion.at<6>() = 0;
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>() = 2*HK69;
Hfusion.at<17>() = -2*HK70;
Hfusion.at<18>() = -HK71;
Hfusion.at<19>() = 0;
Hfusion.at<20>() = 0;
Hfusion.at<21>() = 1;
Hfusion.at<22>() = 0;
Hfusion.at<23>() = 0;
// Kalman gains - axis 2
src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated.cpp
-30
View File
@@ -47,21 +47,6 @@ 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;
@@ -139,21 +124,6 @@ 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;
src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated_alt.cpp
-30
View File
@@ -89,21 +89,6 @@ Hfusion.at<3>() = -HK14*HK4;
Hfusion.at<4>() = HK17;
Hfusion.at<5>() = -HK21;
Hfusion.at<6>() = HK22*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>() = -HK17;
Hfusion.at<23>() = HK21;
@@ -143,21 +128,6 @@ Hfusion.at<3>() = HK49*HK52;
Hfusion.at<4>() = HK54;
Hfusion.at<5>() = HK56;
Hfusion.at<6>() = -HK49*HK57;
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>() = -HK54;
Hfusion.at<23>() = -HK56;
src/modules/ekf2/EKF/python/ekf_derivation/generated/beta_generated.cpp
-15
View File
@@ -58,21 +58,6 @@ Hfusion.at<3>() = -HK30;
Hfusion.at<4>() = HK25*HK32;
Hfusion.at<5>() = -HK33;
Hfusion.at<6>() = -HK34;
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>() = HK14*HK35;
Hfusion.at<23>() = -HK36;
src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated.cpp
-34
View File
@@ -54,23 +54,6 @@ 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
@@ -173,23 +156,6 @@ Hfusion.at<3>() = -HK11*(-HK22*vn + HK23);
Hfusion.at<4>() = -HK10*(2*HK26*Tbs(0,2) - HK29*HK30 - HK33*Tbs(0,0));
Hfusion.at<5>() = -HK10*(2*HK34*Tbs(0,0) - HK37*HK38 - HK40*Tbs(0,1));
Hfusion.at<6>() = -HK10*(2*HK41*Tbs(0,1) - HK42*HK43 - HK44*Tbs(0,2));
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
src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated_alt.cpp
-34
View File
@@ -102,23 +102,6 @@ Hfusion.at<3>() = HK18*HK9;
Hfusion.at<4>() = -HK30*HK8;
Hfusion.at<5>() = -HK38*HK8;
Hfusion.at<6>() = -HK42*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
@@ -156,23 +139,6 @@ Hfusion.at<3>() = -HK9*(-HK73*vn + HK74);
Hfusion.at<4>() = -HK8*(2*HK21*Tbs(0,2) - HK25*HK75 - HK29*Tbs(0,0));
Hfusion.at<5>() = -HK8*(2*HK31*Tbs(0,0) - HK35*HK76 - HK37*Tbs(0,1));
Hfusion.at<6>() = -HK8*(2*HK39*Tbs(0,1) - HK40*HK77 - HK41*Tbs(0,2));
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
src/modules/ekf2/EKF/python/ekf_derivation/generated/gps_yaw_generated.cpp
-20
View File
@@ -32,26 +32,6 @@ Hfusion.at<0>() = HK10*HK13;
Hfusion.at<1>() = -HK13*HK17;
Hfusion.at<2>() = -HK13*HK18;
Hfusion.at<3>() = HK13*HK19;
Hfusion.at<4>() = 0;
Hfusion.at<5>() = 0;
Hfusion.at<6>() = 0;
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
src/modules/ekf2/EKF/python/ekf_derivation/generated/mag_decl_generated.cpp
-22
View File
@@ -12,30 +12,8 @@ const float HK9 = HK4/(-HK0*HK6*HK7 + HK7*HK8*magE/ecl::powf(magN, 3) + R_DECL);
// Observation Jacobians
Hfusion.at<0>() = 0;
Hfusion.at<1>() = 0;
Hfusion.at<2>() = 0;
Hfusion.at<3>() = 0;
Hfusion.at<4>() = 0;
Hfusion.at<5>() = 0;
Hfusion.at<6>() = 0;
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>() = -HK0*HK2*magE;
Hfusion.at<17>() = HK4;
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
src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_generated.cpp
-19
View File
@@ -19,28 +19,9 @@ const float HK16 = HK3/(-HK10*HK14 + HK10*HK9 + HK12*HK13 - HK13*HK15 + HK6*HK7*
// Observation Jacobians
Hfusion.at<0>() = 0;
Hfusion.at<1>() = 0;
Hfusion.at<2>() = 0;
Hfusion.at<3>() = 0;
Hfusion.at<4>() = HK4;
Hfusion.at<5>() = HK5;
Hfusion.at<6>() = HK3*vd;
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>() = -HK4;
Hfusion.at<23>() = -HK5;
src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated.cpp
-51
View File
@@ -30,23 +30,6 @@ H_VEL(3) = 2*HK4;
H_VEL(4) = -HK5;
H_VEL(5) = 2*HK6;
H_VEL(6) = -2*HK7;
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) = -HK14*HK21;
@@ -107,23 +90,6 @@ H_VEL(3) = -2*HK4;
H_VEL(4) = -2*HK5;
H_VEL(5) = -HK6;
H_VEL(6) = 2*HK7;
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) = HK14*HK21;
@@ -184,23 +150,6 @@ H_VEL(3) = 2*HK4;
H_VEL(4) = 2*HK5;
H_VEL(5) = -2*HK6;
H_VEL(6) = -HK7;
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) = -HK14*HK21;
src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated_alt.cpp
-51
View File
@@ -91,23 +91,6 @@ Hfusion.at<3>() = 2*HK12;
Hfusion.at<4>() = -HK15;
Hfusion.at<5>() = 2*HK17;
Hfusion.at<6>() = -2*HK19;
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
@@ -145,23 +128,6 @@ Hfusion.at<3>() = -2*HK41;
Hfusion.at<4>() = -2*HK42;
Hfusion.at<5>() = -HK44;
Hfusion.at<6>() = 2*HK46;
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
@@ -199,23 +165,6 @@ Hfusion.at<3>() = 2*HK64;
Hfusion.at<4>() = 2*HK65;
Hfusion.at<5>() = -2*HK66;
Hfusion.at<6>() = -HK67;
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
src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_generated.cpp
-80
View File
@@ -14,26 +14,6 @@ H_YAW.at<0>() = SA7*q3;
H_YAW.at<1>() = SA7*q2;
H_YAW.at<2>() = SA5*(SA1*SA6 + SA8*q2);
H_YAW.at<3>() = SA5*(SA0*SA6 + SA8*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 321 yaw observation matrix - option B
@@ -52,26 +32,6 @@ H_YAW.at<0>() = SB7*q3;
H_YAW.at<1>() = SB7*q2;
H_YAW.at<2>() = -SB5*(-SB1*SB6 - SB8*q2);
H_YAW.at<3>() = -SB5*(-SB0*SB6 - SB8*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
@@ -90,26 +50,6 @@ H_YAW.at<0>() = SA7*q3;
H_YAW.at<1>() = SA5*(-SA1*SA6 + SA8*q1);
H_YAW.at<2>() = -SA7*q1;
H_YAW.at<3>() = SA5*(SA0*SA6 + SA8*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 B
@@ -127,25 +67,5 @@ H_YAW.at<0>() = -SB6*q3;
H_YAW.at<1>() = -SB4*(-2*SB5*q2 + SB7*q1);
H_YAW.at<2>() = SB6*q1;
H_YAW.at<3>() = -SB4*(SB0*SB5 + SB7*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;