diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index b6ae6e96cc..54cdab8aa9 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -543,6 +543,7 @@ void NavEKF3_core::readGpsData() } else { gpsSpdAccuracy = MAX(gpsSpdAccuracy,gpsSpdAccRaw); gpsSpdAccuracy = MIN(gpsSpdAccuracy,50.0f); + gpsSpdAccuracy = MAX(gpsSpdAccuracy,frontend->_gpsHorizVelNoise); } gpsPosAccuracy *= (1.0f - alpha); float gpsPosAccRaw; @@ -551,6 +552,7 @@ void NavEKF3_core::readGpsData() } else { gpsPosAccuracy = MAX(gpsPosAccuracy,gpsPosAccRaw); gpsPosAccuracy = MIN(gpsPosAccuracy,100.0f); + gpsPosAccuracy = MAX(gpsPosAccuracy, frontend->_gpsHorizPosNoise); } gpsHgtAccuracy *= (1.0f - alpha); float gpsHgtAccRaw; @@ -559,6 +561,7 @@ void NavEKF3_core::readGpsData() } else { gpsHgtAccuracy = MAX(gpsHgtAccuracy,gpsHgtAccRaw); gpsHgtAccuracy = MIN(gpsHgtAccuracy,100.0f); + gpsHgtAccuracy = MAX(gpsHgtAccuracy, 1.5f * frontend->_gpsHorizPosNoise); } // check if we have enough GPS satellites and increase the gps noise scaler if we don't