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

@@ -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
+    // convert reported accuracy to a variance, but limit lower value to protect algorithm stability
-    velObsVar = sq(fmaxf(velAcc, 0.5f));
+    const float velObsVar = sq(fmaxf(velAcc, 0.5f));
-    vel_NE = vel;
 
     // 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[0] = vel[0];
-                EKF[mdl_idx].X[1] = vel_NE[1];
+                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[0] = EKF[mdl_idx].X[0] - vel[0];
-    EKF[mdl_idx].innov[1] = EKF[mdl_idx].X[1] - vel_NE[1];
+    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;

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
+    // Resets states and covariances for the EKF's and GSF including GSF weights, but not the AHRS complementary filters
-    void initialise();
+    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);