EKF: initialize covariances before we reset the heading in order to preserve the yaw uncertainty

Signed-off-by: CarlOlsson <carlolsson.co@gmail.com>

EKF/ekf.cpp

@@ -232,6 +232,9 @@ bool Ekf::initialiseFilter()
 		_state.mag_B.setZero();
 		_state.wind_vel.setZero();
 
+		// initialise the state covariance matrix
+		initialiseCovariance();
+
 		// get initial roll and pitch estimate from delta velocity vector, assuming vehicle is static
 		float pitch = 0.0f;
 		float roll = 0.0f;
@@ -276,9 +279,6 @@ bool Ekf::initialiseFilter()
 
 		}
 
-		// initialise the state covariance matrix
-		initialiseCovariance();
-
 		// try to initialise the terrain estimator
 		_terrain_initialised = initHagl();