From 4a743a382735d5bc7e371aeb4afb86724a1f559f Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 25 Apr 2020 09:45:28 +1000 Subject: [PATCH] AP_NavEKF: Remove unnecessary reset operations and class variables The setting of EKF state variances is only required when commencing or recommencing velocity fusion. The function that resets the EKF and GSF class variables has been renamed to be more consistent with its function. --- libraries/AP_NavEKF/EKFGSF_yaw.cpp | 30 ++++++++++-------------------- libraries/AP_NavEKF/EKFGSF_yaw.h | 8 +++----- 2 files changed, 13 insertions(+), 25 deletions(-) diff --git a/libraries/AP_NavEKF/EKFGSF_yaw.cpp b/libraries/AP_NavEKF/EKFGSF_yaw.cpp index 0e19ebc348..c428d58dd9 100644 --- a/libraries/AP_NavEKF/EKFGSF_yaw.cpp +++ b/libraries/AP_NavEKF/EKFGSF_yaw.cpp @@ -58,7 +58,6 @@ void EKFGSF_yaw::update(const Vector3f &delAng, const bool ok_to_align = ((accel_norm_sq > lower_accel_limit * lower_accel_limit && accel_norm_sq < upper_accel_limit * upper_accel_limit)); if (ok_to_align) { - initialise(); alignTilt(); ahrs_tilt_aligned = true; ahrs_accel = accel; @@ -99,9 +98,6 @@ void EKFGSF_yaw::update(const Vector3f &delAng, } if (vel_fuse_running && !run_ekf_gsf) { - // We have landed so reset EKF-GSF states and wait to fly again - // Do not reset the AHRS as it continues to run when on ground - initialise(); vel_fuse_running = false; } @@ -140,18 +136,18 @@ void EKFGSF_yaw::update(const Vector3f &delAng, void EKFGSF_yaw::fuseVelData(Vector2f vel, float velAcc) { - // set class variables - velObsVar = sq(fmaxf(velAcc, 0.5f)); - vel_NE = vel; + // convert reported accuracy to a variance, but limit lower value to protect algorithm stability + const float velObsVar = sq(fmaxf(velAcc, 0.5f)); // The 3-state EKF models only run when flying to avoid corrupted estimates due to operator handling and GPS interference if (run_ekf_gsf) { if (!vel_fuse_running) { // Perform in-flight alignment + resetEKFGSF(); for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) { // Use the firstGPS measurement to set the velocities and corresponding variances - EKF[mdl_idx].X[0] = vel_NE[0]; - EKF[mdl_idx].X[1] = vel_NE[1]; + EKF[mdl_idx].X[0] = vel[0]; + EKF[mdl_idx].X[1] = vel[1]; EKF[mdl_idx].P[0][0] = velObsVar; EKF[mdl_idx].P[1][1] = velObsVar; } @@ -163,7 +159,7 @@ void EKFGSF_yaw::fuseVelData(Vector2f vel, float velAcc) bool state_update_failed = false; for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) { // Update states and covariances using GPS NE velocity measurements fused as direct state observations - if (!correct(mdl_idx)) { + if (!correct(mdl_idx, vel, velObsVar)) { state_update_failed = true; } } @@ -383,11 +379,11 @@ void EKFGSF_yaw::predict(const uint8_t mdl_idx) // Update EKF states and covariance for specified model index using velocity measurement // Returns false if the sttae and covariance correction failed -bool EKFGSF_yaw::correct(const uint8_t mdl_idx) +bool EKFGSF_yaw::correct(const uint8_t mdl_idx, const Vector2f &vel, const float velObsVar) { // calculate velocity observation innovations - EKF[mdl_idx].innov[0] = EKF[mdl_idx].X[0] - vel_NE[0]; - EKF[mdl_idx].innov[1] = EKF[mdl_idx].X[1] - vel_NE[1]; + EKF[mdl_idx].innov[0] = EKF[mdl_idx].X[0] - vel[0]; + EKF[mdl_idx].innov[1] = EKF[mdl_idx].X[1] - vel[1]; // copy covariance matrix to temporary variables const float P00 = EKF[mdl_idx].P[0][0]; @@ -531,7 +527,7 @@ bool EKFGSF_yaw::correct(const uint8_t mdl_idx) return true; } -void EKFGSF_yaw::initialise() +void EKFGSF_yaw::resetEKFGSF() { memset(&GSF, 0, sizeof(GSF)); vel_fuse_running = false; @@ -546,12 +542,6 @@ void EKFGSF_yaw::initialise() // All filter models start with the same weight GSF.weights[mdl_idx] = 1.0f / (float)N_MODELS_EKFGSF; - // Take state and variance estimates direct from velocity sensor - EKF[mdl_idx].X[0] = vel_NE[0]; - EKF[mdl_idx].X[1] = vel_NE[1]; - EKF[mdl_idx].P[0][0] = velObsVar; - EKF[mdl_idx].P[1][1] = velObsVar; - // Use half yaw interval for yaw uncertainty as that is the maximum that the best model can be away from truth GSF.yaw_variance = sq(0.5f * yaw_increment); EKF[mdl_idx].P[2][2] = GSF.yaw_variance; diff --git a/libraries/AP_NavEKF/EKFGSF_yaw.h b/libraries/AP_NavEKF/EKFGSF_yaw.h index 74aade703d..a560015190 100644 --- a/libraries/AP_NavEKF/EKFGSF_yaw.h +++ b/libraries/AP_NavEKF/EKFGSF_yaw.h @@ -103,18 +103,16 @@ private: EKF_struct EKF[N_MODELS_EKFGSF]; bool vel_fuse_running; // true when the bank of EKF's has started fusing GPS velocity data bool run_ekf_gsf; // true when operating condition is suitable for to run the GSF and EKF models and fuse velocity data - Vector2f vel_NE; // NE velocity observations (m/s) - float velObsVar; // variance of velocity observations (m/s)^2 - // Initialises the EKF's and GSF states, but not the AHRS complementary filters - void initialise(); + // Resets states and covariances for the EKF's and GSF including GSF weights, but not the AHRS complementary filters + void resetEKFGSF(); // Runs the state and covariance prediction for the selected EKF void predict(const uint8_t mdl_idx); // Runs the state and covariance update for the selected EKF using the GPS NE velocity measurement // Returns false if the sttae and covariance correction failed - bool correct(const uint8_t mdl_idx); + bool correct(const uint8_t mdl_idx, const Vector2f &vel, const float velObsVar); // Forces symmetry on the covariance matrix for the selected EKF void forceSymmetry(const uint8_t mdl_idx);