AP_NavEKF3: Apply GPS quality checks following loss of 3D fix if velocity error is bounded

libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

@@ -337,8 +337,10 @@ void NavEKF3_core::setAidingMode()
             // Check if attitude drift has been constrained by a measurement source
             bool attAiding = posUsed || gpsVelUsed || optFlowUsed || airSpdUsed || dragUsed || rngBcnUsed || bodyOdmUsed;
 
-            // check if velocity drift has been constrained by a measurement source
-            bool velAiding = gpsVelUsed || airSpdUsed || dragUsed || optFlowUsed || bodyOdmUsed;
+            // Check if velocity drift has been constrained by a measurement source
+            // Currently these are all the same source as will stabilise attitude because we do not currently have
+            // a sensor that only observes attitude
+            velAiding = posUsed || gpsVelUsed || optFlowUsed || airSpdUsed || dragUsed || rngBcnUsed || bodyOdmUsed;
 
             // check if position drift has been constrained by a measurement source
             bool posAiding = posUsed || rngBcnUsed;

libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

@@ -542,6 +542,20 @@ void NavEKF3_core::readGpsData()
     // check for new GPS data
     const auto &gps = dal.gps();
 
+    if (gps.status(selected_gps) < AP_DAL_GPS::GPS_OK_FIX_3D) {
+        // The GPS has dropped lock so force quality checks to restart
+        gpsGoodToAlign = false;
+        lastGpsVelFail_ms = imuSampleTime_ms;
+        lastGpsVelPass_ms = 0;
+        if (filterStatus.flags.horiz_pos_rel && !filterStatus.flags.horiz_pos_abs) {
+            // If we can do dead reckoning with a data source other than GPS there is time to wait
+            // for GPS alignment checks to pass before using GPS inside the EKF.
+            waitingForGpsChecks = true;
+        } else {
+            waitingForGpsChecks = false;
+        }
+    }
+
     // limit update rate to avoid overflowing the FIFO buffer
     if (gps.last_message_time_ms(selected_gps) - lastTimeGpsReceived_ms <= frontend->sensorIntervalMin_ms) {
         return;

libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

@@ -494,7 +494,8 @@ void NavEKF3_core::SelectVelPosFusion()
     readGpsYawData();
 
     // get data that has now fallen behind the fusion time horizon
-    gpsDataToFuse = storedGPS.recall(gpsDataDelayed,imuDataDelayed.time_ms);
+    gpsDataToFuse = storedGPS.recall(gpsDataDelayed,imuDataDelayed.time_ms) && !waitingForGpsChecks;
+
     if (gpsDataToFuse) {
         CorrectGPSForAntennaOffset(gpsDataDelayed);
         // calculate innovations and variances for reporting purposes only
@@ -794,7 +795,7 @@ void NavEKF3_core::FuseVelPosNED()
                 // if timed out or outside the specified uncertainty radius, reset to the external sensor
                 // if velocity drift is being constrained, dont reset until gps passes quality checks
                 const bool posVarianceIsTooLarge = (frontend->_gpsGlitchRadiusMax > 0) && (P[8][8] + P[7][7]) > sq(ftype(frontend->_gpsGlitchRadiusMax));
-                if (posTimeout || posVarianceIsTooLarge) {
+                if ((posTimeout || posVarianceIsTooLarge) && (!velAiding || gpsGoodToAlign)) {
                     // reset the position to the current external sensor position
                     ResetPosition(resetDataSource::DEFAULT);
 

libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp

@@ -230,6 +230,10 @@ void NavEKF3_core::calcGpsGoodToAlign(void)
     } else if (gpsGoodToAlign && imuSampleTime_ms - lastGpsVelPass_ms > 5000) {
         gpsGoodToAlign = false;
     }
+
+    if (gpsGoodToAlign && waitingForGpsChecks) {
+        waitingForGpsChecks = false;
+    }
 }
 
 // update inflight calculaton that determines if GPS data is good enough for reliable navigation

libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

@@ -258,6 +258,8 @@ void NavEKF3_core::InitialiseVariables()
     PV_AidingModePrev = AID_NONE;
     posTimeout = true;
     velTimeout = true;
+    velAiding = false;
+    waitingForGpsChecks = false;
     memset(&faultStatus, 0, sizeof(faultStatus));
     hgtRate = 0.0f;
     onGround = true;

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -1037,6 +1037,8 @@ private:
     bool tasTimeout;                // boolean true if true airspeed measurements have failed for too long and have timed out
     bool dragTimeout;               // boolean true if drag measurements have failed for too long and have timed out
     bool badIMUdata;                // boolean true if the bad IMU data is detected
+    bool velAiding;                 // boolean true if the velocity drift is constrained by observations
+    bool waitingForGpsChecks;          // boolean true if the EKF should not retrieve GPS data from the buffer until quality checks have passed
     uint32_t badIMUdata_ms;         // time stamp bad IMU data was last detected
     uint32_t goodIMUdata_ms;        // time stamp good IMU data was last detected
     uint32_t vertVelVarClipCounter; // counter used to control reset of vertical velocity variance following collapse against the lower limit