Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 07:09:48 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

EKF: Don't reject vertical aiding data if inertial solution is bad

Browse Source 
Also reduce clipping count threshold
This commit is contained in:
Paul Riseborough
2020-06-29 16:53:43 +10:00
committed by Paul Riseborough
parent c566318db5
commit e948b3505f
2 changed files with 27 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+1 -1
View File
@@ -927,7 +927,7 @@ void Ekf::checkVerticalAccelerationHealth()
// if vertical velocity and position are independent and agree, then do not require evidence of clipping if // if vertical velocity and position are independent and agree, then do not require evidence of clipping if
// innovations are large // innovations are large
const bool bad_vert_accel = (are_vertical_pos_and_vel_independant || _clip_counter > clip_count_limit / 2) && const bool bad_vert_accel = (are_vertical_pos_and_vel_independant || _clip_counter > 0) &&
is_inertial_nav_falling; is_inertial_nav_falling;
if (bad_vert_accel) { if (bad_vert_accel) {
EKF/vel_pos_fusion.cpp
+26 -4
View File
@@ -81,13 +81,24 @@ bool Ekf::fuseVerticalVelocity(const Vector3f &innov, const Vector2f &innov_gate
test_ratio(1) = sq(innov(2)) / (sq(innov_gate(1)) * innov_var(2)); test_ratio(1) = sq(innov(2)) / (sq(innov_gate(1)) * innov_var(2));
_vert_vel_innov_ratio = innov(2) / sqrtf(innov_var(2)); _vert_vel_innov_ratio = innov(2) / sqrtf(innov_var(2));
_vert_vel_fuse_time_us = _time_last_imu; _vert_vel_fuse_time_us = _time_last_imu;
const bool innov_check_pass = (test_ratio(1) <= 1.0f); bool innov_check_pass = (test_ratio(1) <= 1.0f);
// if there is bad vertical acceleration data, then don't reject measurement,
// but limit innovation to prevent spikes that could destabilise the filter
float innovation = innov(2);
if (_bad_vert_accel_detected && !innov_check_pass) {
const float innov_limit = innov_gate(1) * sqrtf(innov_var(2));
innovation = math::constrain(innov(2), -innov_limit, innov_limit);
innov_check_pass = true;
} else {
innovation = innov(2);
}
if (innov_check_pass) { if (innov_check_pass) {
_time_last_ver_vel_fuse = _time_last_imu; _time_last_ver_vel_fuse = _time_last_imu;
_innov_check_fail_status.flags.reject_ver_vel = false; _innov_check_fail_status.flags.reject_ver_vel = false;
fuseVelPosHeight(innov(2), innov_var(2), 2); fuseVelPosHeight(innovation, innov_var(2), 2);
return true; return true;
@@ -141,12 +152,23 @@ bool Ekf::fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate
test_ratio(1) = sq(innov(2)) / (sq(innov_gate(1)) * innov_var(2)); test_ratio(1) = sq(innov(2)) / (sq(innov_gate(1)) * innov_var(2));
_vert_pos_innov_ratio = innov(2) / sqrtf(innov_var(2)); _vert_pos_innov_ratio = innov(2) / sqrtf(innov_var(2));
_vert_pos_fuse_time_us = _time_last_imu; _vert_pos_fuse_time_us = _time_last_imu;
const bool innov_check_pass = test_ratio(1) <= 1.0f; bool innov_check_pass = test_ratio(1) <= 1.0f;
// if there is bad vertical acceleration data, then don't reject measurement,
// but limit innovation to prevent spikes that could destabilise the filter
float innovation = innov(2);
if (_bad_vert_accel_detected && !innov_check_pass) {
const float innov_limit = innov_gate(1) * sqrtf(innov_var(2));
innovation = math::constrain(innov(2), -innov_limit, innov_limit);
innov_check_pass = true;
} else {
innovation = innov(2);
}
if (innov_check_pass) { if (innov_check_pass) {
_time_last_hgt_fuse = _time_last_imu; _time_last_hgt_fuse = _time_last_imu;
_innov_check_fail_status.flags.reject_ver_pos = false; _innov_check_fail_status.flags.reject_ver_pos = false;
fuseVelPosHeight(innov(2), innov_var(2), 5); fuseVelPosHeight(innovation, innov_var(2), 5);
return true; return true;