From 356b2f6ffebc137c7f7c456533c813ffb401636b Mon Sep 17 00:00:00 2001 From: kamilritz Date: Sat, 27 Jun 2020 09:08:51 +0200 Subject: [PATCH] Stop setting zero to zero --- EKF/airspeed_fusion.cpp | 8 +------- EKF/gps_yaw_fusion.cpp | 4 ---- EKF/mag_fusion.cpp | 25 ------------------------- EKF/sideslip_fusion.cpp | 13 +------------ 4 files changed, 2 insertions(+), 48 deletions(-) diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index 07b9c395b6..1821036b22 100644 --- a/EKF/airspeed_fusion.cpp +++ b/EKF/airspeed_fusion.cpp @@ -112,13 +112,7 @@ void Ekf::fuseAirspeed() SK_TAS[1] = SH_TAS[1]; - if (update_wind_only) { - // If we are getting aiding from other sources, then don't allow the airspeed measurements to affect the non-windspeed states - for (unsigned row = 0; row <= 21; row++) { - Kfusion[row] = 0.0f; - } - - } else { + if (!update_wind_only) { // we have no other source of aiding, so use airspeed measurements to correct states 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]); diff --git a/EKF/gps_yaw_fusion.cpp b/EKF/gps_yaw_fusion.cpp index 2252e4abc8..8b4e06e21b 100644 --- a/EKF/gps_yaw_fusion.cpp +++ b/EKF/gps_yaw_fusion.cpp @@ -169,8 +169,6 @@ void Ekf::fuseGpsAntYaw() float Kfusion[_k_num_states] = {}; for (uint8_t row = 0; row <= 15; row++) { - Kfusion[row] = 0.0f; - for (uint8_t col = 0; col <= 3; col++) { Kfusion[row] += P(row,col) * H_YAW[col]; } @@ -180,8 +178,6 @@ void Ekf::fuseGpsAntYaw() if (_control_status.flags.wind) { for (uint8_t row = 22; row <= 23; row++) { - Kfusion[row] = 0.0f; - for (uint8_t col = 0; col <= 3; col++) { Kfusion[row] += P(row,col) * H_YAW[col]; } diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 69873bd043..cc8d3eb278 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -216,13 +216,6 @@ void Ekf::fuseMag() Kfusion[15] = SK_MX[0]*(P(15,19) + P(15,1)*SH_MAG[0] - P(15,2)*SH_MAG[1] + P(15,3)*SH_MAG[2] + P(15,0)*SK_MX[2] - P(15,16)*SK_MX[1] + P(15,17)*SK_MX[4] - P(15,18)*SK_MX[3]); Kfusion[22] = SK_MX[0]*(P(22,19) + P(22,1)*SH_MAG[0] - P(22,2)*SH_MAG[1] + P(22,3)*SH_MAG[2] + P(22,0)*SK_MX[2] - P(22,16)*SK_MX[1] + P(22,17)*SK_MX[4] - P(22,18)*SK_MX[3]); Kfusion[23] = SK_MX[0]*(P(23,19) + P(23,1)*SH_MAG[0] - P(23,2)*SH_MAG[1] + P(23,3)*SH_MAG[2] + P(23,0)*SK_MX[2] - P(23,16)*SK_MX[1] + P(23,17)*SK_MX[4] - P(23,18)*SK_MX[3]); - } else { - for (uint8_t i = 0; i < 16; i++) { - Kfusion[i] = 0.0f; - } - - Kfusion[22] = 0.0f; - Kfusion[23] = 0.0f; } Kfusion[16] = SK_MX[0]*(P(16,19) + P(16,1)*SH_MAG[0] - P(16,2)*SH_MAG[1] + P(16,3)*SH_MAG[2] + P(16,0)*SK_MX[2] - P(16,16)*SK_MX[1] + P(16,17)*SK_MX[4] - P(16,18)*SK_MX[3]); @@ -271,13 +264,6 @@ void Ekf::fuseMag() Kfusion[15] = SK_MY[0]*(P(15,20) + P(15,0)*SH_MAG[2] + P(15,1)*SH_MAG[1] + P(15,2)*SH_MAG[0] - P(15,3)*SK_MY[2] - P(15,17)*SK_MY[1] - P(15,16)*SK_MY[3] + P(15,18)*SK_MY[4]); Kfusion[22] = SK_MY[0]*(P(22,20) + P(22,0)*SH_MAG[2] + P(22,1)*SH_MAG[1] + P(22,2)*SH_MAG[0] - P(22,3)*SK_MY[2] - P(22,17)*SK_MY[1] - P(22,16)*SK_MY[3] + P(22,18)*SK_MY[4]); Kfusion[23] = SK_MY[0]*(P(23,20) + P(23,0)*SH_MAG[2] + P(23,1)*SH_MAG[1] + P(23,2)*SH_MAG[0] - P(23,3)*SK_MY[2] - P(23,17)*SK_MY[1] - P(23,16)*SK_MY[3] + P(23,18)*SK_MY[4]); - } else { - for (uint8_t i = 0; i < 16; i++) { - Kfusion[i] = 0.0f; - } - - Kfusion[22] = 0.0f; - Kfusion[23] = 0.0f; } Kfusion[16] = SK_MY[0]*(P(16,20) + P(16,0)*SH_MAG[2] + P(16,1)*SH_MAG[1] + P(16,2)*SH_MAG[0] - P(16,3)*SK_MY[2] - P(16,17)*SK_MY[1] - P(16,16)*SK_MY[3] + P(16,18)*SK_MY[4]); @@ -332,13 +318,6 @@ void Ekf::fuseMag() Kfusion[15] = SK_MZ[0]*(P(15,21) + P(15,0)*SH_MAG[1] - P(15,1)*SH_MAG[2] + P(15,3)*SH_MAG[0] + P(15,2)*SK_MZ[2] + P(15,18)*SK_MZ[1] + P(15,16)*SK_MZ[4] - P(15,17)*SK_MZ[3]); Kfusion[22] = SK_MZ[0]*(P(22,21) + P(22,0)*SH_MAG[1] - P(22,1)*SH_MAG[2] + P(22,3)*SH_MAG[0] + P(22,2)*SK_MZ[2] + P(22,18)*SK_MZ[1] + P(22,16)*SK_MZ[4] - P(22,17)*SK_MZ[3]); Kfusion[23] = SK_MZ[0]*(P(23,21) + P(23,0)*SH_MAG[1] - P(23,1)*SH_MAG[2] + P(23,3)*SH_MAG[0] + P(23,2)*SK_MZ[2] + P(23,18)*SK_MZ[1] + P(23,16)*SK_MZ[4] - P(23,17)*SK_MZ[3]); - } else { - for (uint8_t i = 0; i < 16; i++) { - Kfusion[i] = 0.0f; - } - - Kfusion[22] = 0.0f; - Kfusion[23] = 0.0f; } Kfusion[16] = SK_MZ[0]*(P(16,21) + P(16,0)*SH_MAG[1] - P(16,1)*SH_MAG[2] + P(16,3)*SH_MAG[0] + P(16,2)*SK_MZ[2] + P(16,18)*SK_MZ[1] + P(16,16)*SK_MZ[4] - P(16,17)*SK_MZ[3]); @@ -654,8 +633,6 @@ void Ekf::updateQuaternion(const float innovation, const float variance, const f float Kfusion[_k_num_states] = {}; for (uint8_t row = 0; row <= 15; row++) { - Kfusion[row] = 0.0f; - for (uint8_t col = 0; col <= 3; col++) { Kfusion[row] += P(row,col) * yaw_jacobian[col]; } @@ -665,8 +642,6 @@ void Ekf::updateQuaternion(const float innovation, const float variance, const f if (_control_status.flags.wind) { for (uint8_t row = 22; row <= 23; row++) { - Kfusion[row] = 0.0f; - for (uint8_t col = 0; col <= 3; col++) { Kfusion[row] += P(row,col) * yaw_jacobian[col]; } diff --git a/EKF/sideslip_fusion.cpp b/EKF/sideslip_fusion.cpp index 099f7db15a..024d56fde6 100644 --- a/EKF/sideslip_fusion.cpp +++ b/EKF/sideslip_fusion.cpp @@ -148,14 +148,8 @@ void Ekf::fuseSideslip() SK_BETA[7] = SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]; // Calculate Kalman gains - if (update_wind_only) { - // If we are getting aiding from other sources, then don't allow the sideslip fusion to affect the non-windspeed states - for (unsigned row = 0; row <= 21; row++) { - Kfusion[row] = 0.0f; + if (!update_wind_only) { - } - - } else { 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]); @@ -182,11 +176,6 @@ void Ekf::fuseSideslip() 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]); - } else { - for (int i = 16; i <= 21; i++) { - Kfusion[i] = 0.0f; - - } } }