From 1540e937b184345c1bd4becb30757200ed80f4e1 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Tue, 17 May 2016 10:42:37 +1000
Subject: [PATCH] EKF: Improve tilt alignment monitoring

Convert quaternion covariances into an angular alignment variance vector and discard the z component so that yaw uncertainty does not affect the result.
---
 EKF/control.cpp    | 15 ++++++++++-----
 EKF/ekf.h          |  3 +++
 EKF/ekf_helper.cpp | 39 +++++++++++++++++++++++++++++++++++++++
 3 files changed, 52 insertions(+), 5 deletions(-)

diff --git a/EKF/control.cpp b/EKF/control.cpp
index 006e933564..ffa7f37ce4 100644
--- a/EKF/control.cpp
+++ b/EKF/control.cpp
@@ -49,11 +49,16 @@ void Ekf::controlFusionModes()
 	// Get the magnetic declination
 	calcMagDeclination();
 
-	// Once the angular uncertainty has reduced sufficiently, initialise the yaw and magnetic field states
-	float total_angle_variance = P[0][0] + P[1][1] + P[2][2] + P[3][3];
-	if (total_angle_variance < 0.002f && !_control_status.flags.tilt_align) {
-		_control_status.flags.tilt_align = true;
-		_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
+	// monitor the tilt alignment
+	if (!_control_status.flags.tilt_align) {
+		// whilst we are aligning the tilt, monitor the variances
+		Vector3f angle_err_var_vec = calcRotVecVariances();
+
+		// Once the tilt variances have reduced sufficiently, initialise the yaw and magnetic field states
+		if ((angle_err_var_vec(0) + angle_err_var_vec(1)) < 0.002f) {
+			_control_status.flags.tilt_align = true;
+			_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
+		}
 	}
 
 	// control use of various external souces for positon and velocity aiding
diff --git a/EKF/ekf.h b/EKF/ekf.h
index 554cd74977..013e01a5c6 100644
--- a/EKF/ekf.h
+++ b/EKF/ekf.h
@@ -363,4 +363,7 @@ private:
 	// calculate the measurement variance for the optical flow sensor
 	float calcOptFlowMeasVar();
 
+	// rotate quaternion covariances into variances for an equivalent rotation vector
+	Vector3f calcRotVecVariances();
+
 };
diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp
index 0e3ac8b008..05071fd7f4 100644
--- a/EKF/ekf_helper.cpp
+++ b/EKF/ekf_helper.cpp
@@ -654,3 +654,42 @@ Matrix3f EstimatorInterface::quat_to_invrotmat(const Quaternion quat)
 
 	return dcm;
 }
+
+// calculate the variances for the rotation vector equivalent
+Vector3f Ekf::calcRotVecVariances()
+{
+	Vector3f rot_var_vec;
+	float q0,q1,q2,q3;
+	if (_state.quat_nominal(0) >= 0.0f) {
+		q0 = _state.quat_nominal(0);
+		q1 = _state.quat_nominal(1);
+		q2 = _state.quat_nominal(2);
+		q3 = _state.quat_nominal(3);
+	} else {
+		q0 = -_state.quat_nominal(0);
+		q1 = -_state.quat_nominal(1);
+		q2 = -_state.quat_nominal(2);
+		q3 = -_state.quat_nominal(3);
+	}
+	float t2 = q0*q0;
+	float t3 = acos(q0);
+	float t4 = -t2+1.0f;
+	float t5 = t2-1.0f;
+	float t6 = 1.0f/t5;
+	float t7 = q1*t6*2.0f;
+	float t8 = 1.0f/powf(t4,1.5f);
+	float t9 = q0*q1*t3*t8*2.0f;
+	float t10 = t7+t9;
+	float t11 = 1.0f/sqrtf(t4);
+	float t12 = q2*t6*2.0f;
+	float t13 = q0*q2*t3*t8*2.0f;
+	float t14 = t12+t13;
+	float t15 = q3*t6*2.0f;
+	float t16 = q0*q3*t3*t8*2.0f;
+	float t17 = t15+t16;
+	rot_var_vec(0) = t10*(P[0][0]*t10+P[1][0]*t3*t11*2.0f)+t3*t11*(P[0][1]*t10+P[1][1]*t3*t11*2.0f)*2.0f;
+	rot_var_vec(1) = t14*(P[0][0]*t14+P[2][0]*t3*t11*2.0f)+t3*t11*(P[0][2]*t14+P[2][2]*t3*t11*2.0f)*2.0f;
+	rot_var_vec(2) = t17*(P[0][0]*t17+P[3][0]*t3*t11*2.0f)+t3*t11*(P[0][3]*t17+P[3][3]*t3*t11*2.0f)*2.0f;
+
+	return rot_var_vec;
+}