Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 18:47:21 +08:00
Code Issues Actions 19 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-19 21:04:52 +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 // innovation gate size
ev_pos_innov_gates(0) = fmaxf(_params.ev_pos_innov_gate, 1.0f); 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 // determine if we should use the velocity observations
@@ -712,7 +712,7 @@ void Ekf::controlGpsFusion()
// fuse GPS measurement // fuse GPS measurement
fuseHorizontalVelocity(_gps_vel_innov, gps_vel_innov_gates,gps_vel_obs_var, _gps_vel_innov_var, _gps_vel_test_ratio); 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); 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)) { } 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; _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(3.0f, 3.0f);
const Vector2f fake_pos_innov_gate(100.0f, 100.0f);
fuseHorizontalPosition(_gps_pos_innov, fake_pos_innov_gate, fake_pos_obs_var, 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 { } else {
EKF/ekf.h
+1 -1
View File
@@ -622,7 +622,7 @@ private:
Vector3f &innov_var, Vector2f &test_ratio); Vector3f &innov_var, Vector2f &test_ratio);
bool fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var, bool fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var,
Vector3f &innov_var, Vector2f &test_ratio); Vector3f &innov_var, Vector2f &test_ratiov, bool inhibit_gate);
bool fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var, bool fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var,
Vector3f &innov_var, Vector2f &test_ratio); Vector3f &innov_var, Vector2f &test_ratio);
EKF/vel_pos_fusion.cpp
+2 -2
View File
@@ -97,7 +97,7 @@ bool Ekf::fuseVerticalVelocity(const Vector3f &innov, const Vector2f &innov_gate
} }
bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var, bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var,
Vector3f &innov_var, Vector2f &test_ratio) Vector3f &innov_var, Vector2f &test_ratio, bool inhibit_gate)
{ {
innov_var(0) = P(7, 7) + obs_var(0); innov_var(0) = P(7, 7) + obs_var(0);
@@ -107,7 +107,7 @@ bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_ga
const bool innov_check_pass = test_ratio(0) <= 1.0f; const bool innov_check_pass = test_ratio(0) <= 1.0f;
if (innov_check_pass) { if (innov_check_pass || inhibit_gate) {
if (!_fuse_hpos_as_odom) { if (!_fuse_hpos_as_odom) {
_time_last_hor_pos_fuse = _time_last_imu; _time_last_hor_pos_fuse = _time_last_imu;