From 744f19cd2de1cb8c3b7d9d8c356a9ab28bf49f65 Mon Sep 17 00:00:00 2001 From: priseborough Date: Mon, 4 Jul 2016 21:11:01 +1000 Subject: [PATCH] AP_NavEKF2: Improve scaling of output predictor I gain Provide consistent overshoot of 5% across a wider range of time constants and prevent selection of larger time constants causing 'ringing' in the position and velocity outputs. --- libraries/AP_NavEKF2/AP_NavEKF2_core.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index ca8047640b..c098f51fd2 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -652,8 +652,8 @@ void NavEKF2_core::calcOutputStates() float velPosGain = dtEkfAvg / constrain_float(tauPosVel, dtEkfAvg, 10.0f); // use a PI feedback to calculate a correction that will be applied to the output state history - posCorrection += posDelta * velPosGain * 0.9f; // I term - velCorrection += velDelta * velPosGain * 0.9f; // I term + posCorrection += posDelta * sq(velPosGain) * 0.1f; // I term + velCorrection += velDelta * sq(velPosGain) * 0.1f; // I term velDelta *= velPosGain; // P term posDelta *= velPosGain; // P term