diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index af630c694c..d194a31945 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -1181,11 +1181,6 @@ void NavEKF3_core::selectHeightForFusion() if (dal.get_takeoff_expected() || dal.get_touchdown_expected()) { posDownObsNoise *= frontend->gndEffectBaroScaler; } - // If we are in takeoff mode, the height measurement is limited to be no less than the measurement at start of takeoff - // This prevents negative baro disturbances due to rotor wash ground interaction corrupting the EKF altitude during initial ascent - if (motorsArmed && dal.get_takeoff_expected() && !assume_zero_sideslip()) { - hgtMea = MAX(hgtMea, meaHgtAtTakeOff); - } velPosObs[5] = -hgtMea; } else { fuseHgtData = false;