From 72a7ab2c25e0078734b358bf3615ea22408664be Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 10 Nov 2017 10:32:49 +1100 Subject: [PATCH] EKF: Improve resistance to bad initial mag offset When magnetic field states have been reset in-flight using a single sample, the magnetic field states are not used to constrain heading drift for a period after the reset. This period has been shortened from 10 to 5 seconds which is enough time to average out the effects of measurement noise (the original concern). The shorter time has enabled the previous practice for RW vehicles of using magnetic heading in that time period to constrain yaw drift to be discontinued. This is necessary becasue while magnetic heading is being used, it fights the yaw corrections obtained from GPs observations and lengthens the time required to recover from a bad mag calibration. --- EKF/control.cpp | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 47d74ff260..5185239f77 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -1244,8 +1244,9 @@ void Ekf::controlMagFusion() _control_status.flags.update_mag_states_only = (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTOFW) && _control_status.flags.fixed_wing; - // For the first 10 seconds after switching to 3-axis fusion we allow the magnetic field state estimates to stabilise - _flt_mag_align_converging = ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) < (uint64_t)1e7); + // For the first 5 seconds after switching to 3-axis fusion we allow the magnetic field state estimates to stabilise + // before they are used to constrain heading drift + _flt_mag_align_converging = ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) < (uint64_t)5e6); if (!_control_status.flags.update_mag_states_only && _control_status_prev.flags.update_mag_states_only) { // When re-commencing use of magnetometer to correct vehicle states @@ -1296,12 +1297,6 @@ void Ekf::controlMagFusion() fuseDeclination(); } - if (_flt_mag_align_converging && !_control_status.flags.fixed_wing) { - _control_status.flags.mag_hdg = true; - fuseHeading(); - _control_status.flags.mag_hdg = false; - } - } else if (_control_status.flags.mag_hdg && _control_status.flags.yaw_align) { // fusion of an Euler yaw angle from either a 321 or 312 rotation sequence fuseHeading();