From eae6e8f19c0a587e54beea1bb461c8bb9aae4ccf Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 6 Aug 2019 18:22:00 +1000 Subject: [PATCH] EKF: Fix on ground yaw drift when using EKF2_MAG_TYPE = 4 --- EKF/control.cpp | 2 +- EKF/ekf.h | 1 - EKF/mag_fusion.cpp | 29 +++++++++++++++++++---------- 3 files changed, 20 insertions(+), 12 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 32d282683f..7957855b73 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -1582,7 +1582,7 @@ void Ekf::controlMagFusion() save_mag_cov_data(); } - } else if (_params.mag_fusion_type == MAG_FUSE_TYPE_HEADING) { + } else if (_params.mag_fusion_type == MAG_FUSE_TYPE_HEADING || _params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR) { // always use heading fusion _control_status.flags.mag_hdg = true; diff --git a/EKF/ekf.h b/EKF/ekf.h index 9f90e61e14..61f19eb95c 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -359,7 +359,6 @@ private: bool _mag_use_inhibit_prev{false}; ///< true when magnetometer use was being inhibited the previous frame bool _mag_inhibit_yaw_reset_req{false}; ///< true when magnetometer inhibit has been active for long enough to require a yaw reset when conditions 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 bool _mag_yaw_reset_req{false}; ///< true when a reset of the yaw using the magnetometer data has been requested bool _mag_decl_cov_reset{false}; ///< true after the fuseDeclination() function has been used to modify the earth field covariances after a magnetic field reset event. bool _synthetic_mag_z_active{false}; ///< true if we are generating synthetic magnetometer Z measurements diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 585884ee21..08d81ea5ba 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -511,9 +511,15 @@ void Ekf::fuseHeading() float Tbn_0_0 = sq(_ev_sample_delayed.quat(0))+sq(_ev_sample_delayed.quat(1))-sq(_ev_sample_delayed.quat(2))-sq(_ev_sample_delayed.quat(3)); measured_hdg = atan2f(Tbn_1_0,Tbn_0_0); + } else if (_mag_use_inhibit) { + // Special case where we either use the current or last known stationary value + // so set to current value as a safe default + measured_hdg = predicted_hdg; + } else { - // there is no yaw observation + // Should not be doing yaw fusion return; + } } else { @@ -604,8 +610,13 @@ void Ekf::fuseHeading() float Tbn_1_1 = sq(_ev_sample_delayed.quat(0))-sq(_ev_sample_delayed.quat(1))+sq(_ev_sample_delayed.quat(2))-sq(_ev_sample_delayed.quat(3)); measured_hdg = atan2f(Tbn_0_1_neg,Tbn_1_1); + } else if (_mag_use_inhibit) { + // Special case where we either use the current or last known stationary value + // so set to current value as a safe default + measured_hdg = predicted_hdg; + } else { - // there is no yaw observation + // Should not be doing yaw fusion return; } @@ -621,8 +632,8 @@ void Ekf::fuseHeading() R_YAW = sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f)); } else { - // there is no yaw observation - return; + // default value + R_YAW = 0.01f; } // wrap the heading to the interval between +-pi @@ -637,22 +648,20 @@ void Ekf::fuseHeading() // Vehicle is not at rest so fuse a zero innovation and record the // predicted heading to use as an observation when movement ceases. _heading_innov = 0.0f; - _vehicle_at_rest_prev = false; + _last_static_yaw = predicted_hdg; + } else { // Vehicle is at rest so use the last moving prediction as an observation // to prevent the heading from drifting and to enable yaw gyro bias learning // before takeoff. - if (!_vehicle_at_rest_prev || !_mag_use_inhibit_prev) { - _last_static_yaw = predicted_hdg; - _vehicle_at_rest_prev = true; - } _heading_innov = predicted_hdg - _last_static_yaw; - R_YAW = 0.01f; innov_gate = 5.0f; + } } else { _heading_innov = predicted_hdg - measured_hdg; _last_static_yaw = predicted_hdg; + } _mag_use_inhibit_prev = _mag_use_inhibit;