fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 23:53:34 +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
Download Patch File Download Diff File Expand all files Collapse all files
EKF/vel_pos_fusion.cpp
+4 -3
View File
@@ -163,15 +163,16 @@ void Ekf::fuseVelPosHeight()
// check position, velocity and height innovations // check position, velocity and height innovations
// treat 3D velocity, 2D position and height as separate sensors // 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) 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); && (_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; 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 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)) 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[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 // record the successful velocity fusion time
if (vel_check_pass && _fuse_hor_vel) { if (vel_check_pass && _fuse_hor_vel) {