mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-18 21:20:35 +08:00
EKF: capture yaw innovation test failures
This commit is contained in:
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user