Browse Source

EKF: capture yaw innovation test failures

master
Paul Riseborough 9 years ago
parent
commit
3fb449295e
  1. 2
      EKF/mag_fusion.cpp

2
EKF/mag_fusion.cpp

@ -604,6 +604,7 @@ void Ekf::fuseHeading() @@ -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() @@ -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

Loading…
Cancel
Save