fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-18 03:27:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Remove tilt align from position innovation check

Browse Source
This commit is contained in:
Kamil Ritz
2020-05-24 08:42:39 +02:00
committed by Mathieu Bresciani
parent b024cdd282
commit 86fc94b75a
1 changed files with 2 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/vel_pos_fusion.cpp
+2 -2
View File
@@ -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)), test_ratio(0) = fmaxf(sq(innov(0)) / (sq(innov_gate(0)) * innov_var(0)),
sq(innov(1)) / (sq(innov_gate(0)) * innov_var(1))); 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 (innov_check_pass) {
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;
@@ -122,7 +122,7 @@ bool Ekf::fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate
innov_var(2) = P(9, 9) + obs_var(2); innov_var(2) = P(9, 9) + obs_var(2);
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));
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) { 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;