From 2c2850c0ce084f951fbededcfe711924c0da83cf Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Mon, 7 Mar 2016 20:22:06 +1100
Subject: [PATCH] EKF: Add functions to get position and velocity state
 variance

---
 EKF/covariance.cpp | 14 ++++++++++++++
 1 file changed, 14 insertions(+)

diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp
index 59a87db520..ececbb4cb3 100644
--- a/EKF/covariance.cpp
+++ b/EKF/covariance.cpp
@@ -103,6 +103,20 @@ void Ekf::initialiseCovariance()
 
 }
 
+void Ekf::get_pos_var(Vector3f &pos_var)
+{
+	pos_var(0) = P[6][6];
+	pos_var(1) = P[7][7];
+	pos_var(2) = P[8][8];
+}
+
+void Ekf::get_vel_var(Vector3f &vel_var)
+{
+	vel_var(0) = P[3][3];
+	vel_var(1) = P[4][4];
+	vel_var(2) = P[5][5];
+}
+
 void Ekf::predictCovariance()
 {
 	// assign intermediate state variables