EKF: Improve initialisation of quaternion covariances

Convert uncertainty in initial rotate vector into quaternion covariances using symbolic toolbox derived expressions.
Enable setting of initial angle uncertainty via a parameter

EKF/common.h

@@ -197,9 +197,10 @@ struct parameters {
 	float terrain_p_noise;		// process noise for terrain offset (m/sec)
 	float terrain_gradient;		// gradient of terrain used to estimate process noise due to changing position (m/m)
 
-	// initial switch on bias uncertainty
+	// initialisation errors
 	float switch_on_gyro_bias;	// 1-sigma gyro bias uncertainty at switch on (rad/sec)
 	float switch_on_accel_bias;	// 1-sigma accelerometer bias uncertainty at switch on (m/s**2)
+	float initial_tilt_err;		// 1-sigma tilt error after initial alignment using gravity vector (rad)
 
 	// position and velocity fusion
 	float gps_vel_noise;		// observation noise for gps velocity fusion (m/sec)
@@ -292,9 +293,10 @@ struct parameters {
 		terrain_p_noise = 5.0f;
 		terrain_gradient = 0.5f;
 
-		// initial switch on bias uncertainty
+		// initialisation errors
 		switch_on_gyro_bias = 0.1f;
 		switch_on_accel_bias = 0.2f;
+		initial_tilt_err = 0.1f;
 
 		// position and velocity fusion
 		gps_vel_noise = 5.0e-1f;

EKF/covariance.cpp

@@ -55,11 +55,12 @@ void Ekf::initialiseCovariance()
 	// calculate average prediction time step in sec
 	float dt = 0.001f * (float)FILTER_UPDATE_PERRIOD_MS;
 
-	// quaternion error
-	P[0][0]   = 0.01f;
-	P[1][1]   = 0.01f;
-	P[2][2]   = 0.01f;
-	P[3][3]   = 0.01f;
+	// define the initial angle uncertainty as variances for a rotation vector
+	Vector3f rot_vec_var;
+	rot_vec_var(2) = rot_vec_var(1) = rot_vec_var(0) = sq(_params.initial_tilt_err);
+
+	// update the quaternion state covariances
+	initialiseQuatCovariances(rot_vec_var);
 
 	// velocity
 	P[4][4] = sq(fmaxf(_params.gps_vel_noise, 0.01f));

EKF/ekf.h

@@ -366,4 +366,7 @@ private:
 	// rotate quaternion covariances into variances for an equivalent rotation vector
 	Vector3f calcRotVecVariances();
 
+	// initialise the quaternion covariances using rotation vector variances
+	void initialiseQuatCovariances(Vector3f &rot_vec_var);
+
 };

EKF/ekf_helper.cpp

@@ -693,3 +693,111 @@ Vector3f Ekf::calcRotVecVariances()
 
 	return rot_var_vec;
 }
+
+// initialise the quaternion covariances using rotation vector variances
+void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var)
+{
+	// calculate an equivalent rotation vector from the quaternion
+	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 delta = 2.0f*acosf(q0);
+	float scaler = (delta/sinf(delta*0.5f));
+	float rotX = scaler*q1;
+	float rotY = scaler*q2;
+	float rotZ = scaler*q3;
+
+	// autocode generated using matlab symbolic toolbox
+	float t2 = rotX*rotX;
+	float t4 = rotY*rotY;
+	float t5 = rotZ*rotZ;
+	float t6 = t2+t4+t5;
+	if (t6 > 1e-9f) {
+		float t7 = sqrtf(t6);
+		float t8 = t7*0.5f;
+		float t3 = sinf(t8);
+		float t9 = t3*t3;
+		float t10 = 1.0f/t6;
+		float t11 = 1.0f/sqrtf(t6);
+		float t12 = cosf(t8);
+		float t13 = 1.0f/powf(t6,1.5f);
+		float t14 = t3*t11;
+		float t15 = rotX*rotY*t3*t13;
+		float t16 = rotX*rotZ*t3*t13;
+		float t17 = rotY*rotZ*t3*t13;
+		float t18 = t2*t10*t12*0.5f;
+		float t27 = t2*t3*t13;
+		float t19 = t14+t18-t27;
+		float t23 = rotX*rotY*t10*t12*0.5f;
+		float t28 = t15-t23;
+		float t20 = rotY*rot_vec_var(1)*t3*t11*t28*0.5f;
+		float t25 = rotX*rotZ*t10*t12*0.5f;
+		float t31 = t16-t25;
+		float t21 = rotZ*rot_vec_var(2)*t3*t11*t31*0.5f;
+		float t22 = t20+t21-rotX*rot_vec_var(0)*t3*t11*t19*0.5f;
+		float t24 = t15-t23;
+		float t26 = t16-t25;
+		float t29 = t4*t10*t12*0.5f;
+		float t34 = t3*t4*t13;
+		float t30 = t14+t29-t34;
+		float t32 = t5*t10*t12*0.5f;
+		float t40 = t3*t5*t13;
+		float t33 = t14+t32-t40;
+		float t36 = rotY*rotZ*t10*t12*0.5f;
+		float t39 = t17-t36;
+		float t35 = rotZ*rot_vec_var(2)*t3*t11*t39*0.5f;
+		float t37 = t15-t23;
+		float t38 = t17-t36;
+		float t41 = rot_vec_var(0)*(t15-t23)*(t16-t25);
+		float t42 = t41-rot_vec_var(1)*t30*t39-rot_vec_var(2)*t33*t39;
+		float t43 = t16-t25;
+		float t44 = t17-t36;
+
+		// auto-code generated using matlab symbolic toolbox
+		P[0][0] = rot_vec_var(0)*t2*t9*t10*0.25f+rot_vec_var(1)*t4*t9*t10*0.25f+rot_vec_var(2)*t5*t9*t10*0.25f;
+		P[0][1] = t22;
+		P[0][2] = t35+rotX*rot_vec_var(0)*t3*t11*(t15-rotX*rotY*t10*t12*0.5f)*0.5f-rotY*rot_vec_var(1)*t3*t11*t30*0.5f;
+		P[0][3] = rotX*rot_vec_var(0)*t3*t11*(t16-rotX*rotZ*t10*t12*0.5f)*0.5f+rotY*rot_vec_var(1)*t3*t11*(t17-rotY*rotZ*t10*t12*0.5f)*0.5f-rotZ*rot_vec_var(2)*t3*t11*t33*0.5f;
+		P[1][0] = t22;
+		P[1][1] = rot_vec_var(0)*(t19*t19)+rot_vec_var(1)*(t24*t24)+rot_vec_var(2)*(t26*t26);
+		P[1][2] = rot_vec_var(2)*(t16-t25)*(t17-rotY*rotZ*t10*t12*0.5f)-rot_vec_var(0)*t19*t28-rot_vec_var(1)*t28*t30;
+		P[1][3] = rot_vec_var(1)*(t15-t23)*(t17-rotY*rotZ*t10*t12*0.5f)-rot_vec_var(0)*t19*t31-rot_vec_var(2)*t31*t33;
+		P[2][0] = t35-rotY*rot_vec_var(1)*t3*t11*t30*0.5f+rotX*rot_vec_var(0)*t3*t11*(t15-t23)*0.5f;
+		P[2][1] = rot_vec_var(2)*(t16-t25)*(t17-t36)-rot_vec_var(0)*t19*t28-rot_vec_var(1)*t28*t30;
+		P[2][2] = rot_vec_var(1)*(t30*t30)+rot_vec_var(0)*(t37*t37)+rot_vec_var(2)*(t38*t38);
+		P[2][3] = t42;
+		P[3][0] = rotZ*rot_vec_var(2)*t3*t11*t33*(-0.5f)+rotX*rot_vec_var(0)*t3*t11*(t16-t25)*0.5f+rotY*rot_vec_var(1)*t3*t11*(t17-t36)*0.5f;
+		P[3][1] = rot_vec_var(1)*(t15-t23)*(t17-t36)-rot_vec_var(0)*t19*t31-rot_vec_var(2)*t31*t33;
+		P[3][2] = t42;
+		P[3][3] = rot_vec_var(2)*(t33*t33)+rot_vec_var(0)*(t43*t43)+rot_vec_var(1)*(t44*t44);
+
+	} else {
+		// the equations are badly conditioned so use a small angle approximation
+		P[0][0] = 0.0f;
+		P[0][1] = 0.0f;
+		P[0][2] = 0.0f;
+		P[0][3] = 0.0f;
+		P[1][0] = 0.0f;
+		P[1][1] = 0.25f*rot_vec_var(0);
+		P[1][2] = 0.0f;
+		P[1][3] = 0.0f;
+		P[2][0] = 0.0f;
+		P[2][1] = 0.0f;
+		P[2][2] = 0.25f*rot_vec_var(1);
+		P[2][3] = 0.0f;
+		P[3][0] = 0.0f;
+		P[3][1] = 0.0f;
+		P[3][2] = 0.0f;
+		P[3][3] = 0.25f*rot_vec_var(2);
+
+	}
+}