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

AP_NavEKF2: Update output filter tuning 

Updated tuning to take advantage of corrected time delta dtEkfAvg
This commit is contained in:
priseborough 2016-07-04 16:20:11 +10:00 committed by Andrew Tridgell
parent 55dee347e0
commit a49c16d63c
2 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -455,10 +455,10 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
// @Param: TAU_OUTPUT // @Param: TAU_OUTPUT
// @DisplayName: Output complementary filter time constant (centi-sec) // @DisplayName: Output complementary filter time constant (centi-sec)
// @Description: Sets the time constant of the output complementary filter/predictor in centi-seconds. // @Description: Sets the time constant of the output complementary filter/predictor in centi-seconds.
// @Range: 5 30 // @Range: 10 50
// @Increment: 5 // @Increment: 5
// @User: Advanced // @User: Advanced
AP_GROUPINFO("TAU_OUTPUT", 39, NavEKF2, _tauVelPosOutput, 20), AP_GROUPINFO("TAU_OUTPUT", 39, NavEKF2, _tauVelPosOutput, 25),
// @Param: MAGE_P_NSE // @Param: MAGE_P_NSE
// @DisplayName: Earth magnetic field process noise (gauss/s) // @DisplayName: Earth magnetic field process noise (gauss/s)

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); 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 // use a PI feedback to calculate a correction that will be applied to the output state history
posCorrection += posDelta * velPosGain; // I term posCorrection += posDelta * velPosGain * 0.9f; // I term
velCorrection += velDelta * velPosGain; // I term velCorrection += velDelta * velPosGain * 0.9f; // I term
velDelta *= velPosGain; // P term velDelta *= velPosGain; // P term
posDelta *= velPosGain; // P term posDelta *= velPosGain; // P term