diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 004b6722f8..6ef7504cee 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -686,17 +686,16 @@ void NavEKF::SelectVelPosFusion() } } else { - // in static mode we only fuse position and height to improve long term numerical stability - // and only when the rate of change of velocity is less than 0.5g. This prevents attitude errors - // due to launch acceleration. position is assumed to be zero. - fuseVelData = false; + // in static 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 if (accNavMag < 4.9f) { fusePosData = true; - fuseHgtData = true; } else { fusePosData = false; - fuseHgtData = false; } + fuseVelData = false; + fuseHgtData = true; } // check for and read new height data