AP_NavEKF2: Remove GPS glitch offset logic

Correction for steps in position and velocity caused by resets following GPS glitches and other events are now handled by the control loops.

libraries/AP_NavEKF2/AP_NavEKF2.cpp

@@ -817,4 +817,24 @@ uint32_t NavEKF2::getLastYawResetAngle(float &yawAng)
     return core->getLastYawResetAngle(yawAng);
 }
 
+// return the amount of NE position change due to the last position reset in metres
+// returns the time of the last reset or 0 if no reset has ever occurred
+uint32_t NavEKF2::getLastPosNorthEastReset(Vector2f &pos)
+{
+    if (!core) {
+        return false;
+    }
+    return core->getLastPosNorthEastReset(pos);
+}
+
+// return the amount of NE velocity change due to the last velocity reset in metres/sec
+// returns the time of the last reset or 0 if no reset has ever occurred
+uint32_t NavEKF2::getLastVelNorthEastReset(Vector2f &vel)
+{
+    if (!core) {
+        return false;
+    }
+    return core->getLastVelNorthEastReset(vel);
+}
+
 #endif //HAL_CPU_CLASS

libraries/AP_NavEKF2/AP_NavEKF2.h

@@ -227,6 +227,14 @@ public:
     // returns the time of the last yaw angle reset or 0 if no reset has ever occurred
     uint32_t getLastYawResetAngle(float &yawAng);
 
+    // return the amount of NE position change due to the last position reset in metres
+    // returns the time of the last reset or 0 if no reset has ever occurred
+    uint32_t getLastPosNorthEastReset(Vector2f &pos);
+
+    // return the amount of NE velocity change due to the last velocity reset in metres/sec
+    // returns the time of the last reset or 0 if no reset has ever occurred
+    uint32_t getLastVelNorthEastReset(Vector2f &vel);
+
     // allow the enable flag to be set by Replay
     void set_enable(bool enable) { _enable.set(enable); }
     

libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp

@@ -54,8 +54,8 @@ void NavEKF2_core::FuseAirspeed()
     vwn = stateStruct.wind_vel.x;
     vwe = stateStruct.wind_vel.y;
 
-    // calculate the predicted airspeed, compensating for bias in GPS velocity when we are pulling a glitch offset back in
-    VtasPred = pythagorous3((ve - gpsVelGlitchOffset.y - vwe) , (vn - gpsVelGlitchOffset.x - vwn) , vd);
+    // calculate the predicted airspeed
+    VtasPred = pythagorous3((ve - vwe) , (vn - vwn) , vd);
     // perform fusion of True Airspeed measurement
     if (VtasPred > 1.0f)
     {
@@ -330,9 +330,9 @@ void NavEKF2_core::FuseSideslip()
     vwn = stateStruct.wind_vel.x;
     vwe = stateStruct.wind_vel.y;
 
-    // calculate predicted wind relative velocity in NED, compensating for offset in velcity when we are pulling a GPS glitch offset back in
-    vel_rel_wind.x = vn - vwn - gpsVelGlitchOffset.x;
-    vel_rel_wind.y = ve - vwe - gpsVelGlitchOffset.y;
+    // calculate predicted wind relative velocity in NED
+    vel_rel_wind.x = vn - vwn;
+    vel_rel_wind.y = ve - vwe;
     vel_rel_wind.z = vd;
 
     // rotate into body axes

libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp

@@ -531,9 +531,6 @@ void NavEKF2_core::readGpsData()
                 }
             }
 
-            // calculate a position offset which is applied to NE position and velocity wherever it is used throughout code to allow GPS position jumps to be accommodated gradually
-            decayGpsOffset();
-
             // save measurement to buffer to be fused later
             StoreGPS();
 
@@ -584,10 +581,6 @@ void NavEKF2_core::readGpsData()
                     // put the filter into constant position mode
                     PV_AidingMode = AID_NONE;
 
-                    // reset all glitch states
-                    gpsPosGlitchOffsetNE.zero();
-                    gpsVelGlitchOffset.zero();
-
                     // Reset the velocity and position states
                     ResetVelocity();
                     ResetPosition();
@@ -654,34 +647,6 @@ bool NavEKF2_core::readDeltaAngle(uint8_t ins_index, Vector3f &dAng) {
 }
 
 
-// decay GPS horizontal position offset to close to zero at a rate of 1 m/s for copters and 5 m/s for planes
-// limit radius to a maximum of 50m
-void NavEKF2_core::decayGpsOffset()
-{
-    float offsetDecaySpd;
-    if (assume_zero_sideslip()) {
-        offsetDecaySpd = 5.0f;
-    } else {
-        offsetDecaySpd = 1.0f;
-    }
-    float lapsedTime = 0.001f*float(imuSampleTime_ms - lastDecayTime_ms);
-    lastDecayTime_ms = imuSampleTime_ms;
-    float offsetRadius = pythagorous2(gpsPosGlitchOffsetNE.x, gpsPosGlitchOffsetNE.y);
-    // decay radius if larger than offset decay speed multiplied by lapsed time (plus a margin to prevent divide by zero)
-    if (offsetRadius > (offsetDecaySpd * lapsedTime + 0.1f)) {
-        // Calculate the GPS velocity offset required. This is necessary to prevent the position measurement being rejected for inconsistency when the radius is being pulled back in.
-        gpsVelGlitchOffset = -gpsPosGlitchOffsetNE*offsetDecaySpd/offsetRadius;
-        // calculate scale factor to be applied to both offset components
-        float scaleFactor = constrain_float((offsetRadius - offsetDecaySpd * lapsedTime), 0.0f, 50.0f) / offsetRadius;
-        gpsPosGlitchOffsetNE.x *= scaleFactor;
-        gpsPosGlitchOffsetNE.y *= scaleFactor;
-    } else {
-        gpsVelGlitchOffset.zero();
-        gpsPosGlitchOffsetNE.zero();
-    }
-}
-
-
 /********************************************************
 *                  Height Measurements                  *
 ********************************************************/

libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp

@@ -183,7 +183,6 @@ void NavEKF2_core::EstimateTerrainOffset()
 
         if (fuseOptFlowData) {
 
-            Vector3f vel; // velocity of sensor relative to ground in NED axes
             Vector3f relVelSensor; // velocity of sensor relative to ground in sensor axes
             float losPred; // predicted optical flow angular rate measurement
             float q0 = stateStruct.quat[0]; // quaternion at optical flow measurement time
@@ -193,11 +192,6 @@ void NavEKF2_core::EstimateTerrainOffset()
             float K_OPT;
             float H_OPT;
 
-            // Correct velocities for GPS glitch recovery offset
-            vel.x          = stateStruct.velocity[0] - gpsVelGlitchOffset.x;
-            vel.y          = stateStruct.velocity[1] - gpsVelGlitchOffset.y;
-            vel.z          = stateStruct.velocity[2];
-
             // predict range to centre of image
             float flowRngPred = max((terrainState - stateStruct.position[2]),rngOnGnd) / Tnb_flow.c.z;
 
@@ -205,7 +199,7 @@ void NavEKF2_core::EstimateTerrainOffset()
             terrainState = max(terrainState, stateStruct.position[2] + rngOnGnd);
 
             // calculate relative velocity in sensor frame
-            relVelSensor = Tnb_flow*vel;
+            relVelSensor = Tnb_flow*stateStruct.velocity;
 
             // divide velocity by range, subtract body rates and apply scale factor to
             // get predicted sensed angular optical rates relative to X and Y sensor axes
@@ -222,25 +216,25 @@ void NavEKF2_core::EstimateTerrainOffset()
             float t10 = q0*q3*2.0f;
             float t11 = q1*q2*2.0f;
             float t14 = t3+t4-t5-t6;
-            float t15 = t14*vel.x;
+            float t15 = t14*stateStruct.velocity.x;
             float t16 = t10+t11;
-            float t17 = t16*vel.y;
+            float t17 = t16*stateStruct.velocity.y;
             float t18 = q0*q2*2.0f;
             float t19 = q1*q3*2.0f;
             float t20 = t18-t19;
-            float t21 = t20*vel.z;
+            float t21 = t20*stateStruct.velocity.z;
             float t2 = t15+t17-t21;
             float t7 = t3-t4-t5+t6;
             float t8 = stateStruct.position[2]-terrainState;
             float t9 = 1.0f/sq(t8);
             float t24 = t3-t4+t5-t6;
-            float t25 = t24*vel.y;
+            float t25 = t24*stateStruct.velocity.y;
             float t26 = t10-t11;
-            float t27 = t26*vel.x;
+            float t27 = t26*stateStruct.velocity.x;
             float t28 = q0*q1*2.0f;
             float t29 = q2*q3*2.0f;
             float t30 = t28+t29;
-            float t31 = t30*vel.z;
+            float t31 = t30*stateStruct.velocity.z;
             float t12 = t25-t27+t31;
             float t13 = sq(t7);
             float t22 = sq(t2);
@@ -288,7 +282,6 @@ void NavEKF2_core::EstimateTerrainOffset()
 void NavEKF2_core::FuseOptFlow()
 {
     Vector24 H_LOS;
-    Vector3f velNED_local;
     Vector3f relVelSensor;
     Vector14 SH_LOS;
     Vector2 losPred;
@@ -303,11 +296,6 @@ void NavEKF2_core::FuseOptFlow()
     float vd = stateStruct.velocity.z;
     float pd = stateStruct.position.z;
 
-    // Correct velocities for GPS glitch recovery offset
-    velNED_local.x = vn - gpsVelGlitchOffset.x;
-    velNED_local.y = ve - gpsVelGlitchOffset.y;
-    velNED_local.z = vd;
-
     // constrain height above ground to be above range measured on ground
     float heightAboveGndEst = max((terrainState - pd), rngOnGnd);
     float ptd = pd + heightAboveGndEst;
@@ -334,7 +322,7 @@ void NavEKF2_core::FuseOptFlow()
         float range = constrain_float((heightAboveGndEst/Tnb_flow.c.z),rngOnGnd,1000.0f);
 
         // calculate relative velocity in sensor frame
-        relVelSensor = Tnb_flow*velNED_local;
+        relVelSensor = Tnb_flow*stateStruct.velocity;
 
         // divide velocity by range  to get predicted angular LOS rates relative to X and Y axes
         losPred[0] =  relVelSensor.y/range;

libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp

@@ -337,7 +337,7 @@ void  NavEKF2_core::getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vecto
 
 // return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements
 // this indicates the amount of margin available when tuning the various error traps
-// also return the current offsets applied to the GPS position measurements
+// also return the delta in position due to the last position reset
 void  NavEKF2_core::getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const
 {
     velVar   = sqrtf(velTestRatio);
@@ -347,7 +347,7 @@ void  NavEKF2_core::getVariances(float &velVar, float &posVar, float &hgtVar, Ve
     magVar.y = sqrtf(magTestRatio.y);
     magVar.z = sqrtf(magTestRatio.z);
     tasVar   = sqrtf(tasTestRatio);
-    offset   = gpsPosGlitchOffsetNE;
+    offset   = posResetNE;
 }
 
 

libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp

@@ -21,12 +21,16 @@ extern const AP_HAL::HAL& hal;
 // Do not reset vertical velocity using GPS as there is baro alt available to constrain drift
 void NavEKF2_core::ResetVelocity(void)
 {
+    // Store the position before the reset so that we can record the reset delta
+    velResetNE.x = stateStruct.velocity.x;
+    velResetNE.y = stateStruct.velocity.y;
+
     if (PV_AidingMode != AID_ABSOLUTE) {
         stateStruct.velocity.zero();
     } else if (!gpsNotAvailable) {
-        // reset horizontal velocity states, applying an offset to the GPS velocity to prevent the GPS position being rejected when the GPS position offset is being decayed to zero.
-        stateStruct.velocity.x  = gpsDataNew.vel.x + gpsVelGlitchOffset.x; // north velocity from blended accel data
-        stateStruct.velocity.y  = gpsDataNew.vel.y + gpsVelGlitchOffset.y; // east velocity from blended accel data
+        // reset horizontal velocity states to the GPS velocity
+        stateStruct.velocity.x  = gpsDataNew.vel.x; // north velocity from blended accel data
+        stateStruct.velocity.y  = gpsDataNew.vel.y; // east velocity from blended accel data
     }
     for (uint8_t i=0; i<IMU_BUFFER_LENGTH; i++) {
         storedOutput[i].velocity.x = stateStruct.velocity.x;
@@ -36,19 +40,30 @@ void NavEKF2_core::ResetVelocity(void)
     outputDataNew.velocity.y = stateStruct.velocity.y;
     outputDataDelayed.velocity.x = stateStruct.velocity.x;
     outputDataDelayed.velocity.y = stateStruct.velocity.y;
+
+    // Calculate the position jump due to the reset
+    velResetNE.x = stateStruct.velocity.x - velResetNE.x;
+    velResetNE.y = stateStruct.velocity.y - velResetNE.y;
+
+    // store the time of the reset
+    lastVelReset_ms = imuSampleTime_ms;
 }
 
 // resets position states to last GPS measurement or to zero if in constant position mode
 void NavEKF2_core::ResetPosition(void)
 {
+    // Store the position before the reset so that we can record the reset delta
+    posResetNE.x = stateStruct.position.x;
+    posResetNE.y = stateStruct.position.y;
+
     if (PV_AidingMode != AID_ABSOLUTE) {
         // reset all position state history to the last known position
         stateStruct.position.x = lastKnownPositionNE.x;
         stateStruct.position.y = lastKnownPositionNE.y;
     } else if (!gpsNotAvailable) {
         // write to state vector and compensate for offset  between last GPs measurement and the EKF time horizon
-        stateStruct.position.x = gpsDataNew.pos.x + gpsPosGlitchOffsetNE.x + 0.001f*gpsDataNew.vel.x*(float(imuDataDelayed.time_ms) - float(lastTimeGpsReceived_ms));
-        stateStruct.position.y = gpsDataNew.pos.y + gpsPosGlitchOffsetNE.y + 0.001f*gpsDataNew.vel.y*(float(imuDataDelayed.time_ms) - float(lastTimeGpsReceived_ms));
+        stateStruct.position.x = gpsDataNew.pos.x  + 0.001f*gpsDataNew.vel.x*(float(imuDataDelayed.time_ms) - float(lastTimeGpsReceived_ms));
+        stateStruct.position.y = gpsDataNew.pos.y  + 0.001f*gpsDataNew.vel.y*(float(imuDataDelayed.time_ms) - float(lastTimeGpsReceived_ms));
     }
     for (uint8_t i=0; i<IMU_BUFFER_LENGTH; i++) {
         storedOutput[i].position.x = stateStruct.position.x;
@@ -58,6 +73,13 @@ void NavEKF2_core::ResetPosition(void)
     outputDataNew.position.y = stateStruct.position.y;
     outputDataDelayed.position.x = stateStruct.position.x;
     outputDataDelayed.position.y = stateStruct.position.y;
+
+    // Calculate the position jump due to the reset
+    posResetNE.x = stateStruct.position.x - posResetNE.x;
+    posResetNE.y = stateStruct.position.y - posResetNE.y;
+
+    // store the time of the reset
+    lastPosReset_ms = imuSampleTime_ms;
 }
 
 // reset the vertical position state using the last height measurement
@@ -218,11 +240,11 @@ void NavEKF2_core::FuseVelPosNED()
         else gpsRetryTime = frontend.gpsRetryTimeNoTAS_ms;
 
         // form the observation vector
-        observation[0] = gpsDataDelayed.vel.x + gpsVelGlitchOffset.x;
-        observation[1] = gpsDataDelayed.vel.y + gpsVelGlitchOffset.y;
+        observation[0] = gpsDataDelayed.vel.x;
+        observation[1] = gpsDataDelayed.vel.y;
         observation[2] = gpsDataDelayed.vel.z;
-        observation[3] = gpsDataDelayed.pos.x + gpsPosGlitchOffsetNE.x;
-        observation[4] = gpsDataDelayed.pos.y + gpsPosGlitchOffsetNE.y;
+        observation[3] = gpsDataDelayed.pos.x;
+        observation[4] = gpsDataDelayed.pos.y;
         observation[5] = -baroDataDelayed.hgt;
 
         // calculate additional error in GPS position caused by manoeuvring
@@ -304,13 +326,9 @@ void NavEKF2_core::FuseVelPosNED()
                 // only reset the failed time and do glitch timeout checks if we are doing full aiding
                 if (PV_AidingMode == AID_ABSOLUTE) {
                     lastPosPassTime_ms = imuSampleTime_ms;
-                    // if timed out or outside the specified uncertainty radius, increment the offset applied to GPS data to compensate for large GPS position jumps
+                    // if timed out or outside the specified uncertainty radius, reset to the GPS
                     if (posTimeout || ((P[6][6] + P[7][7]) > sq(float(frontend._gpsGlitchRadiusMax)))) {
-                        gpsPosGlitchOffsetNE.x += innovVelPos[3];
-                        gpsPosGlitchOffsetNE.y += innovVelPos[4];
-                        // limit the radius of the offset and decay the offset to zero radially
-                        decayGpsOffset();
-                        // reset the position to the current GPS position which will include the glitch correction offset
+                        // reset the position to the current GPS position
                         ResetPosition();
                         // reset the velocity to the GPS velocity
                         ResetVelocity();

libraries/AP_NavEKF2/AP_NavEKF2_core.cpp

@@ -91,6 +91,8 @@ void NavEKF2_core::InitialiseVariables()
     timeTasReceived_ms = 0;
     magYawResetTimer_ms = imuSampleTime_ms;
     lastPreAlignGpsCheckTime_ms = imuSampleTime_ms;
+    lastPosReset_ms = 0;
+    lastVelReset_ms = 0;
 
     // initialise other variables
     gpsNoiseScaler = 1.0f;
@@ -106,7 +108,6 @@ void NavEKF2_core::InitialiseVariables()
     velDotNEDfilt.zero();
     summedDelAng.zero();
     summedDelVel.zero();
-    gpsPosGlitchOffsetNE.zero();
     lastKnownPositionNE.zero();
     prevTnb.zero();
     memset(&P[0][0], 0, sizeof(P));
@@ -127,7 +128,6 @@ void NavEKF2_core::InitialiseVariables()
     PV_AidingMode = AID_NONE;
     posTimeout = true;
     velTimeout = true;
-    gpsVelGlitchOffset.zero();
     isAiding = false;
     prevIsAiding = false;
     memset(&faultStatus, 0, sizeof(faultStatus));
@@ -195,6 +195,8 @@ void NavEKF2_core::InitialiseVariables()
     airSpdFusionDelayed = false;
     sideSlipFusionDelayed = false;
     magFuseTiltInhibit = false;
+    posResetNE.zero();
+    velResetNE.zero();
 }
 
 // Initialise the states from accelerometer and magnetometer data (if present)

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -531,10 +531,6 @@ private:
     // return true if the vehicle code has requested the filter to be ready for flight
     bool readyToUseGPS(void) const;
 
-    // decay GPS horizontal position offset to close to zero at a rate of 1 m/s
-    // this allows large GPS position jumps to be accomodated gradually
-    void decayGpsOffset(void);
-
     // Check for filter divergence
     void checkDivergence(void);
 
@@ -708,7 +704,6 @@ private:
     Vector8 SQ;                     // intermediate variables used to calculate predicted covariance matrix
     Vector23 SPP;                   // intermediate variables used to calculate predicted covariance matrix
     bool yawAligned;                // true when the yaw angle has been aligned
-    Vector2f gpsPosGlitchOffsetNE;  // offset applied to GPS data in the NE direction to compensate for rapid changes in GPS solution
     Vector2f lastKnownPositionNE;   // last known position
     uint32_t lastDecayTime_ms;      // time of last decay of GPS position offset
     float velTestRatio;             // sum of squares of GPS velocity innovation divided by fail threshold
@@ -720,7 +715,6 @@ private:
     bool inhibitMagStates;          // true when magnetic field states and covariances are to remain constant
     bool firstMagYawInit;           // true when the first post takeoff initialisation of earth field and yaw angle has been performed
     bool secondMagYawInit;          // true when the second post takeoff initialisation of earth field and yaw angle has been performed
-    Vector2f gpsVelGlitchOffset;    // Offset applied to the GPS velocity when the gltch radius is being  decayed back to zero
     bool gpsNotAvailable;           // bool true when valid GPS data is not available
     bool isAiding;                  // true when the filter is fusing position, velocity or flow measurements
     bool prevIsAiding;              // isAiding from previous frame