forked from Archive/PX4-Autopilot
EKF: Don't reject vertical aiding data if inertial solution is bad
Also reduce clipping count threshold
This commit is contained in:
parent
c566318db5
commit
e948b3505f
|
@ -927,7 +927,7 @@ void Ekf::checkVerticalAccelerationHealth()
|
|||
|
||||
// if vertical velocity and position are independent and agree, then do not require evidence of clipping if
|
||||
// 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;
|
||||
|
||||
if (bad_vert_accel) {
|
||||
|
|
|
@ -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));
|
||||
_vert_vel_innov_ratio = innov(2) / sqrtf(innov_var(2));
|
||||
_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) {
|
||||
_time_last_ver_vel_fuse = _time_last_imu;
|
||||
_innov_check_fail_status.flags.reject_ver_vel = false;
|
||||
|
||||
fuseVelPosHeight(innov(2), innov_var(2), 2);
|
||||
fuseVelPosHeight(innovation, innov_var(2), 2);
|
||||
|
||||
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));
|
||||
_vert_pos_innov_ratio = innov(2) / sqrtf(innov_var(2));
|
||||
_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) {
|
||||
_time_last_hgt_fuse = _time_last_imu;
|
||||
_innov_check_fail_status.flags.reject_ver_pos = false;
|
||||
fuseVelPosHeight(innov(2), innov_var(2), 5);
|
||||
fuseVelPosHeight(innovation, innov_var(2), 5);
|
||||
|
||||
return true;
|
||||
|
||||
|
|
Loading…
Reference in New Issue