fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: handle air data fusion covariance reset consistently

Browse Source 
Both the sideslip and airspeed fusion should not be resetting covariances for states they do not modify.
This commit is contained in:
Paul Riseborough 2017-10-20 10:56:24 +11:00
parent 0d32128701
commit 55a2dc94df
1 changed files with 13 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
EKF/airspeed_fusion.cpp
View File

@ -98,8 +98,19 @@ void Ekf::fuseAirspeed()
} else { // Reset the estimator covarinace matrix
_fault_status.flags.bad_airspeed = true;
initialiseCovariance();
ECL_ERR("EKF airspeed fusion numerical error - covariance reset");
// if we are getting aiding from other sources, warn and reset the wind states and covariances only
if (update_wind_only) {
resetWindStates();
resetWindCovariance();
ECL_ERR("EKF airspeed fusion badly conditioned - wind covariance reset");
} else {
initialiseCovariance();
_state.wind_vel.setZero();
ECL_ERR("EKF airspeed fusion badly conditioned - full covariance reset");
}
return;
}