dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Remove tilt align from position innovation check

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
EKF/vel_pos_fusion.cpp
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)),
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;