From 23038e7243c3426fa9d1b38cdecd953f8ef4f89d Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Sat, 9 Jan 2016 11:10:57 +1100
Subject: [PATCH] AP_NavEKF2: Use measurement uncertainties to initialise
 covariance

---
 libraries/AP_NavEKF2/AP_NavEKF2_core.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
index 1a3193fafa..2f3db89836 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
@@ -359,11 +359,11 @@ void NavEKF2_core::CovarianceInit()
     P[1][1]   = 0.1f;
     P[2][2]   = 0.1f;
     // velocities
-    P[3][3]   = sq(0.7f);
+    P[3][3]   = sq(frontend->_gpsHorizVelNoise);
     P[4][4]   = P[3][3];
-    P[5][5]   = sq(0.7f);
+    P[5][5]   = sq(frontend->_gpsVertVelNoise);
     // positions
-    P[6][6]   = sq(15.0f);
+    P[6][6]   = sq(frontend->_gpsHorizPosNoise);
     P[7][7]   = P[6][6];
     P[8][8]   = sq(frontend->_baroAltNoise);
     // gyro delta angle biases