From 2d3b65231b35890ed54f02a8385f997a557fa989 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 15 May 2018 15:09:27 +1000 Subject: [PATCH] EKF: Reset yaw gyro bias learning when resuming use of magnetometer The previous lack of yaw drift constraint will also mean that the yaw gyro bias estimate is likely to be incorrect and needs to be re-learned. --- EKF/control.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 46079493d2..2320cc0614 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -476,7 +476,14 @@ void Ekf::controlGpsFusion() _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; + // Handle the special case where we have not been constraining yaw drift or learning yaw bias due + // to assumed invalid mag field associated with indoor operation with a downwards looking flow sensor. + if (_mag_inhibit_yaw_reset_req) { + _mag_inhibit_yaw_reset_req = false; + // Zero the yaw bias covariance and set the variance to the initial alignment uncertainty + float dt = 0.001f * (float)FILTER_UPDATE_PERIOD_MS; + setDiag(P, 12, 12, sq(_params.switch_on_gyro_bias * dt)); + } } // If the heading is valid start using gps aiding