From 4b687beb3bff06b9175f169c63edc596fece0321 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 3 Oct 2022 14:06:25 +0200 Subject: [PATCH] ekf2: remove old airspeed fusion code --- .../tas_fusion_generated_compare.cpp | 227 ------------------ .../generated/tas_generated.cpp | 61 ----- .../generated/tas_hk_generated.cpp | 55 ----- .../generated/tas_var_generated.cpp | 25 -- 4 files changed, 368 deletions(-) delete mode 100644 src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_fusion_generated_compare.cpp delete mode 100644 src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_generated.cpp delete mode 100644 src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_hk_generated.cpp delete mode 100644 src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_var_generated.cpp diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_fusion_generated_compare.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_fusion_generated_compare.cpp deleted file mode 100644 index aa1d05c8e9..0000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_fusion_generated_compare.cpp +++ /dev/null @@ -1,227 +0,0 @@ -#include -#include -#include -#include "../../../../../matrix/matrix/math.hpp" -#include "util.h" - -typedef matrix::Vector Vector24f; -typedef matrix::SquareMatrix SquareMatrix24f; -template -using SparseVector24f = matrix::SparseVectorf<24, Idxs...>; - -int main() -{ - // Compare calculation of observation Jacobians and Kalman gains for sympy and matlab generated equations - - float airspeed_innov_var; - - Vector24f Kfusion; // Kalman gain vector - - Vector24f Hfusion_sympy; - Vector24f Kfusion_sympy; - - Vector24f Hfusion_matlab; - Vector24f Kfusion_matlab; - - const float R_TAS = sq(1.5f); - - const bool update_wind_only = false; - - // get latest velocity in earth frame - const float vn = 9.0f; - const float ve = 12.0f; - const float vd = -1.5f; - - // get latest wind velocity in earth frame - const float vwn = -4.0f; - const float vwe = 3.0f; - - // create a symmetrical positive dfinite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1 - SquareMatrix24f P; - for (int col=0; col<=23; col++) { - for (int row=0; row<=col; row++) { - if (row == col) { - P(row,col) = (float)rand(); - } else { - P(col,row) = P(row,col) = 2.0f * ((float)rand() - 0.5f); - } - } - } - - // First calculate observationjacobians and Kalman gains using sympy generated equations - - { - // Intermediate variables - const float HK0 = vn - vwn; - const float HK1 = ve - vwe; - const float HK2 = ecl::powf(HK0, 2) + ecl::powf(HK1, 2) + ecl::powf(vd, 2); - const float v_tas_pred = sqrtf(HK2); // predicted airspeed - //const float HK3 = powf(HK2, -1.0F/2.0F); - if (v_tas_pred < 1.0f) { - // calculation can be badly conditioned for very low airspeed values so don't fuse this time - return 0; - } - const float HK3 = 1.0f / v_tas_pred; - const float HK4 = HK0*HK3; - const float HK5 = HK1*HK3; - const float HK6 = 1.0F/HK2; - const float HK7 = HK0*P(4,6) - HK0*P(6,22) + HK1*P(5,6) - HK1*P(6,23) + P(6,6)*vd; - const float HK8 = HK1*P(5,23); - const float HK9 = HK0*P(4,5) - HK0*P(5,22) + HK1*P(5,5) - HK8 + P(5,6)*vd; - const float HK10 = HK1*HK6; - const float HK11 = HK0*P(4,22); - const float HK12 = HK0*P(4,4) - HK1*P(4,23) + HK1*P(4,5) - HK11 + P(4,6)*vd; - const float HK13 = HK0*HK6; - const float HK14 = -HK0*P(22,23) + HK0*P(4,23) - HK1*P(23,23) + HK8 + P(6,23)*vd; - const float HK15 = -HK0*P(22,22) - HK1*P(22,23) + HK1*P(5,22) + HK11 + P(6,22)*vd; - const float HK16 = HK3/(-HK10*HK14 + HK10*HK9 + HK12*HK13 - HK13*HK15 + HK6*HK7*vd + R_TAS); - - // Observation Jacobians - SparseVector24f<4,5,6,22,23> Hfusion; - Hfusion.at<4>() = HK4; - Hfusion.at<5>() = HK5; - Hfusion.at<6>() = HK3*vd; - Hfusion.at<22>() = -HK4; - Hfusion.at<23>() = -HK5; - - if (true) { - // we have no other source of aiding, so use airspeed measurements to correct states - for (unsigned row = 0; row <= 3; row++) { - Kfusion(row) = HK16*(HK0*P(4,row) - HK0*P(row,22) + HK1*P(5,row) - HK1*P(row,23) + P(6,row)*vd); - } - - Kfusion(4) = HK12*HK16; - Kfusion(5) = HK16*HK9; - Kfusion(6) = HK16*HK7; - - for (unsigned row = 7; row <= 21; row++) { - Kfusion(row) = HK16*(HK0*P(4,row) - HK0*P(row,22) + HK1*P(5,row) - HK1*P(row,23) + P(6,row)*vd); - } - } - - Kfusion(22) = HK15*HK16; - Kfusion(23) = HK14*HK16; - - // save output - Hfusion_sympy(4) = Hfusion.at<4>(); - Hfusion_sympy(5) = Hfusion.at<5>(); - Hfusion_sympy(6) = Hfusion.at<6>(); - Hfusion_sympy(22) = Hfusion.at<22>(); - Hfusion_sympy(23) = Hfusion.at<23>(); - Kfusion_sympy = Kfusion; - } - // repeat calculation using matlab generated equations - { - const float v_tas_pred = sqrtf((ve - vwe) * (ve - vwe) + (vn - vwn) * (vn - vwn) + vd * vd); - - // intermediate variable from algebraic optimisation - float SH_TAS[3]; - SH_TAS[0] = 1.0f/v_tas_pred; - SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2.0f*vwe))*0.5f; - SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2.0f*vwn))*0.5f; - - // Observation Jacobian - Vector24f H_TAS = {}; - H_TAS(4) = SH_TAS[2]; - H_TAS(5) = SH_TAS[1]; - H_TAS(6) = vd*SH_TAS[0]; - H_TAS(22) = -SH_TAS[2]; - H_TAS(23) = -SH_TAS[1]; - - airspeed_innov_var = (R_TAS + SH_TAS[2]*(P(4,4)*SH_TAS[2] + P(5,4)*SH_TAS[1] - P(22,4)*SH_TAS[2] - P(23,4)*SH_TAS[1] + P(6,4)*vd*SH_TAS[0]) + SH_TAS[1]*(P(4,5)*SH_TAS[2] + P(5,5)*SH_TAS[1] - P(22,5)*SH_TAS[2] - P(23,5)*SH_TAS[1] + P(6,5)*vd*SH_TAS[0]) - SH_TAS[2]*(P(4,22)*SH_TAS[2] + P(5,22)*SH_TAS[1] - P(22,22)*SH_TAS[2] - P(23,22)*SH_TAS[1] + P(6,22)*vd*SH_TAS[0]) - SH_TAS[1]*(P(4,23)*SH_TAS[2] + P(5,23)*SH_TAS[1] - P(22,23)*SH_TAS[2] - P(23,23)*SH_TAS[1] + P(6,23)*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P(4,6)*SH_TAS[2] + P(5,6)*SH_TAS[1] - P(22,6)*SH_TAS[2] - P(23,6)*SH_TAS[1] + P(6,6)*vd*SH_TAS[0])); - - float SK_TAS[2]; - SK_TAS[0] = 1.0f / airspeed_innov_var; - SK_TAS[1] = SH_TAS[1]; - - // Kalman gain - Kfusion(0) = SK_TAS[0]*(P(0,4)*SH_TAS[2] - P(0,22)*SH_TAS[2] + P(0,5)*SK_TAS[1] - P(0,23)*SK_TAS[1] + P(0,6)*vd*SH_TAS[0]); - Kfusion(1) = SK_TAS[0]*(P(1,4)*SH_TAS[2] - P(1,22)*SH_TAS[2] + P(1,5)*SK_TAS[1] - P(1,23)*SK_TAS[1] + P(1,6)*vd*SH_TAS[0]); - Kfusion(2) = SK_TAS[0]*(P(2,4)*SH_TAS[2] - P(2,22)*SH_TAS[2] + P(2,5)*SK_TAS[1] - P(2,23)*SK_TAS[1] + P(2,6)*vd*SH_TAS[0]); - Kfusion(3) = SK_TAS[0]*(P(3,4)*SH_TAS[2] - P(3,22)*SH_TAS[2] + P(3,5)*SK_TAS[1] - P(3,23)*SK_TAS[1] + P(3,6)*vd*SH_TAS[0]); - Kfusion(4) = SK_TAS[0]*(P(4,4)*SH_TAS[2] - P(4,22)*SH_TAS[2] + P(4,5)*SK_TAS[1] - P(4,23)*SK_TAS[1] + P(4,6)*vd*SH_TAS[0]); - Kfusion(5) = SK_TAS[0]*(P(5,4)*SH_TAS[2] - P(5,22)*SH_TAS[2] + P(5,5)*SK_TAS[1] - P(5,23)*SK_TAS[1] + P(5,6)*vd*SH_TAS[0]); - Kfusion(6) = SK_TAS[0]*(P(6,4)*SH_TAS[2] - P(6,22)*SH_TAS[2] + P(6,5)*SK_TAS[1] - P(6,23)*SK_TAS[1] + P(6,6)*vd*SH_TAS[0]); - Kfusion(7) = SK_TAS[0]*(P(7,4)*SH_TAS[2] - P(7,22)*SH_TAS[2] + P(7,5)*SK_TAS[1] - P(7,23)*SK_TAS[1] + P(7,6)*vd*SH_TAS[0]); - Kfusion(8) = SK_TAS[0]*(P(8,4)*SH_TAS[2] - P(8,22)*SH_TAS[2] + P(8,5)*SK_TAS[1] - P(8,23)*SK_TAS[1] + P(8,6)*vd*SH_TAS[0]); - Kfusion(9) = SK_TAS[0]*(P(9,4)*SH_TAS[2] - P(9,22)*SH_TAS[2] + P(9,5)*SK_TAS[1] - P(9,23)*SK_TAS[1] + P(9,6)*vd*SH_TAS[0]); - Kfusion(10) = SK_TAS[0]*(P(10,4)*SH_TAS[2] - P(10,22)*SH_TAS[2] + P(10,5)*SK_TAS[1] - P(10,23)*SK_TAS[1] + P(10,6)*vd*SH_TAS[0]); - Kfusion(11) = SK_TAS[0]*(P(11,4)*SH_TAS[2] - P(11,22)*SH_TAS[2] + P(11,5)*SK_TAS[1] - P(11,23)*SK_TAS[1] + P(11,6)*vd*SH_TAS[0]); - Kfusion(12) = SK_TAS[0]*(P(12,4)*SH_TAS[2] - P(12,22)*SH_TAS[2] + P(12,5)*SK_TAS[1] - P(12,23)*SK_TAS[1] + P(12,6)*vd*SH_TAS[0]); - Kfusion(13) = SK_TAS[0]*(P(13,4)*SH_TAS[2] - P(13,22)*SH_TAS[2] + P(13,5)*SK_TAS[1] - P(13,23)*SK_TAS[1] + P(13,6)*vd*SH_TAS[0]); - Kfusion(14) = SK_TAS[0]*(P(14,4)*SH_TAS[2] - P(14,22)*SH_TAS[2] + P(14,5)*SK_TAS[1] - P(14,23)*SK_TAS[1] + P(14,6)*vd*SH_TAS[0]); - Kfusion(15) = SK_TAS[0]*(P(15,4)*SH_TAS[2] - P(15,22)*SH_TAS[2] + P(15,5)*SK_TAS[1] - P(15,23)*SK_TAS[1] + P(15,6)*vd*SH_TAS[0]); - Kfusion(16) = SK_TAS[0]*(P(16,4)*SH_TAS[2] - P(16,22)*SH_TAS[2] + P(16,5)*SK_TAS[1] - P(16,23)*SK_TAS[1] + P(16,6)*vd*SH_TAS[0]); - Kfusion(17) = SK_TAS[0]*(P(17,4)*SH_TAS[2] - P(17,22)*SH_TAS[2] + P(17,5)*SK_TAS[1] - P(17,23)*SK_TAS[1] + P(17,6)*vd*SH_TAS[0]); - Kfusion(18) = SK_TAS[0]*(P(18,4)*SH_TAS[2] - P(18,22)*SH_TAS[2] + P(18,5)*SK_TAS[1] - P(18,23)*SK_TAS[1] + P(18,6)*vd*SH_TAS[0]); - Kfusion(19) = SK_TAS[0]*(P(19,4)*SH_TAS[2] - P(19,22)*SH_TAS[2] + P(19,5)*SK_TAS[1] - P(19,23)*SK_TAS[1] + P(19,6)*vd*SH_TAS[0]); - Kfusion(20) = SK_TAS[0]*(P(20,4)*SH_TAS[2] - P(20,22)*SH_TAS[2] + P(20,5)*SK_TAS[1] - P(20,23)*SK_TAS[1] + P(20,6)*vd*SH_TAS[0]); - Kfusion(21) = SK_TAS[0]*(P(21,4)*SH_TAS[2] - P(21,22)*SH_TAS[2] + P(21,5)*SK_TAS[1] - P(21,23)*SK_TAS[1] + P(21,6)*vd*SH_TAS[0]); - Kfusion(22) = SK_TAS[0]*(P(22,4)*SH_TAS[2] - P(22,22)*SH_TAS[2] + P(22,5)*SK_TAS[1] - P(22,23)*SK_TAS[1] + P(22,6)*vd*SH_TAS[0]); - Kfusion(23) = SK_TAS[0]*(P(23,4)*SH_TAS[2] - P(23,22)*SH_TAS[2] + P(23,5)*SK_TAS[1] - P(23,23)*SK_TAS[1] + P(23,6)*vd*SH_TAS[0]); - - // save output; - Hfusion_matlab = H_TAS; - Kfusion_matlab = Kfusion; - } - - // find largest observation variance difference as a fraction of the matlab value - float max_diff_fraction = 0.0f; - int max_row; - float max_old, max_new; - for (int row=0; row<24; row++) { - float diff_fraction; - if (Hfusion_matlab(row) != 0.0f) { - diff_fraction = fabsf(Hfusion_sympy(row) - Hfusion_matlab(row)) / fabsf(Hfusion_matlab(row)); - } else if (Hfusion_sympy(row) != 0.0f) { - diff_fraction = fabsf(Hfusion_sympy(row) - Hfusion_matlab(row)) / fabsf(Hfusion_sympy(row)); - } else { - diff_fraction = 0.0f; - } - if (Hfusion_sympy(row) - Hfusion_matlab(row) != 0.0f) { - printf("new,old Hfusion(%i) = %e,%e\n",row,Hfusion_sympy(row),Hfusion_matlab(row)); - } - if (diff_fraction > max_diff_fraction) { - max_diff_fraction = diff_fraction; - max_row = row; - max_old = Hfusion_matlab(row); - max_new = Hfusion_sympy(row); - } - } - - if (max_diff_fraction > 1e-5f) { - printf("Fail: Airspeed Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row); - } else { - printf("Pass: Airspeed Hfusion max diff fraction = %e\n",max_diff_fraction); - } - - // find largest Kalman gain difference as a fraction of the matlab value - max_diff_fraction = 0.0f; - for (int row=0; row<4; row++) { - float diff_fraction; - if (Kfusion(row) != 0.0f) { - diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion(row)) / fabsf(Kfusion(row)); - } else if (Kfusion_sympy(row) != 0.0f) { - diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion(row)) / fabsf(Kfusion_sympy(row)); - } else { - diff_fraction = 0.0f; - } - // if (Kfusion_sympy(row) - Kfusion(row) != 0.0f) { - // printf("new,old Kfusion(%i) = %e,%e\n",row,Kfusion_sympy(row),Kfusion(row)); - // } - if (diff_fraction > max_diff_fraction) { - max_diff_fraction = diff_fraction; - max_row = row; - max_old = Kfusion(row); - max_new = Kfusion_sympy(row); - } - } - - if (max_diff_fraction > 1e-5f) { - printf("Fail: Airspeed Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row); - } else { - printf("Pass: Airspeed Kfusion max diff fraction = %e\n",max_diff_fraction); - } - - return 0; -} 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 deleted file mode 100644 index 8f0bd97f5a..0000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_generated.cpp +++ /dev/null @@ -1,61 +0,0 @@ -// Sub Expressions -const float HK0 = vn - vwn; -const float HK1 = ve - vwe; -const float HK2 = (HK0)*(HK0) + (HK1)*(HK1) + (vd)*(vd); -const float HK3 = 1.0F/sqrtf(HK2); -const float HK4 = HK0*HK3; -const float HK5 = HK1*HK3; -const float HK6 = 1.0F/(HK2); -const float HK7 = HK0*P(4,6) - HK0*P(6,22) + HK1*P(5,6) - HK1*P(6,23) + P(6,6)*vd; -const float HK8 = HK1*P(5,23); -const float HK9 = HK0*P(4,5) - HK0*P(5,22) + HK1*P(5,5) - HK8 + P(5,6)*vd; -const float HK10 = HK1*HK6; -const float HK11 = HK0*P(4,22); -const float HK12 = HK0*P(4,4) - HK1*P(4,23) + HK1*P(4,5) - HK11 + P(4,6)*vd; -const float HK13 = HK0*HK6; -const float HK14 = -HK0*P(22,23) + HK0*P(4,23) - HK1*P(23,23) + HK8 + P(6,23)*vd; -const float HK15 = -HK0*P(22,22) - HK1*P(22,23) + HK1*P(5,22) + HK11 + P(6,22)*vd; -const float HK16 = HK3/(-HK10*HK14 + HK10*HK9 + HK12*HK13 - HK13*HK15 + HK6*HK7*vd + R_TAS); - - -// Observation Jacobians -Hfusion.at<4>() = HK4; -Hfusion.at<5>() = HK5; -Hfusion.at<6>() = HK3*vd; -Hfusion.at<22>() = -HK4; -Hfusion.at<23>() = -HK5; - - -// Kalman gains -Kfusion(0) = HK16*(-HK0*P(0,22) + HK0*P(0,4) - HK1*P(0,23) + HK1*P(0,5) + P(0,6)*vd); -Kfusion(1) = HK16*(-HK0*P(1,22) + HK0*P(1,4) - HK1*P(1,23) + HK1*P(1,5) + P(1,6)*vd); -Kfusion(2) = HK16*(-HK0*P(2,22) + HK0*P(2,4) - HK1*P(2,23) + HK1*P(2,5) + P(2,6)*vd); -Kfusion(3) = HK16*(-HK0*P(3,22) + HK0*P(3,4) - HK1*P(3,23) + HK1*P(3,5) + P(3,6)*vd); -Kfusion(4) = HK12*HK16; -Kfusion(5) = HK16*HK9; -Kfusion(6) = HK16*HK7; -Kfusion(7) = HK16*(HK0*P(4,7) - HK0*P(7,22) + HK1*P(5,7) - HK1*P(7,23) + P(6,7)*vd); -Kfusion(8) = HK16*(HK0*P(4,8) - HK0*P(8,22) + HK1*P(5,8) - HK1*P(8,23) + P(6,8)*vd); -Kfusion(9) = HK16*(HK0*P(4,9) - HK0*P(9,22) + HK1*P(5,9) - HK1*P(9,23) + P(6,9)*vd); -Kfusion(10) = HK16*(-HK0*P(10,22) + HK0*P(4,10) - HK1*P(10,23) + HK1*P(5,10) + P(6,10)*vd); -Kfusion(11) = HK16*(-HK0*P(11,22) + HK0*P(4,11) - HK1*P(11,23) + HK1*P(5,11) + P(6,11)*vd); -Kfusion(12) = HK16*(-HK0*P(12,22) + HK0*P(4,12) - HK1*P(12,23) + HK1*P(5,12) + P(6,12)*vd); -Kfusion(13) = HK16*(-HK0*P(13,22) + HK0*P(4,13) - HK1*P(13,23) + HK1*P(5,13) + P(6,13)*vd); -Kfusion(14) = HK16*(-HK0*P(14,22) + HK0*P(4,14) - HK1*P(14,23) + HK1*P(5,14) + P(6,14)*vd); -Kfusion(15) = HK16*(-HK0*P(15,22) + HK0*P(4,15) - HK1*P(15,23) + HK1*P(5,15) + P(6,15)*vd); -Kfusion(16) = HK16*(-HK0*P(16,22) + HK0*P(4,16) - HK1*P(16,23) + HK1*P(5,16) + P(6,16)*vd); -Kfusion(17) = HK16*(-HK0*P(17,22) + HK0*P(4,17) - HK1*P(17,23) + HK1*P(5,17) + P(6,17)*vd); -Kfusion(18) = HK16*(-HK0*P(18,22) + HK0*P(4,18) - HK1*P(18,23) + HK1*P(5,18) + P(6,18)*vd); -Kfusion(19) = HK16*(-HK0*P(19,22) + HK0*P(4,19) - HK1*P(19,23) + HK1*P(5,19) + P(6,19)*vd); -Kfusion(20) = HK16*(-HK0*P(20,22) + HK0*P(4,20) - HK1*P(20,23) + HK1*P(5,20) + P(6,20)*vd); -Kfusion(21) = HK16*(-HK0*P(21,22) + HK0*P(4,21) - HK1*P(21,23) + HK1*P(5,21) + P(6,21)*vd); -Kfusion(22) = HK15*HK16; -Kfusion(23) = HK14*HK16; - - -// Predicted observation - - -// Innovation variance - - diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_hk_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_hk_generated.cpp deleted file mode 100644 index fecfb5f54f..0000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_hk_generated.cpp +++ /dev/null @@ -1,55 +0,0 @@ -// Sub Expressions -const float HK0 = vn - vwn; -const float HK1 = ve - vwe; -const float HK2 = sqrtf((HK0)*(HK0) + (HK1)*(HK1) + (vd)*(vd)); -const float HK3 = 1.0F/(HK2); -const float HK4 = HK0*HK3; -const float HK5 = HK1*HK3; -const float HK6 = HK3*vd; -const float HK7 = -HK0*HK3; -const float HK8 = -HK1*HK3; -const float HK9 = 1.0F/(innov_var); - - -// Observation Jacobians -Hfusion.at<4>() = HK4; -Hfusion.at<5>() = HK5; -Hfusion.at<6>() = HK6; -Hfusion.at<22>() = HK7; -Hfusion.at<23>() = HK8; - - -// Kalman gains -Kfusion(0) = HK9*(HK4*P(0,4) + HK5*P(0,5) + HK6*P(0,6) + HK7*P(0,22) + HK8*P(0,23)); -Kfusion(1) = HK9*(HK4*P(1,4) + HK5*P(1,5) + HK6*P(1,6) + HK7*P(1,22) + HK8*P(1,23)); -Kfusion(2) = HK9*(HK4*P(2,4) + HK5*P(2,5) + HK6*P(2,6) + HK7*P(2,22) + HK8*P(2,23)); -Kfusion(3) = HK9*(HK4*P(3,4) + HK5*P(3,5) + HK6*P(3,6) + HK7*P(3,22) + HK8*P(3,23)); -Kfusion(4) = HK9*(HK4*P(4,4) + HK5*P(4,5) + HK6*P(4,6) + HK7*P(4,22) + HK8*P(4,23)); -Kfusion(5) = HK9*(HK4*P(4,5) + HK5*P(5,5) + HK6*P(5,6) + HK7*P(5,22) + HK8*P(5,23)); -Kfusion(6) = HK9*(HK4*P(4,6) + HK5*P(5,6) + HK6*P(6,6) + HK7*P(6,22) + HK8*P(6,23)); -Kfusion(7) = HK9*(HK4*P(4,7) + HK5*P(5,7) + HK6*P(6,7) + HK7*P(7,22) + HK8*P(7,23)); -Kfusion(8) = HK9*(HK4*P(4,8) + HK5*P(5,8) + HK6*P(6,8) + HK7*P(8,22) + HK8*P(8,23)); -Kfusion(9) = HK9*(HK4*P(4,9) + HK5*P(5,9) + HK6*P(6,9) + HK7*P(9,22) + HK8*P(9,23)); -Kfusion(10) = HK9*(HK4*P(4,10) + HK5*P(5,10) + HK6*P(6,10) + HK7*P(10,22) + HK8*P(10,23)); -Kfusion(11) = HK9*(HK4*P(4,11) + HK5*P(5,11) + HK6*P(6,11) + HK7*P(11,22) + HK8*P(11,23)); -Kfusion(12) = HK9*(HK4*P(4,12) + HK5*P(5,12) + HK6*P(6,12) + HK7*P(12,22) + HK8*P(12,23)); -Kfusion(13) = HK9*(HK4*P(4,13) + HK5*P(5,13) + HK6*P(6,13) + HK7*P(13,22) + HK8*P(13,23)); -Kfusion(14) = HK9*(HK4*P(4,14) + HK5*P(5,14) + HK6*P(6,14) + HK7*P(14,22) + HK8*P(14,23)); -Kfusion(15) = HK9*(HK4*P(4,15) + HK5*P(5,15) + HK6*P(6,15) + HK7*P(15,22) + HK8*P(15,23)); -Kfusion(16) = HK9*(HK4*P(4,16) + HK5*P(5,16) + HK6*P(6,16) + HK7*P(16,22) + HK8*P(16,23)); -Kfusion(17) = HK9*(HK4*P(4,17) + HK5*P(5,17) + HK6*P(6,17) + HK7*P(17,22) + HK8*P(17,23)); -Kfusion(18) = HK9*(HK4*P(4,18) + HK5*P(5,18) + HK6*P(6,18) + HK7*P(18,22) + HK8*P(18,23)); -Kfusion(19) = HK9*(HK4*P(4,19) + HK5*P(5,19) + HK6*P(6,19) + HK7*P(19,22) + HK8*P(19,23)); -Kfusion(20) = HK9*(HK4*P(4,20) + HK5*P(5,20) + HK6*P(6,20) + HK7*P(20,22) + HK8*P(20,23)); -Kfusion(21) = HK9*(HK4*P(4,21) + HK5*P(5,21) + HK6*P(6,21) + HK7*P(21,22) + HK8*P(21,23)); -Kfusion(22) = HK9*(HK4*P(4,22) + HK5*P(5,22) + HK6*P(6,22) + HK7*P(22,22) + HK8*P(22,23)); -Kfusion(23) = HK9*(HK4*P(4,23) + HK5*P(5,23) + HK6*P(6,23) + HK7*P(22,23) + HK8*P(23,23)); - - -// Predicted observation -meas_pred = HK2; - - -// Innovation variance - - diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_var_generated.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_var_generated.cpp deleted file mode 100644 index 62d44a55e4..0000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/tas_var_generated.cpp +++ /dev/null @@ -1,25 +0,0 @@ -// Sub Expressions -const float IV0 = ve - vwe; -const float IV1 = vn - vwn; -const float IV2 = (IV0)*(IV0) + (IV1)*(IV1) + (vd)*(vd); -const float IV3 = 1.0F/(IV2); -const float IV4 = IV0*P(5,23); -const float IV5 = IV0*IV3; -const float IV6 = IV1*P(4,22); -const float IV7 = IV1*IV3; - - -// Observation Jacobians - - -// Kalman gains - - -// Predicted observation -meas_pred = sqrtf(IV2); - - -// Innovation variance -innov_var = IV3*vd*(IV0*P(5,6) - IV0*P(6,23) + IV1*P(4,6) - IV1*P(6,22) + P(6,6)*vd) - IV5*(-IV0*P(23,23) - IV1*P(22,23) + IV1*P(4,23) + IV4 + P(6,23)*vd) + IV5*(IV0*P(5,5) + IV1*P(4,5) - IV1*P(5,22) - IV4 + P(5,6)*vd) - IV7*(-IV0*P(22,23) + IV0*P(5,22) - IV1*P(22,22) + IV6 + P(6,22)*vd) + IV7*(-IV0*P(4,23) + IV0*P(4,5) + IV1*P(4,4) - IV6 + P(4,6)*vd) + R_TAS; - -