|
|
|
@ -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
|
|
|
|
|