diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/code_gen.py b/src/modules/ekf2/EKF/python/ekf_derivation/code_gen.py index 29a5df1064..513ea8c957 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/code_gen.py +++ b/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" diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated.cpp index f0d2e80115..eae5017664 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated.cpp +++ b/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 diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated_alt.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated_alt.cpp index 3a58a69a81..3069162821 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/3Dmag_generated_alt.cpp +++ b/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 diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated.cpp index 79453f7c49..3a6c4aae1d 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated.cpp +++ b/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; diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated_alt.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated_alt.cpp index fc888c4e87..8fa4777f36 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/acc_bf_generated_alt.cpp +++ b/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; diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/beta_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/beta_generated.cpp index 613e9df66f..52e2f894c9 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/beta_generated.cpp +++ b/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; diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated.cpp index 6177740d4e..96370ce1cd 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated.cpp +++ b/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 diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated_alt.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated_alt.cpp index 28b67b6168..7b45e81bd0 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/flow_generated_alt.cpp +++ b/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 diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/gps_yaw_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/gps_yaw_generated.cpp index c19f5894b5..31f99f7e55 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/gps_yaw_generated.cpp +++ b/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 diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/mag_decl_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/mag_decl_generated.cpp index 7d5f07696f..79aaf58737 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/mag_decl_generated.cpp +++ b/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 diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_generated.cpp index d97e969f66..4a1a8c39b9 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_generated.cpp +++ b/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; diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated.cpp index 14cc2b39f3..1268c8a2ff 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated.cpp +++ b/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; diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated_alt.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated_alt.cpp index d88011752d..22713eab1f 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/vel_bf_generated_alt.cpp +++ b/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 diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_generated.cpp index 711028c645..ee99c11832 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_generated.cpp +++ b/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;