From a1308482fd5c1a19f799bbed610d1d558feab350 Mon Sep 17 00:00:00 2001 From: CarlOlsson Date: Wed, 31 May 2017 10:18:40 +0200 Subject: [PATCH] EKF: Reset the mag states to the field state variance when switching from the inhibit mag mode to standard mode --- EKF/control.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index f9ab4d3b27..1204ec3edd 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -979,13 +979,13 @@ void Ekf::controlMagFusion() // perform switch-over from only updating the mag states to updating all states if (!_control_status.flags.update_mag_states_only && _control_status_prev.flags.update_mag_states_only) { - // reset the mag field covariances + // When re-commencing use of magnetometer to correct vehicle states + // set the field state variance to the observation variance and zero + // the covariance terms to allow the field states re-learn rapidly zeroRows(P, 16, 21); zeroCols(P, 16, 21); - - // re-instate the last used variances for (uint8_t index = 0; index <= 5; index ++) { - P[index+16][index+16] = _saved_mag_variance[index]; + P[index+16][index+16] = sq(_params.mag_noise); } }