From 86fc94b75ac861feb54944247d73e5601038fab6 Mon Sep 17 00:00:00 2001 From: Kamil Ritz Date: Sun, 24 May 2020 08:42:39 +0200 Subject: [PATCH] Remove tilt align from position innovation check --- EKF/vel_pos_fusion.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index be8c47a45a..ea6ca5192b 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -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 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;