fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-13 09:23:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Don't try to fuse air data if not in normal aiding mode

Browse Source 
Taking off before passing GPS checks would cause airspeed or sideslip to be fused when the filter was still using a constant position assumption. This would cause large airspeed innovations, invalid wind estimates and degrade filter performance after GPS was gained and position and velocity was reset.
This commit is contained in:
Paul Riseborough
2016-11-26 10:28:58 +01:00
committed by Lorenz Meier
parent 5ad329b641
commit 460c9e5250
1 changed files with 11 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+11 -4
View File
@@ -761,8 +761,9 @@ void Ekf::controlAirDataFusion()
}
// Always try to fuse airspeed data if available and we are in flight
if (_tas_data_ready && _control_status.flags.in_air) {
// Always try to fuse airspeed data if available and we are in flight and the filter is operating in a normal aiding mode
bool is_aiding = _control_status.flags.gps || _control_status.flags.opt_flow || _control_status.flags.ev_pos;
if (_tas_data_ready && _control_status.flags.in_air && is_aiding) {
// If starting wind state estimation, reset the wind states and covariances before fusing any data
if (!_control_status.flags.wind) {
// activate the wind states
@@ -793,9 +794,15 @@ void Ekf::controlBetaFusion()
}
// Attempt synthetic sideslip fusion at the prescribed rate if we are in-flight and fusion has been enabled
// Perform synthetic sideslip fusion when in-air and sideslip fuson had been enabled externally in addition to the following criteria:
// Suffient time has lapsed sice the last fusion
bool beta_fusion_time_triggered = _time_last_imu - _time_last_beta_fuse > _params.beta_avg_ft_us;
if(beta_fusion_time_triggered && _control_status.flags.fuse_beta && _control_status.flags.in_air){
// The filter is operating in a mode where velocity states can be used
bool vel_states_active = _control_status.flags.gps || _control_status.flags.opt_flow || _control_status.flags.ev_pos;
if(beta_fusion_time_triggered && _control_status.flags.fuse_beta && _control_status.flags.in_air && vel_states_active) {
// If starting wind state estimation, reset the wind states and covariances before fusing any data
if (!_control_status.flags.wind) {
// activate the wind states