aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF2: Clean up output observer and reduce pos vel time constant

Browse Source 
Previous Time constant was too large leading to poor tracking of EKF states in the presence of bad quality IMU data.
This commit is contained in:
priseborough 2016-06-29 17:17:37 +10:00 committed by Andrew Tridgell
parent b9f65996bc
commit 118d5b88b2
2 changed files with 3 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -458,7 +458,7 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
// @Range: -1 100
// @Increment: 10
// @User: Advanced
AP_GROUPINFO("TAU_OUTPUT", 39, NavEKF2, _tauVelPosOutput, 50),
AP_GROUPINFO("TAU_OUTPUT", 39, NavEKF2, _tauVelPosOutput, 10),
// @Param: MAGE_P_NSE
// @DisplayName: Earth magnetic field process noise (gauss/s)

9
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -642,16 +642,11 @@ void NavEKF2_core::calcOutputStates()
// convert time constant from centi-seconds to seconds
float tauPosVel = 0.01f*(float)frontend->_tauVelPosOutput;
// calculate a position correction that will be applied to the output state history
// calculate a position and velocity correction that will be applied to the output state history
// to track the EKF position states with the specified time constant
float velPosGain = dtEkfAvg / constrain_float(tauPosVel, dtEkfAvg, 10.0f);
Vector3f posDelta = (stateStruct.position - outputDataDelayed.position) * velPosGain;
// calculate a velocity correction that will be applied to the output state history
// to track the EKF velocity states with the specified time constant
Vector3f velDelta;
float velGain = dtEkfAvg / constrain_float(tauPosVel, dtEkfAvg, 10.0f);
velDelta = (stateStruct.velocity - outputDataDelayed.velocity) * velGain;
Vector3f velDelta = (stateStruct.velocity - outputDataDelayed.velocity) * velPosGain;
// loop through the output filter state history and apply the corrections to the velocity and position states
// this method is too expensive to use for the attitude states due to the quaternion operations required