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