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

EKF: Improve handling of non position mode large position innovations

Browse Source 
These changes fix a bug that allowed IMU gyro errors that cause large tilt errors prior to start of aiding not resulting in large innovation test ratios.
This commit is contained in:
Paul Riseborough
2021-01-21 20:55:42 +11:00
committed by Paul Riseborough
parent 99575d73e1
commit 556a195320
3 changed files with 7 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+4 -5
View File
@@ -300,7 +300,7 @@ void Ekf::controlExternalVisionFusion()
// innovation gate size
ev_pos_innov_gates(0) = fmaxf(_params.ev_pos_innov_gate, 1.0f);
fuseHorizontalPosition(_ev_pos_innov, ev_pos_innov_gates, ev_pos_obs_var, _ev_pos_innov_var, _ev_pos_test_ratio);
fuseHorizontalPosition(_ev_pos_innov, ev_pos_innov_gates, ev_pos_obs_var, _ev_pos_innov_var, _ev_pos_test_ratio, false);
}
// determine if we should use the velocity observations
@@ -712,7 +712,7 @@ void Ekf::controlGpsFusion()
// fuse GPS measurement
fuseHorizontalVelocity(_gps_vel_innov, gps_vel_innov_gates,gps_vel_obs_var, _gps_vel_innov_var, _gps_vel_test_ratio);
fuseVerticalVelocity(_gps_vel_innov, gps_vel_innov_gates, gps_vel_obs_var, _gps_vel_innov_var, _gps_vel_test_ratio);
fuseHorizontalPosition(_gps_pos_innov, gps_pos_innov_gates, gps_pos_obs_var, _gps_pos_innov_var, _gps_pos_test_ratio);
fuseHorizontalPosition(_gps_pos_innov, gps_pos_innov_gates, gps_pos_obs_var, _gps_pos_innov_var, _gps_pos_test_ratio, false);
}
} else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)10e6)) {
@@ -1289,11 +1289,10 @@ void Ekf::controlFakePosFusion()
_gps_pos_innov.xy() = Vector2f(_state.pos) - _last_known_posNE;
// glitch protection is not required so set gate to a large value
const Vector2f fake_pos_innov_gate(100.0f, 100.0f);
const Vector2f fake_pos_innov_gate(3.0f, 3.0f);
fuseHorizontalPosition(_gps_pos_innov, fake_pos_innov_gate, fake_pos_obs_var,
_gps_pos_innov_var, _gps_pos_test_ratio);
_gps_pos_innov_var, _gps_pos_test_ratio, true);
}
} else {