AttPosEKF: Fix sensor loss recovery

3 files changed, 26 insertions, 20 deletions

diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index 5e5208e78..1c79cb61d 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -633,14 +633,10 @@ void AttitudePositionEstimatorEKF::task_main()
 				} 
 				else if (!_ekf->statesInitialised) {
 					// North, East Down position (m)
-					float posNED[3] = {0.0f, 0.0f, 0.0f}; 
-					float initVelNED[3];
-	
-					initVelNED[0] = 0.0f;
-					initVelNED[1] = 0.0f;
-					initVelNED[2] = 0.0f;
-					_ekf->posNE[0] = posNED[0];
-					_ekf->posNE[1] = posNED[1];
+					float initVelNED[3] = {0.0f, 0.0f, 0.0f}; 
+
+					_ekf->posNE[0] = 0.0f;
+					_ekf->posNE[1] = 0.0f;
 
 					_local_pos.ref_alt = _baro_ref;
 					_baro_ref_offset = 0.0f;
@@ -1291,7 +1287,6 @@ void AttitudePositionEstimatorEKF::pollData()
 				if (dtGoodGPS > POS_RESET_THRESHOLD) {
 					_ekf->ResetPosition();
 					_ekf->ResetVelocity();
-					_ekf->ResetStoredStates();
 				}
 			}
 
diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
index 700979eda..bdab0e5bc 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
@@ -1121,10 +1121,9 @@ void AttPosEKF::FuseVelposNED()
                 current_ekf_state.velHealth = true;
                 current_ekf_state.velFailTime = millis();
             } else if (current_ekf_state.velTimeout || !current_ekf_state.posHealth) {
-                // XXX check
                 current_ekf_state.velHealth = true;
                 ResetVelocity();
-                ResetStoredStates();
+
                 // do not fuse bad data
                 fuseVelData = false;
             }
@@ -1152,9 +1151,6 @@ void AttPosEKF::FuseVelposNED()
                 if (current_ekf_state.posTimeout) {
                     ResetPosition();
                     
-                    // XXX cross-check the state reset
-                    ResetStoredStates();
-
                     // do not fuse position data on this time
                     // step
                     fusePosData = false;
@@ -1183,7 +1179,6 @@ void AttPosEKF::FuseVelposNED()
                 // the height data, but reset height and stored states
                 if (current_ekf_state.hgtTimeout) {
                     ResetHeight();
-                    ResetStoredStates();
                     fuseHgtData = false;
                 }
             }
@@ -2855,6 +2850,12 @@ void AttPosEKF::ResetPosition(void)
         // reset the states from the GPS measurements
         states[7] = posNE[0];
         states[8] = posNE[1];
+
+        // stored horizontal position states to prevent subsequent GPS measurements from being rejected
+        for (size_t i = 0; i < EKF_DATA_BUFFER_SIZE; ++i){
+            storedStates[7][i] = states[7];
+            storedStates[8][i] = states[8];
+        }        
     }
 }
 
@@ -2862,6 +2863,11 @@ void AttPosEKF::ResetHeight(void)
 {
     // write to the state vector
     states[9]   = -hgtMea;
+
+    // stored horizontal position states to prevent subsequent Barometer measurements from being rejected
+    for (size_t i = 0; i < EKF_DATA_BUFFER_SIZE; ++i){
+        storedStates[9][i] = states[9];
+    }    
 }
 
 void AttPosEKF::ResetVelocity(void)
@@ -2871,10 +2877,16 @@ void AttPosEKF::ResetVelocity(void)
         states[5] = 0.0f;
         states[6] = 0.0f;
     } else if (GPSstatus >= GPS_FIX_3D) {
+        //Do not use Z velocity, we trust the Barometer history more
 
         states[4]  = velNED[0]; // north velocity from last reading
         states[5]  = velNED[1]; // east velocity from last reading
-        states[6]  = velNED[2]; // down velocity from last reading
+
+        // stored horizontal position states to prevent subsequent GPS measurements from being rejected
+        for (size_t i = 0; i < EKF_DATA_BUFFER_SIZE; ++i){
+            storedStates[4][i] = states[4];
+            storedStates[5][i] = states[5];
+        }                
     }
 }
 
@@ -2992,10 +3004,10 @@ int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error)
         // Fill error report
         GetFilterState(&last_ekf_error);
 
+        ResetStoredStates();
         ResetVelocity();
         ResetPosition();
         ResetHeight();
-        ResetStoredStates();
 
         // Timeout cleared with this reset
         current_ekf_state.imuTimeout = false;
@@ -3009,10 +3021,10 @@ int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error)
         // Fill error report, but not setting error flag
         GetFilterState(&last_ekf_error);
 
+        ResetStoredStates();
         ResetVelocity();
         ResetPosition();
         ResetHeight();
-        ResetStoredStates();
 
         ret = 0;
     }
@@ -3182,6 +3194,7 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
     states[20] = magBias.y; // Magnetic Field Bias Y
     states[21] = magBias.z; // Magnetic Field Bias Z
 
+    ResetStoredStates();
     ResetVelocity();
     ResetPosition();
     ResetHeight();
diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h
index 8e820bfd9..070b6a63c 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_22states.h
+++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h
@@ -298,8 +298,6 @@ public:
 
     void RecallOmega(float *omegaForFusion, uint64_t msec);
 
-    void ResetStoredStates();
-
     void quat2Tbn(Mat3f &TBodyNed, const float (&quat)[4]);
 
     void calcEarthRateNED(Vector3f &omega, float latitude);