|
|
|
@ -96,7 +96,7 @@ bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_ga
@@ -96,7 +96,7 @@ bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_ga
|
|
|
|
|
test_ratio(0) = fmaxf(sq(innov(0)) / (sq(innov_gate(0)) * innov_var(0)), |
|
|
|
|
sq(innov(1)) / (sq(innov_gate(0)) * innov_var(1))); |
|
|
|
|
|
|
|
|
|
const bool innov_check_pass = (test_ratio(0) <= 1.0f) || !_control_status.flags.tilt_align; |
|
|
|
|
const bool innov_check_pass = test_ratio(0) <= 1.0f; |
|
|
|
|
if (innov_check_pass) { |
|
|
|
|
if (!_fuse_hpos_as_odom) { |
|
|
|
|
_time_last_hor_pos_fuse = _time_last_imu; |
|
|
|
@ -122,7 +122,7 @@ bool Ekf::fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate
@@ -122,7 +122,7 @@ bool Ekf::fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate
|
|
|
|
|
innov_var(2) = P(9, 9) + obs_var(2); |
|
|
|
|
test_ratio(1) = sq(innov(2)) / (sq(innov_gate(1)) * innov_var(2)); |
|
|
|
|
|
|
|
|
|
const bool innov_check_pass = (test_ratio(1) <= 1.0f) || !_control_status.flags.tilt_align; |
|
|
|
|
const bool innov_check_pass = test_ratio(1) <= 1.0f; |
|
|
|
|
if (innov_check_pass) { |
|
|
|
|
_time_last_hgt_fuse = _time_last_imu; |
|
|
|
|
_innov_check_fail_status.flags.reject_ver_pos = false; |
|
|
|
|