From 0160aaa568a27f3d2ed98002a9430e12a8de2cb7 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 15 May 2018 09:48:09 +1000 Subject: [PATCH] EKF: Don't use magnetometer with optical flow only nav if GPS checks are failing --- EKF/control.cpp | 20 +++++++++++++++++++- EKF/ekf.h | 3 +++ EKF/mag_fusion.cpp | 8 +++++++- EKF/optflow_fusion.cpp | 1 - 4 files changed, 29 insertions(+), 3 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 329c27f896..698029e521 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -472,10 +472,11 @@ void Ekf::controlGpsFusion() // If the heading is not aligned, reset the yaw and magnetic field states // Do not use external vision for yaw if using GPS because yaw needs to be // defined relative to an NED reference frame - if (!_control_status.flags.yaw_align || _control_status.flags.ev_yaw) { + if (!_control_status.flags.yaw_align || _control_status.flags.ev_yaw || _mag_inhibit_yaw_reset_req) { _control_status.flags.yaw_align = false; _control_status.flags.ev_yaw = false; _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); + _mag_inhibit_yaw_reset_req = false; } // If the heading is valid start using gps aiding @@ -1391,6 +1392,23 @@ void Ekf::controlMagFusion() _control_status.flags.mag_dec = false; } + // If optical flow is the only aiding source and GPS checks are failing, then assume that we are operating + // indoors and the magnetometer is unreliable. + if (_control_status.flags.opt_flow + && !_control_status.flags.gps + && !_control_status.flags.ev_pos + && ((_last_gps_fail_us - _time_last_imu) < 5E6)) { + _mag_use_inhibit = true; + } else { + _mag_use_inhibit = false; + _mag_use_not_inhibit_us = _imu_sample_delayed.time_us; + } + + // If magnetomer use has been inhibited continuously then a yaw reset is required for a valid heading + if (uint32_t(_imu_sample_delayed.time_us - _mag_use_not_inhibit_us) > (uint32_t)5e6) { + _mag_inhibit_yaw_reset_req = true; + } + // fuse magnetometer data using the selected methods if (_control_status.flags.mag_3D && _control_status.flags.yaw_align) { fuseMag(); diff --git a/EKF/ekf.h b/EKF/ekf.h index bfe6af0c70..0fef011f42 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -322,6 +322,9 @@ private: bool _yaw_angle_observable{false}; ///< true when there is enough horizontal acceleration to make yaw observable uint64_t _time_yaw_started{0}; ///< last system time in usec that a yaw rotation moaneouvre was detected 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_inhibit_yaw_reset_req{false}; ///< true when magnetomer inhibit has been active for long enough to require a yaw reset when conditons improve. float P[_k_num_states][_k_num_states] {}; ///< state covariance matrix diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index dd3c60724b..9df5d9f0ae 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -675,7 +675,13 @@ void Ekf::fuseHeading() measured_hdg = wrap_pi(measured_hdg); // calculate the innovation - _heading_innov = predicted_hdg - measured_hdg; + if (_mag_use_inhibit) { + // The magnetomer cannot be trusted but we need to fuse a heading to prevent a badly + // conditoned covariance matrix developing over time. + _heading_innov = 0.0f; + } else { + _heading_innov = predicted_hdg - measured_hdg; + } // wrap the innovation to the interval between +-pi _heading_innov = wrap_pi(_heading_innov); diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp index e381b4d765..c79416ee26 100644 --- a/EKF/optflow_fusion.cpp +++ b/EKF/optflow_fusion.cpp @@ -500,7 +500,6 @@ void Ekf::fuseOptFlow() fuse(gain, _flow_innov[obs_index]); _time_last_of_fuse = _time_last_imu; - _gps_check_fail_status.value = 0; } } }