From adb4a09bebc6519b9eb35bf7effc68dbc9c3b515 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 17 May 2018 14:13:20 +1000 Subject: [PATCH] EKF: Fix bug causing large yaw innovations when GPS is lost --- EKF/ekf.h | 1 + EKF/mag_fusion.cpp | 4 +++- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/EKF/ekf.h b/EKF/ekf.h index 6a2361e361..d127d23f87 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -324,6 +324,7 @@ private: uint8_t _num_bad_flight_yaw_events{0}; ///< number of times a bad heading has been detected in flight and required a yaw reset uint64_t _mag_use_not_inhibit_us{0}; ///< last system time in usec before magnetomer use was inhibited bool _mag_use_inhibit{false}; ///< true when magnetomer use is being inhibited + bool _mag_use_inhibit_prev{false}; ///< true when magnetomer use was being inhibited the previous frame bool _mag_inhibit_yaw_reset_req{false}; ///< true when magnetomer inhibit has been active for long enough to require a yaw reset when conditons improve. float _last_static_yaw{0.0f}; ///< last yaw angle recorded when on ground motion checks were passing (rad) bool _vehicle_at_rest_prev{false}; ///< true when the vehicle was at rest the previous time the status was checked diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 1b3d9e50c8..eff0a4c2b0 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -629,7 +629,7 @@ void Ekf::fuseHeading() // Vehicle is at rest so use the last moving prediciton as an observation // to prevent the heading from drifting and to enable yaw gyro bias learning // before takeoff. - if (!_vehicle_at_rest_prev) { + if (!_vehicle_at_rest_prev || !_mag_use_inhibit_prev) { _last_static_yaw = predicted_hdg; _vehicle_at_rest_prev = true; } @@ -639,7 +639,9 @@ void Ekf::fuseHeading() } } else { _heading_innov = predicted_hdg - measured_hdg; + _last_static_yaw = predicted_hdg; } + _mag_use_inhibit_prev = _mag_use_inhibit; // wrap the innovation to the interval between +-pi _heading_innov = wrap_pi(_heading_innov);