diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 9c93a285bb..416368549c 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -737,7 +737,7 @@ void NavEKF3_core::readBaroData() // 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 (dal.get_takeoff_expected()) { + if (dal.get_takeoff_expected() && !assume_zero_sideslip()) { baroDataNew.hgt = MAX(baroDataNew.hgt, meaHgtAtTakeOff); } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index 21dd4d68d1..a3ce94b4ef 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -1182,7 +1182,7 @@ void NavEKF3_core::selectHeightForFusion() } // 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()) { + if (motorsArmed && dal.get_takeoff_expected() && !assume_zero_sideslip()) { hgtMea = MAX(hgtMea, meaHgtAtTakeOff); } velPosObs[5] = -hgtMea;