diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 376c0ec596..e18b9fabd9 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -856,13 +856,13 @@ void NavEKF::UpdateStrapdownEquationsNED() // use weighted average of both IMU units for delta velocities correctedDelVel12 = correctedDelVel1 * IMU1_weighting + correctedDelVel2 * (1.0f - IMU1_weighting); - // save current measurements - prevDelAng = correctedDelAng; - // apply corrections for earths rotation rate and coning errors // % * - and + operators have been overloaded correctedDelAng = correctedDelAng - prevTnb * earthRateNED*dtIMU + (prevDelAng % correctedDelAng) * 8.333333e-2f; + // save current measurements + prevDelAng = correctedDelAng; + // convert the rotation vector to its equivalent quaternion rotationMag = correctedDelAng.length(); if (rotationMag < 1e-12f)