From d3c9a0aef13d1f930f662f28e67f2e09a64b1a2e Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 16 Feb 2016 18:42:59 +1100 Subject: [PATCH] AP_NavEKF: Limit heading innovations after the consistency check --- libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp index 7ae5cf9b57..3580a3dc20 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp @@ -684,13 +684,6 @@ void NavEKF2_core::fuseCompass() // Copy raw value to output variable used for data logging innovYaw = innovation; - // limit the innovation so that initial corrections are not too large - if (innovation > 0.5f) { - innovation = 0.5f; - } else if (innovation < -0.5f) { - innovation = -0.5f; - } - // calculate the innovation test ratio yawTestRatio = sq(innovation) / (sq(MAX(0.01f * (float)frontend->_magInnovGate, 1.0f)) * varInnov); @@ -706,6 +699,13 @@ void NavEKF2_core::fuseCompass() magHealth = true; } + // limit the innovation so that initial corrections are not too large + if (innovation > 0.5f) { + innovation = 0.5f; + } else if (innovation < -0.5f) { + innovation = -0.5f; + } + // correct the state vector stateStruct.angErr.zero(); for (uint8_t i=0; i<=stateIndexLim; i++) {