From 9e47b6e1b618f94481133b85bddc2ee65a1bb53d Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Thu, 9 Nov 2017 10:32:35 +1100
Subject: [PATCH] EKF: don't reset quaternions unnecessarily

When performing the initial in-flight magnetic field reset for fixed wing vehicles, resetting the quaternion states and their corresponding covariances should be avoided unless yaw errors are large, because state estimates are vulnerable to transient sensor errors immediately following a reset.
---
 EKF/ekf_helper.cpp | 18 ++++++++++++++++--
 1 file changed, 16 insertions(+), 2 deletions(-)

diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp
index 4159a104b4..792b98fdf9 100644
--- a/EKF/ekf_helper.cpp
+++ b/EKF/ekf_helper.cpp
@@ -461,9 +461,23 @@ bool Ekf::realignYawGPS()
 			return true;
 
 		} else {
-			// attempt a normal alignment using the magnetometer
-			return resetMagHeading(_mag_sample_delayed.mag);
+			// align mag states only
 
+			// calculate initial earth magnetic field states
+			_state.mag_I = _R_to_earth * _mag_sample_delayed.mag;
+
+			// reset the corresponding rows and columns in the covariance matrix and set the variances on the magnetic field states to the measurement variance
+			zeroRows(P, 16, 21);
+			zeroCols(P, 16, 21);
+
+			for (uint8_t index = 16; index <= 21; index ++) {
+				P[index][index] = sq(_params.mag_noise);
+			}
+
+			// record the start time for the magnetic field alignment
+			_flt_mag_align_start_time = _imu_sample_delayed.time_us;
+
+			return true;
 		}
 
 	} else {