EKF: Fix bug causing incorrect covariance initialisation

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

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;
 	}
 }