From d618c55e2ff52c7fad943bbeca7b85a03b8079e5 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 6 Apr 2015 21:17:00 +1000 Subject: [PATCH] AP_NavEKF: Improved handling of noisy GPS speed accuracy data --- libraries/AP_NavEKF/AP_NavEKF.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 56f24cf243..cdb0366f30 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -4004,9 +4004,15 @@ void NavEKF::readGpsData() // read the NED velocity from the GPS velNED = _ahrs->get_gps().velocity(); - // use the speed accuracy from the GPS if available, otherwise set it to zero. - if (!_ahrs->get_gps().speed_accuracy(gpsSpdAccuracy)) { + // Use the speed accuracy from the GPS if available, otherwise set it to zero. + // Apply a decaying envelope filter with a 5 second time constant to the raw speed accuracy data + float alpha = constrain_float(0.0002f * (lastFixTime_ms - secondLastFixTime_ms),0.0f,1.0f); + gpsSpdAccuracy *= (1.0f - alpha); + float gpsSpdAccRaw; + if (!_ahrs->get_gps().speed_accuracy(gpsSpdAccRaw)) { gpsSpdAccuracy = 0.0f; + } else { + gpsSpdAccuracy = max(gpsSpdAccuracy,gpsSpdAccRaw); } // check if we have enough GPS satellites and increase the gps noise scaler if we don't