EKF: Fix bug causing incorrect covariance initialisation

The covariance initialisation should not be performed before the quaternion states are initialised.

EKF/covariance.cpp

@@ -46,6 +46,8 @@
 #include <math.h>
 #include <mathlib/mathlib.h>
 
+// Sets initial values for the covariance matrix
+// Do not call before quaternion states have been initialised
 void Ekf::initialiseCovariance()
 {
 	P.zero();

EKF/ekf.cpp

@@ -67,8 +67,6 @@ void Ekf::reset()
 	_output_new.pos.setZero();
 	_output_new.quat_nominal.setIdentity();
 
-	initialiseCovariance();
-
 	_delta_angle_corr.setZero();
 
 	_imu_updated = false;
@@ -195,7 +193,6 @@ bool Ekf::initialiseFilter()
 		// calculate the initial magnetic field and yaw alignment
 		_control_status.flags.yaw_align = resetMagHeading(_mag_lpf.getState(), false, false);
 
-
 		// update the yaw angle variance using the variance of the measurement
 		if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
 			// using magnetic heading tuning parameter
@@ -216,6 +213,9 @@ bool Ekf::initialiseFilter()
 		// reset the output predictor state history to match the EKF initial values
 		alignOutputFilter();
 
+		// initialise the state covariance matrix now we have starting values for all lthe states
+		initialiseCovariance();
+
 		return true;
 	}
 }

EKF/ekf.h

@@ -746,6 +746,7 @@ private:
 	Vector3f calcRotVecVariances();
 
 	// initialise the quaternion covariances using rotation vector variances
+	// do not call before quaternion states are initialised
 	void initialiseQuatCovariances(Vector3f &rot_vec_var);
 
 	// perform a limited reset of the magnetic field state covariances

EKF/ekf_helper.cpp

@@ -1480,6 +1480,7 @@ Vector3f Ekf::calcRotVecVariances()
 }
 
 // initialise the quaternion covariances using rotation vector variances
+// do not call before quaternion states are initialised
 void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var)
 {
 	// calculate an equivalent rotation vector from the quaternion