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

EKF: protect against large IMU errors during initial tilt alignment

Browse Source 
Turn off innovation consistency checks for height and synthetic position measurements until the tilt as aligned
This commit is contained in:
Paul Riseborough 2016-05-08 17:54:45 +10:00
parent 00973d6215
commit feb4db6347
1 changed files with 4 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
EKF/vel_pos_fusion.cpp
View File

@ -163,15 +163,16 @@ void Ekf::fuseVelPosHeight()
// check position, velocity and height innovations
// treat 3D velocity, 2D position and height as separate sensors
// always pass position checks if using synthetic position measurements
// always pass position checks if using synthetic position measurements or yet to complete tilt alignment
// always pass height checks if yet to complete tilt alignment
bool vel_check_pass = (_vel_pos_test_ratio[0] <= 1.0f) && (_vel_pos_test_ratio[1] <= 1.0f)
&& (_vel_pos_test_ratio[2] <= 1.0f);
innov_check_pass_map[2] = innov_check_pass_map[1] = innov_check_pass_map[0] = vel_check_pass;
bool using_synthetic_measurements = !_control_status.flags.gps && !_control_status.flags.opt_flow;
bool pos_check_pass = ((_vel_pos_test_ratio[3] <= 1.0f) && (_vel_pos_test_ratio[4] <= 1.0f))
|| using_synthetic_measurements;
|| using_synthetic_measurements || !_control_status.flags.tilt_align;
innov_check_pass_map[4] = innov_check_pass_map[3] = pos_check_pass;
innov_check_pass_map[5] = (_vel_pos_test_ratio[5] <= 1.0f);
innov_check_pass_map[5] = (_vel_pos_test_ratio[5] <= 1.0f) || !_control_status.flags.tilt_align;
// record the successful velocity fusion time
if (vel_check_pass && _fuse_hor_vel) {