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/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));