From 94a63ec9d56675eb99b9d86368e3be83ebffcda3 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Wed, 18 May 2016 19:19:38 +1000
Subject: [PATCH] EKF: don't interfere with quaternion covariances during tilt
 alignment

---
 EKF/ekf_helper.cpp | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp
index a6748de7f1..0dfa6c0fbd 100644
--- a/EKF/ekf_helper.cpp
+++ b/EKF/ekf_helper.cpp
@@ -407,8 +407,10 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
 	// reset the quaternion variances because the yaw angle could have changed by a significant amount
 	// by setting them to zero we avoid 'kicks' in angle when 3-D fusion starts and the imu process noise
 	// will grow them again.
-	zeroRows(P, 0, 3);
-	zeroCols(P, 0, 3);
+	if (_control_status.flags.tilt_align) {
+		zeroRows(P, 0, 3);
+		zeroCols(P, 0, 3);
+	}
 
 	// update transformation matrix from body to world frame using the current estimate
 	_R_to_earth = quat_to_invrotmat(_state.quat_nominal);