From 472f2286b7a4e1c5c697ef3406b379d65a791b1f Mon Sep 17 00:00:00 2001
From: CarlOlsson <carlolsson.co@gmail.com>
Date: Thu, 10 Jan 2019 13:32:28 +0100
Subject: [PATCH] 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 | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp
index ae524295d3..6c4f47cb2c 100644
--- a/EKF/ekf.cpp
+++ b/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();