From 84421e0a355e860821e6342cc08817aab8a51884 Mon Sep 17 00:00:00 2001 From: priseborough Date: Sat, 1 Nov 2014 11:00:56 +1100 Subject: [PATCH] AP_NavEKF : Adjust initial flow scale factor and limits --- libraries/AP_NavEKF/AP_NavEKF.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 285a3a6f08..12bac1bff9 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -2648,7 +2648,7 @@ void NavEKF::RunAuxiliaryEKF() flowStates[i] -= K_RNG[i] * innovRng; } // constrain the states - flowStates[0] = constrain_float(flowStates[0], 0.1f, 10.0f); + flowStates[0] = constrain_float(flowStates[0], 0.8f, 1.25f); flowStates[1] = max(flowStates[1], statesAtRngTime.position[2] + 0.1f); // correct the covariance matrix @@ -2771,7 +2771,7 @@ void NavEKF::RunAuxiliaryEKF() flowStates[i] -= K_OPT[i][obsIndex] * auxFlowObsInnov[obsIndex]; } // constrain the states - flowStates[0] = constrain_float(flowStates[0], 0.1f, 10.0f); + flowStates[0] = constrain_float(flowStates[0], 0.8f, 1.25f); flowStates[1] = max(flowStates[1], statesAtFlowTime.position[2] + 0.1f); // correct the covariance matrix