diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 33b1f7974c..22040583c9 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -963,11 +963,7 @@ void NavEKF3_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat, } extNavDataNew.pos = pos; - if (posErr > 0) { - extNavDataNew.posErr = posErr; - } else { - extNavDataNew.posErr = frontend->_gpsHorizPosNoise; - } + extNavDataNew.posErr = posErr; // calculate timestamp timeStamp_ms = timeStamp_ms - delay_ms;