ekf2_derivation: do not print null equations

src/modules/ekf2/EKF/python/ekf_derivation/code_gen.py

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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,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

@@ -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

@@ -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

@@ -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;