diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/beta_fusion_generated_compare.cpp b/src/modules/ekf2/EKF/python/ekf_derivation/generated/beta_fusion_generated_compare.cpp deleted file mode 100644 index 9f4cc2f22c..0000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/beta_fusion_generated_compare.cpp +++ /dev/null @@ -1,333 +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 - Vector24f Hfusion_sympy; - Vector24f Kfusion_sympy; - - Vector24f Hfusion_matlab; - Vector24f Kfusion_matlab; - - float _beta_innov_var; - - const float R_BETA = sq(2.5f); - - // quaternion inputs must be normalised - float q0 = 2.0f * ((float)rand() - 0.5f); - float q1 = 2.0f * ((float)rand() - 0.5f); - float q2 = 2.0f * ((float)rand() - 0.5f); - float q3 = 2.0f * ((float)rand() - 0.5f); - const float length = sqrtf(sq(q0) + sq(q1) + sq(q2) + sq(q3)); - q0 /= length; - q1 /= length; - q2 /= length; - q3 /= length; - - // get latest velocity in earth frame - const float vn = 10.0f * 2.0f * ((float)rand() - 0.5f); - const float ve = 10.0f * 2.0f * ((float)rand() - 0.5f); - const float vd = 2.0f * ((float)rand() - 0.5f); - - // get latest wind velocity in earth frame - const float vwn = 5.0f * 2.0f * ((float)rand() - 0.5f); - const float vwe = 5.0f * 2.0f * ((float)rand() - 0.5f); - - // 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 Values - const float HK0 = vn - vwn; - const float HK1 = ve - vwe; - const float HK2 = HK0*q0 + HK1*q3 - q2*vd; - const float HK3 = q0*q2 - q1*q3; - const float HK4 = 2*vd; - const float HK5 = q0*q3; - const float HK6 = q1*q2; - const float HK7 = 2*HK5 + 2*HK6; - const float HK8 = ecl::powf(q0, 2); - const float HK9 = ecl::powf(q3, 2); - const float HK10 = HK8 - HK9; - const float HK11 = ecl::powf(q1, 2); - const float HK12 = ecl::powf(q2, 2); - const float HK13 = HK11 - HK12; - const float HK14 = HK10 + HK13; - const float HK15 = HK0*HK14 + HK1*HK7 - HK3*HK4; - const float HK16 = 1.0F/HK15; - const float HK17 = q0*q1 + q2*q3; - const float HK18 = HK10 - HK11 + HK12; - const float HK19 = HK16*(-2*HK0*(HK5 - HK6) + HK1*HK18 + HK17*HK4); - const float HK20 = -HK0*q3 + HK1*q0 + q1*vd; - const float HK21 = -HK19*HK2 + HK20; - const float HK22 = 2*HK16; - const float HK23 = HK0*q1 + HK1*q2 + q3*vd; - const float HK24 = HK0*q2 - HK1*q1 + q0*vd; - const float HK25 = -HK19*HK23 + HK24; - const float HK26 = HK19*HK24 + HK23; - const float HK27 = HK19*HK20 + HK2; - const float HK28 = HK14*HK19 + 2*HK5 - 2*HK6; - const float HK29 = HK16*HK28; - const float HK30 = HK19*HK7; - const float HK31 = HK17 + HK19*HK3; - const float HK32 = HK13 + HK30 - HK8 + HK9; - const float HK33 = 2*HK31; - const float HK34 = 2*HK26; - const float HK35 = 2*HK25; - const float HK36 = 2*HK27; - const float HK37 = 2*HK21; - const float HK38 = HK28*P(0,22) - HK28*P(0,4) + HK32*P(0,23) - HK32*P(0,5) + HK33*P(0,6) + HK34*P(0,2) + HK35*P(0,1) - HK36*P(0,3) + HK37*P(0,0); - const float HK39 = ecl::powf(HK15, -2); - const float HK40 = -HK28*P(4,6) + HK28*P(6,22) - HK32*P(5,6) + HK32*P(6,23) + HK33*P(6,6) + HK34*P(2,6) + HK35*P(1,6) - HK36*P(3,6) + HK37*P(0,6); - const float HK41 = HK32*P(5,23); - const float HK42 = HK28*P(22,23) - HK28*P(4,23) + HK32*P(23,23) + HK33*P(6,23) + HK34*P(2,23) + HK35*P(1,23) - HK36*P(3,23) + HK37*P(0,23) - HK41; - const float HK43 = HK32*HK39; - const float HK44 = HK28*P(4,22); - const float HK45 = HK28*P(22,22) + HK32*P(22,23) - HK32*P(5,22) + HK33*P(6,22) + HK34*P(2,22) + HK35*P(1,22) - HK36*P(3,22) + HK37*P(0,22) - HK44; - const float HK46 = HK28*HK39; - const float HK47 = -HK28*P(4,5) + HK28*P(5,22) - HK32*P(5,5) + HK33*P(5,6) + HK34*P(2,5) + HK35*P(1,5) - HK36*P(3,5) + HK37*P(0,5) + HK41; - const float HK48 = -HK28*P(4,4) + HK32*P(4,23) - HK32*P(4,5) + HK33*P(4,6) + HK34*P(2,4) + HK35*P(1,4) - HK36*P(3,4) + HK37*P(0,4) + HK44; - const float HK49 = HK28*P(2,22) - HK28*P(2,4) + HK32*P(2,23) - HK32*P(2,5) + HK33*P(2,6) + HK34*P(2,2) + HK35*P(1,2) - HK36*P(2,3) + HK37*P(0,2); - const float HK50 = HK28*P(1,22) - HK28*P(1,4) + HK32*P(1,23) - HK32*P(1,5) + HK33*P(1,6) + HK34*P(1,2) + HK35*P(1,1) - HK36*P(1,3) + HK37*P(0,1); - const float HK51 = HK28*P(3,22) - HK28*P(3,4) + HK32*P(3,23) - HK32*P(3,5) + HK33*P(3,6) + HK34*P(2,3) + HK35*P(1,3) - HK36*P(3,3) + HK37*P(0,3); - //const float HK52 = HK16/(HK33*HK39*HK40 + HK34*HK39*HK49 + HK35*HK39*HK50 - HK36*HK39*HK51 + HK37*HK38*HK39 + HK42*HK43 - HK43*HK47 + HK45*HK46 - HK46*HK48 + R_BETA); - - // innovation variance - _beta_innov_var = (HK33*HK39*HK40 + HK34*HK39*HK49 + HK35*HK39*HK50 - HK36*HK39*HK51 + HK37*HK38*HK39 + HK42*HK43 - HK43*HK47 + HK45*HK46 - HK46*HK48 + R_BETA); - - // // Reset covariance and states if the calculation is badly conditioned - // if (_beta_innov_var < R_BETA) { - // _fault_status.flags.bad_sideslip = true; - - // // if we are getting aiding from other sources, warn and reset the wind states and covariances only - // const char* action_string = nullptr; - // if (update_wind_only) { - // resetWindStates(); - // resetWindCovariance(); - // action_string = "wind"; - - // } else { - // initialiseCovariance(); - // _state.wind_vel.setZero(); - // action_string = "full"; - // } - // ECL_ERR("sideslip badly conditioned - %s covariance reset", action_string); - - // return; - // } - // _fault_status.flags.bad_sideslip = false; - const float HK52 = HK16/_beta_innov_var; - - // determine if we need the sideslip fusion to correct states other than wind - // bool update_wind_only = !_control_status.flags.wind_dead_reckoning; - bool update_wind_only = false; - - // // Calculate predicted sideslip angle and innovation using small angle approximation - // _beta_innov = rel_wind_body(1) / rel_wind_body(0); - - // // Compute the ratio of innovation to gate size - // _beta_test_ratio = sq(_beta_innov) / (sq(fmaxf(_params.beta_innov_gate, 1.0f)) * _beta_innov_var); - - // // if the innovation consistency check fails then don't fuse the sample and indicate bad beta health - // if (_beta_test_ratio > 1.0f) { - // _innov_check_fail_status.flags.reject_sideslip = true; - // return; - - // } else { - // _innov_check_fail_status.flags.reject_sideslip = false; - // } - - // Observation Jacobians - SparseVector24f<0,1,2,3,4,5,6,22,23> Hfusion; - Hfusion.at<0>() = HK21*HK22; - Hfusion.at<1>() = HK22*HK25; - Hfusion.at<2>() = HK22*HK26; - Hfusion.at<3>() = -HK22*HK27; - Hfusion.at<4>() = -HK29; - Hfusion.at<5>() = HK16*(HK18 - HK30); - Hfusion.at<6>() = HK22*HK31; - Hfusion.at<22>() = HK29; - Hfusion.at<23>() = HK16*HK32; - - // Calculate Kalman gains - Vector24f Kfusion; - if (!update_wind_only) { - - Kfusion(0) = HK38*HK52; - Kfusion(1) = HK50*HK52; - Kfusion(2) = HK49*HK52; - Kfusion(3) = HK51*HK52; - Kfusion(4) = HK48*HK52; - Kfusion(5) = HK47*HK52; - Kfusion(6) = HK40*HK52; - - for (unsigned row = 7; row <= 21; row++) { - Kfusion(row) = HK52*(HK28*P(row,22) - HK28*P(4,row) + HK32*P(row,23) - HK32*P(5,row) + HK33*P(6,row) + HK34*P(2,row) + HK35*P(1,row) - HK36*P(3,row) + HK37*P(0,row)); - } - - } - - Kfusion(22) = HK45*HK52; - Kfusion(23) = HK42*HK52; - - // save output and repeat calculation using legacy matlab generated code - Hfusion_sympy(0) = Hfusion.at<0>(); - Hfusion_sympy(1) = Hfusion.at<1>(); - Hfusion_sympy(2) = Hfusion.at<2>(); - Hfusion_sympy(3) = Hfusion.at<3>(); - 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 - { - // Calculate the observation jacobians - - const matrix::Vector3f rel_wind_earth(vn - vwn, ve - vwe, vd); - - float SH_BETA[13]; // intermediate variable for algebraic optimisation - - SH_BETA[0] = (vn - vwn)*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) - vd*(2.0f*q0*q2 - 2.0f*q1*q3) + (ve - vwe)*(2.0f*q0*q3 + 2.0f*q1*q2); - - SH_BETA[1] = rel_wind_earth(1)*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + vd*(2.0f*q0*q1 + 2.0f*q2*q3) - rel_wind_earth(0)*(2.0f*q0*q3 - 2.0f*q1*q2); - SH_BETA[2] = rel_wind_earth(0); - SH_BETA[3] = rel_wind_earth(1); - SH_BETA[4] = 1.0f/sq(SH_BETA[0]); - SH_BETA[5] = 1.0f/SH_BETA[0]; - SH_BETA[6] = SH_BETA[5]*(sq(q0) - sq(q1) + sq(q2) - sq(q3)); - SH_BETA[7] = sq(q0) + sq(q1) - sq(q2) - sq(q3); - SH_BETA[8] = 2.0f*q0*SH_BETA[3] - 2.0f*q3*SH_BETA[2] + 2.0f*q1*vd; - SH_BETA[9] = 2.0f*q0*SH_BETA[2] + 2.0f*q3*SH_BETA[3] - 2.0f*q2*vd; - SH_BETA[10] = 2.0f*q2*SH_BETA[2] - 2.0f*q1*SH_BETA[3] + 2.0f*q0*vd; - SH_BETA[11] = 2.0f*q1*SH_BETA[2] + 2.0f*q2*SH_BETA[3] + 2.0f*q3*vd; - SH_BETA[12] = 2.0f*q0*q3; - - Vector24f H_BETA; - H_BETA(0) = SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]; - H_BETA(1) = SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]; - H_BETA(2) = SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]; - H_BETA(3) = - SH_BETA[5]*SH_BETA[9] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8]; - H_BETA(4) = - SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) - SH_BETA[1]*SH_BETA[4]*SH_BETA[7]; - H_BETA(5) = SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2); - H_BETA(6) = SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3); - H_BETA(22) = SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]; - H_BETA(23) = SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2) - SH_BETA[6]; - - _beta_innov_var = (R_BETA - (SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7])*(P(22,4)*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P(4,4)*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P(5,4)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P(23,4)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P(0,4)*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P(1,4)*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P(2,4)*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P(3,4)*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P(6,4)*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) + (SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7])*(P(22,22)*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P(4,22)*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P(5,22)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P(23,22)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P(0,22)*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P(1,22)*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P(2,22)*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P(3,22)*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P(6,22)*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) + (SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2))*(P(22,5)*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P(4,5)*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P(5,5)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P(23,5)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P(0,5)*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P(1,5)*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P(2,5)*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P(3,5)*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P(6,5)*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) - (SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2))*(P(22,23)*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P(4,23)*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P(5,23)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P(23,23)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P(0,23)*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P(1,23)*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P(2,23)*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P(3,23)*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P(6,23)*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) + (SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9])*(P(22,0)*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P(4,0)*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P(5,0)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P(23,0)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P(0,0)*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P(1,0)*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P(2,0)*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P(3,0)*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P(6,0)*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) + (SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11])*(P(22,1)*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P(4,1)*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P(5,1)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P(23,1)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P(0,1)*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P(1,1)*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P(2,1)*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P(3,1)*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P(6,1)*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) + (SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10])*(P(22,2)*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P(4,2)*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P(5,2)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P(23,2)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P(0,2)*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P(1,2)*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P(2,2)*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P(3,2)*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P(6,2)*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) - (SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8])*(P(22,3)*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P(4,3)*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P(5,3)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P(23,3)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P(0,3)*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P(1,3)*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P(2,3)*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P(3,3)*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P(6,3)*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))) + (SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3))*(P(22,6)*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P(4,6)*(SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P(5,6)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) - P(23,6)*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2)) + P(0,6)*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P(1,6)*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P(2,6)*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P(3,6)*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P(6,6)*(SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3)))); - - // intermediate variables for optimising calculations of the Kalman gain - float SK_BETA[8]; - SK_BETA[0] = 1.0f / _beta_innov_var; - SK_BETA[1] = SH_BETA[5]*(SH_BETA[12] - 2.0f*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]; - SK_BETA[2] = SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2.0f*q1*q2); - SK_BETA[3] = SH_BETA[5]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2.0f*q0*q2 - 2.0f*q1*q3); - SK_BETA[4] = SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]; - SK_BETA[5] = SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]; - SK_BETA[6] = SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]; - SK_BETA[7] = SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]; - - Vector24f Kfusion; - Kfusion(0) = SK_BETA[0]*(P(0,0)*SK_BETA[5] + P(0,1)*SK_BETA[4] - P(0,4)*SK_BETA[1] + P(0,5)*SK_BETA[2] + P(0,2)*SK_BETA[6] + P(0,6)*SK_BETA[3] - P(0,3)*SK_BETA[7] + P(0,22)*SK_BETA[1] - P(0,23)*SK_BETA[2]); - Kfusion(1) = SK_BETA[0]*(P(1,0)*SK_BETA[5] + P(1,1)*SK_BETA[4] - P(1,4)*SK_BETA[1] + P(1,5)*SK_BETA[2] + P(1,2)*SK_BETA[6] + P(1,6)*SK_BETA[3] - P(1,3)*SK_BETA[7] + P(1,22)*SK_BETA[1] - P(1,23)*SK_BETA[2]); - Kfusion(2) = SK_BETA[0]*(P(2,0)*SK_BETA[5] + P(2,1)*SK_BETA[4] - P(2,4)*SK_BETA[1] + P(2,5)*SK_BETA[2] + P(2,2)*SK_BETA[6] + P(2,6)*SK_BETA[3] - P(2,3)*SK_BETA[7] + P(2,22)*SK_BETA[1] - P(2,23)*SK_BETA[2]); - Kfusion(3) = SK_BETA[0]*(P(3,0)*SK_BETA[5] + P(3,1)*SK_BETA[4] - P(3,4)*SK_BETA[1] + P(3,5)*SK_BETA[2] + P(3,2)*SK_BETA[6] + P(3,6)*SK_BETA[3] - P(3,3)*SK_BETA[7] + P(3,22)*SK_BETA[1] - P(3,23)*SK_BETA[2]); - Kfusion(4) = SK_BETA[0]*(P(4,0)*SK_BETA[5] + P(4,1)*SK_BETA[4] - P(4,4)*SK_BETA[1] + P(4,5)*SK_BETA[2] + P(4,2)*SK_BETA[6] + P(4,6)*SK_BETA[3] - P(4,3)*SK_BETA[7] + P(4,22)*SK_BETA[1] - P(4,23)*SK_BETA[2]); - Kfusion(5) = SK_BETA[0]*(P(5,0)*SK_BETA[5] + P(5,1)*SK_BETA[4] - P(5,4)*SK_BETA[1] + P(5,5)*SK_BETA[2] + P(5,2)*SK_BETA[6] + P(5,6)*SK_BETA[3] - P(5,3)*SK_BETA[7] + P(5,22)*SK_BETA[1] - P(5,23)*SK_BETA[2]); - Kfusion(6) = SK_BETA[0]*(P(6,0)*SK_BETA[5] + P(6,1)*SK_BETA[4] - P(6,4)*SK_BETA[1] + P(6,5)*SK_BETA[2] + P(6,2)*SK_BETA[6] + P(6,6)*SK_BETA[3] - P(6,3)*SK_BETA[7] + P(6,22)*SK_BETA[1] - P(6,23)*SK_BETA[2]); - Kfusion(7) = SK_BETA[0]*(P(7,0)*SK_BETA[5] + P(7,1)*SK_BETA[4] - P(7,4)*SK_BETA[1] + P(7,5)*SK_BETA[2] + P(7,2)*SK_BETA[6] + P(7,6)*SK_BETA[3] - P(7,3)*SK_BETA[7] + P(7,22)*SK_BETA[1] - P(7,23)*SK_BETA[2]); - Kfusion(8) = SK_BETA[0]*(P(8,0)*SK_BETA[5] + P(8,1)*SK_BETA[4] - P(8,4)*SK_BETA[1] + P(8,5)*SK_BETA[2] + P(8,2)*SK_BETA[6] + P(8,6)*SK_BETA[3] - P(8,3)*SK_BETA[7] + P(8,22)*SK_BETA[1] - P(8,23)*SK_BETA[2]); - Kfusion(9) = SK_BETA[0]*(P(9,0)*SK_BETA[5] + P(9,1)*SK_BETA[4] - P(9,4)*SK_BETA[1] + P(9,5)*SK_BETA[2] + P(9,2)*SK_BETA[6] + P(9,6)*SK_BETA[3] - P(9,3)*SK_BETA[7] + P(9,22)*SK_BETA[1] - P(9,23)*SK_BETA[2]); - Kfusion(10) = SK_BETA[0]*(P(10,0)*SK_BETA[5] + P(10,1)*SK_BETA[4] - P(10,4)*SK_BETA[1] + P(10,5)*SK_BETA[2] + P(10,2)*SK_BETA[6] + P(10,6)*SK_BETA[3] - P(10,3)*SK_BETA[7] + P(10,22)*SK_BETA[1] - P(10,23)*SK_BETA[2]); - Kfusion(11) = SK_BETA[0]*(P(11,0)*SK_BETA[5] + P(11,1)*SK_BETA[4] - P(11,4)*SK_BETA[1] + P(11,5)*SK_BETA[2] + P(11,2)*SK_BETA[6] + P(11,6)*SK_BETA[3] - P(11,3)*SK_BETA[7] + P(11,22)*SK_BETA[1] - P(11,23)*SK_BETA[2]); - Kfusion(12) = SK_BETA[0]*(P(12,0)*SK_BETA[5] + P(12,1)*SK_BETA[4] - P(12,4)*SK_BETA[1] + P(12,5)*SK_BETA[2] + P(12,2)*SK_BETA[6] + P(12,6)*SK_BETA[3] - P(12,3)*SK_BETA[7] + P(12,22)*SK_BETA[1] - P(12,23)*SK_BETA[2]); - Kfusion(13) = SK_BETA[0]*(P(13,0)*SK_BETA[5] + P(13,1)*SK_BETA[4] - P(13,4)*SK_BETA[1] + P(13,5)*SK_BETA[2] + P(13,2)*SK_BETA[6] + P(13,6)*SK_BETA[3] - P(13,3)*SK_BETA[7] + P(13,22)*SK_BETA[1] - P(13,23)*SK_BETA[2]); - Kfusion(14) = SK_BETA[0]*(P(14,0)*SK_BETA[5] + P(14,1)*SK_BETA[4] - P(14,4)*SK_BETA[1] + P(14,5)*SK_BETA[2] + P(14,2)*SK_BETA[6] + P(14,6)*SK_BETA[3] - P(14,3)*SK_BETA[7] + P(14,22)*SK_BETA[1] - P(14,23)*SK_BETA[2]); - Kfusion(15) = SK_BETA[0]*(P(15,0)*SK_BETA[5] + P(15,1)*SK_BETA[4] - P(15,4)*SK_BETA[1] + P(15,5)*SK_BETA[2] + P(15,2)*SK_BETA[6] + P(15,6)*SK_BETA[3] - P(15,3)*SK_BETA[7] + P(15,22)*SK_BETA[1] - P(15,23)*SK_BETA[2]); - Kfusion(16) = SK_BETA[0]*(P(16,0)*SK_BETA[5] + P(16,1)*SK_BETA[4] - P(16,4)*SK_BETA[1] + P(16,5)*SK_BETA[2] + P(16,2)*SK_BETA[6] + P(16,6)*SK_BETA[3] - P(16,3)*SK_BETA[7] + P(16,22)*SK_BETA[1] - P(16,23)*SK_BETA[2]); - Kfusion(17) = SK_BETA[0]*(P(17,0)*SK_BETA[5] + P(17,1)*SK_BETA[4] - P(17,4)*SK_BETA[1] + P(17,5)*SK_BETA[2] + P(17,2)*SK_BETA[6] + P(17,6)*SK_BETA[3] - P(17,3)*SK_BETA[7] + P(17,22)*SK_BETA[1] - P(17,23)*SK_BETA[2]); - Kfusion(18) = SK_BETA[0]*(P(18,0)*SK_BETA[5] + P(18,1)*SK_BETA[4] - P(18,4)*SK_BETA[1] + P(18,5)*SK_BETA[2] + P(18,2)*SK_BETA[6] + P(18,6)*SK_BETA[3] - P(18,3)*SK_BETA[7] + P(18,22)*SK_BETA[1] - P(18,23)*SK_BETA[2]); - Kfusion(19) = SK_BETA[0]*(P(19,0)*SK_BETA[5] + P(19,1)*SK_BETA[4] - P(19,4)*SK_BETA[1] + P(19,5)*SK_BETA[2] + P(19,2)*SK_BETA[6] + P(19,6)*SK_BETA[3] - P(19,3)*SK_BETA[7] + P(19,22)*SK_BETA[1] - P(19,23)*SK_BETA[2]); - Kfusion(20) = SK_BETA[0]*(P(20,0)*SK_BETA[5] + P(20,1)*SK_BETA[4] - P(20,4)*SK_BETA[1] + P(20,5)*SK_BETA[2] + P(20,2)*SK_BETA[6] + P(20,6)*SK_BETA[3] - P(20,3)*SK_BETA[7] + P(20,22)*SK_BETA[1] - P(20,23)*SK_BETA[2]); - Kfusion(21) = SK_BETA[0]*(P(21,0)*SK_BETA[5] + P(21,1)*SK_BETA[4] - P(21,4)*SK_BETA[1] + P(21,5)*SK_BETA[2] + P(21,2)*SK_BETA[6] + P(21,6)*SK_BETA[3] - P(21,3)*SK_BETA[7] + P(21,22)*SK_BETA[1] - P(21,23)*SK_BETA[2]); - - // copy results - Hfusion_matlab = H_BETA; - 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 (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: Sideslip 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: Sideslip 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_matlab(row) != 0.0f) { - diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion_matlab(row)) / fabsf(Kfusion_matlab(row)); - } else if (Kfusion_sympy(row) != 0.0f) { - diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion_matlab(row)) / fabsf(Kfusion_sympy(row)); - } else { - diff_fraction = 0.0f; - } - if (diff_fraction > max_diff_fraction) { - max_diff_fraction = diff_fraction; - max_row = row; - max_old = Kfusion_matlab(row); - max_new = Kfusion_sympy(row); - } - } - - if (max_diff_fraction > 1e-5f) { - printf("Fail: Sideslip 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: Sideslip Kfusion max diff fraction = %e\n",max_diff_fraction); - } - - return 0; -} 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 deleted file mode 100644 index 7293022905..0000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/beta_generated.cpp +++ /dev/null @@ -1,99 +0,0 @@ -// Sub Expressions -const float HK0 = vn - vwn; -const float HK1 = ve - vwe; -const float HK2 = HK0*q0 + HK1*q3 - q2*vd; -const float HK3 = q0*q2 - q1*q3; -const float HK4 = 2*vd; -const float HK5 = q0*q3; -const float HK6 = q1*q2; -const float HK7 = 2*HK5 + 2*HK6; -const float HK8 = (q0)*(q0); -const float HK9 = (q3)*(q3); -const float HK10 = HK8 - HK9; -const float HK11 = (q1)*(q1); -const float HK12 = (q2)*(q2); -const float HK13 = HK11 - HK12; -const float HK14 = HK10 + HK13; -const float HK15 = HK0*HK14 + HK1*HK7 - HK3*HK4; -const float HK16 = 1.0F/(HK15); -const float HK17 = q0*q1 + q2*q3; -const float HK18 = HK16*(-2*HK0*(HK5 - HK6) + HK1*(HK10 - HK11 + HK12) + HK17*HK4); -const float HK19 = -HK0*q3 + HK1*q0 + q1*vd; -const float HK20 = -HK18*HK2 + HK19; -const float HK21 = 2*HK16; -const float HK22 = HK0*q1 + HK1*q2 + q3*vd; -const float HK23 = HK0*q2 - HK1*q1 + q0*vd; -const float HK24 = -HK18*HK22 + HK23; -const float HK25 = HK18*HK23 + HK22; -const float HK26 = HK18*HK19 + HK2; -const float HK27 = HK14*HK18 + 2*HK5 - 2*HK6; -const float HK28 = HK16*HK27; -const float HK29 = HK13 + HK18*HK7 - HK8 + HK9; -const float HK30 = HK17 + HK18*HK3; -const float HK31 = 2*HK30; -const float HK32 = 2*HK25; -const float HK33 = 2*HK24; -const float HK34 = 2*HK26; -const float HK35 = 2*HK20; -const float HK36 = HK27*P(0,22) - HK27*P(0,4) + HK29*P(0,23) - HK29*P(0,5) + HK31*P(0,6) + HK32*P(0,2) + HK33*P(0,1) - HK34*P(0,3) + HK35*P(0,0); -const float HK37 = 1.0F/((HK15)*(HK15)); -const float HK38 = -HK27*P(4,6) + HK27*P(6,22) - HK29*P(5,6) + HK29*P(6,23) + HK31*P(6,6) + HK32*P(2,6) + HK33*P(1,6) - HK34*P(3,6) + HK35*P(0,6); -const float HK39 = HK29*P(5,23); -const float HK40 = HK27*P(22,23) - HK27*P(4,23) + HK29*P(23,23) + HK31*P(6,23) + HK32*P(2,23) + HK33*P(1,23) - HK34*P(3,23) + HK35*P(0,23) - HK39; -const float HK41 = HK29*HK37; -const float HK42 = HK27*P(4,22); -const float HK43 = HK27*P(22,22) + HK29*P(22,23) - HK29*P(5,22) + HK31*P(6,22) + HK32*P(2,22) + HK33*P(1,22) - HK34*P(3,22) + HK35*P(0,22) - HK42; -const float HK44 = HK27*HK37; -const float HK45 = -HK27*P(4,5) + HK27*P(5,22) - HK29*P(5,5) + HK31*P(5,6) + HK32*P(2,5) + HK33*P(1,5) - HK34*P(3,5) + HK35*P(0,5) + HK39; -const float HK46 = -HK27*P(4,4) + HK29*P(4,23) - HK29*P(4,5) + HK31*P(4,6) + HK32*P(2,4) + HK33*P(1,4) - HK34*P(3,4) + HK35*P(0,4) + HK42; -const float HK47 = HK27*P(2,22) - HK27*P(2,4) + HK29*P(2,23) - HK29*P(2,5) + HK31*P(2,6) + HK32*P(2,2) + HK33*P(1,2) - HK34*P(2,3) + HK35*P(0,2); -const float HK48 = HK27*P(1,22) - HK27*P(1,4) + HK29*P(1,23) - HK29*P(1,5) + HK31*P(1,6) + HK32*P(1,2) + HK33*P(1,1) - HK34*P(1,3) + HK35*P(0,1); -const float HK49 = HK27*P(3,22) - HK27*P(3,4) + HK29*P(3,23) - HK29*P(3,5) + HK31*P(3,6) + HK32*P(2,3) + HK33*P(1,3) - HK34*P(3,3) + HK35*P(0,3); -const float HK50 = HK16/(HK31*HK37*HK38 + HK32*HK37*HK47 + HK33*HK37*HK48 - HK34*HK37*HK49 + HK35*HK36*HK37 + HK40*HK41 - HK41*HK45 + HK43*HK44 - HK44*HK46 + R_BETA); - - -// Observation Jacobians -Hfusion.at<0>() = HK20*HK21; -Hfusion.at<1>() = HK21*HK24; -Hfusion.at<2>() = HK21*HK25; -Hfusion.at<3>() = -HK21*HK26; -Hfusion.at<4>() = -HK28; -Hfusion.at<5>() = -HK16*HK29; -Hfusion.at<6>() = HK21*HK30; -Hfusion.at<22>() = HK28; -Hfusion.at<23>() = HK16*HK29; - - -// Kalman gains -Kfusion(0) = HK36*HK50; -Kfusion(1) = HK48*HK50; -Kfusion(2) = HK47*HK50; -Kfusion(3) = HK49*HK50; -Kfusion(4) = HK46*HK50; -Kfusion(5) = HK45*HK50; -Kfusion(6) = HK38*HK50; -Kfusion(7) = HK50*(-HK27*P(4,7) + HK27*P(7,22) - HK29*P(5,7) + HK29*P(7,23) + HK31*P(6,7) + HK32*P(2,7) + HK33*P(1,7) - HK34*P(3,7) + HK35*P(0,7)); -Kfusion(8) = HK50*(-HK27*P(4,8) + HK27*P(8,22) - HK29*P(5,8) + HK29*P(8,23) + HK31*P(6,8) + HK32*P(2,8) + HK33*P(1,8) - HK34*P(3,8) + HK35*P(0,8)); -Kfusion(9) = HK50*(-HK27*P(4,9) + HK27*P(9,22) - HK29*P(5,9) + HK29*P(9,23) + HK31*P(6,9) + HK32*P(2,9) + HK33*P(1,9) - HK34*P(3,9) + HK35*P(0,9)); -Kfusion(10) = HK50*(HK27*P(10,22) - HK27*P(4,10) + HK29*P(10,23) - HK29*P(5,10) + HK31*P(6,10) + HK32*P(2,10) + HK33*P(1,10) - HK34*P(3,10) + HK35*P(0,10)); -Kfusion(11) = HK50*(HK27*P(11,22) - HK27*P(4,11) + HK29*P(11,23) - HK29*P(5,11) + HK31*P(6,11) + HK32*P(2,11) + HK33*P(1,11) - HK34*P(3,11) + HK35*P(0,11)); -Kfusion(12) = HK50*(HK27*P(12,22) - HK27*P(4,12) + HK29*P(12,23) - HK29*P(5,12) + HK31*P(6,12) + HK32*P(2,12) + HK33*P(1,12) - HK34*P(3,12) + HK35*P(0,12)); -Kfusion(13) = HK50*(HK27*P(13,22) - HK27*P(4,13) + HK29*P(13,23) - HK29*P(5,13) + HK31*P(6,13) + HK32*P(2,13) + HK33*P(1,13) - HK34*P(3,13) + HK35*P(0,13)); -Kfusion(14) = HK50*(HK27*P(14,22) - HK27*P(4,14) + HK29*P(14,23) - HK29*P(5,14) + HK31*P(6,14) + HK32*P(2,14) + HK33*P(1,14) - HK34*P(3,14) + HK35*P(0,14)); -Kfusion(15) = HK50*(HK27*P(15,22) - HK27*P(4,15) + HK29*P(15,23) - HK29*P(5,15) + HK31*P(6,15) + HK32*P(2,15) + HK33*P(1,15) - HK34*P(3,15) + HK35*P(0,15)); -Kfusion(16) = HK50*(HK27*P(16,22) - HK27*P(4,16) + HK29*P(16,23) - HK29*P(5,16) + HK31*P(6,16) + HK32*P(2,16) + HK33*P(1,16) - HK34*P(3,16) + HK35*P(0,16)); -Kfusion(17) = HK50*(HK27*P(17,22) - HK27*P(4,17) + HK29*P(17,23) - HK29*P(5,17) + HK31*P(6,17) + HK32*P(2,17) + HK33*P(1,17) - HK34*P(3,17) + HK35*P(0,17)); -Kfusion(18) = HK50*(HK27*P(18,22) - HK27*P(4,18) + HK29*P(18,23) - HK29*P(5,18) + HK31*P(6,18) + HK32*P(2,18) + HK33*P(1,18) - HK34*P(3,18) + HK35*P(0,18)); -Kfusion(19) = HK50*(HK27*P(19,22) - HK27*P(4,19) + HK29*P(19,23) - HK29*P(5,19) + HK31*P(6,19) + HK32*P(2,19) + HK33*P(1,19) - HK34*P(3,19) + HK35*P(0,19)); -Kfusion(20) = HK50*(HK27*P(20,22) - HK27*P(4,20) + HK29*P(20,23) - HK29*P(5,20) + HK31*P(6,20) + HK32*P(2,20) + HK33*P(1,20) - HK34*P(3,20) + HK35*P(0,20)); -Kfusion(21) = HK50*(HK27*P(21,22) - HK27*P(4,21) + HK29*P(21,23) - HK29*P(5,21) + HK31*P(6,21) + HK32*P(2,21) + HK33*P(1,21) - HK34*P(3,21) + HK35*P(0,21)); -Kfusion(22) = HK43*HK50; -Kfusion(23) = HK40*HK50; - - -// Predicted observation - - -// Innovation variance - -