From aa5335b16e7dd06ab3120306f8134f2af237c973 Mon Sep 17 00:00:00 2001 From: priseborough Date: Thu, 3 Apr 2014 05:15:59 +1100 Subject: [PATCH] AP_NavEKF : Improved GPS position glitch handling When using GPS after previously rejecting it, the GPS position will always be offset if outside the specified glitch radius. This was the original intent of the design and makes handling of glitches smoother. It has been tested on replay using glitchy flight data --- libraries/AP_NavEKF/AP_NavEKF.cpp | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 5da6b44e3a..6073895ea5 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -1633,19 +1633,17 @@ void NavEKF::FuseVelPosNED() float maxPosInnov2 = sq(_gpsPosInnovGate * _gpsHorizPosNoise + 0.005f * accelScale * float(_gpsGlitchAccelMax) * sq(0.001f * float(hal.scheduler->millis() - posFailTime))); posTestRatio = (sq(posInnov[0]) + sq(posInnov[1])) / maxPosInnov2; posHealth = ((posTestRatio < 1.0f) || badIMUdata); - // if we have timed out or exceeded the max radius, then we offset the GPS position to match the inertial solution and then decay the - // offset to bring the vehicle back gradually to the new GPS position - posTimeout = ((maxPosInnov2 > sq(float(_gpsGlitchRadiusMax))) || ((hal.scheduler->millis() - posFailTime) > gpsRetryTime)); - // fuse position data if healthy, timed out, or in static mode + // declare a timeout condition if we have been too long without data + posTimeout = ((hal.scheduler->millis() - posFailTime) > gpsRetryTime); + // use position data if healthy, timed out, or in static mode if (posHealth || posTimeout || staticMode) { posHealth = true; posFailTime = hal.scheduler->millis(); - // if timed out, increment the offset applied to GPS data to compensate for large GPS position jumps + // if timed out or outside the specified glitch radius, increment the offset applied to GPS data to compensate for large GPS position jumps // offset is decayed to zero at 1.0 m/s and limited to a maximum value of 100m before it is applied to // subsequent GPS measurements so we don't have to do any limiting here - // increment the offset applied to future reads from the GPS - if (posTimeout) { + if (posTimeout || (maxPosInnov2 > sq(float(_gpsGlitchRadiusMax)))) { posnOffsetNorth += posInnov[0]; posnOffsetEast += posInnov[1]; // apply the offset to the current GPS measurement