Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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.
This commit is contained in:
priseborough 2016-07-04 21:11:01 +10:00 committed by Andrew Tridgell
parent a49c16d63c
commit 744f19cd2d
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -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