diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 6aa56edf41..a82298fa53 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -788,7 +788,7 @@ void NavEKF::SelectVelPosFusion() fuseVelData = false; fusePosData = false; } - } else if (constPosMode ) { + } else if (constPosMode && covPredStep) { // in constant position mode use synthetic position measurements set to zero // only fuse synthetic measurements when rate of change of velocity is less than 0.5g to reduce attitude errors due to launch acceleration // do not use velocity fusion to reduce the effect of movement on attitude @@ -798,7 +798,7 @@ void NavEKF::SelectVelPosFusion() fusePosData = false; } fuseVelData = false; - } else if (constVelMode) { + } else if (constVelMode && covPredStep) { // In constant velocity mode we fuse the last valid velocity vector // Reset the stored velocity vector when we enter the mode if (constVelMode && !lastConstVelMode) {