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

EKF: capture yaw innovation test failures

This commit is contained in:
Paul Riseborough 2016-05-23 14:23:08 +10:00
parent 52f6eea52b
commit 3fb449295e
1 changed files with 2 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
EKF/mag_fusion.cpp
View File

@ -604,6 +604,7 @@ void Ekf::fuseHeading()
// set the magnetometer unhealthy if the test fails
if (_yaw_test_ratio > 1.0f) {
_mag_healthy = false;
_sensor_fault_status.flags.reject_yaw = true;
// if we are in air we don't want to fuse the measurement
// we allow to use it when on the ground because the large innovation could be caused
@ -619,6 +620,7 @@ void Ekf::fuseHeading()
} else {
_mag_healthy = true;
_sensor_fault_status.flags.reject_yaw = false;
}
// apply covariance correction via P_new = (I -K*H)*P