Ekf: add more useful methods to interface with the covariances (#543)

EKF/ekf.h

@@ -49,7 +49,7 @@ class Ekf : public EstimatorInterface
 public:
 
 	Ekf() = default;
-	~Ekf() = default;
+	virtual ~Ekf() = default;
 
 	// initialise variables to sane values (also interface class)
 	bool init(uint64_t timestamp);
@@ -122,8 +122,20 @@ public:
 	// get the true airspeed in m/s
 	void get_true_airspeed(float *tas);
 
+	// get the full covariance matrix
+	matrix::SquareMatrix<float, 24> covariances() const { return matrix::SquareMatrix<float, _k_num_states>(P); }
+
 	// get the diagonal elements of the covariance matrix
-	void get_covariances(float *covariances);
+	matrix::Vector<float, 24> covariances_diagonal() const { return covariances().diag(); }
+
+	// get the orientation (quaterion) covariances
+	matrix::SquareMatrix<float, 4> orientation_covariances() const { return covariances().slice<4, 4>(0, 0); }
+
+	// get the linear velocity covariances
+	matrix::SquareMatrix<float, 3> velocity_covariances() const { return covariances().slice<3, 3>(4, 4); }
+
+	// get the position covariances
+	matrix::SquareMatrix<float, 3> position_covariances() const { return covariances().slice<3, 3>(7, 7); }
 
 	// ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined
 	bool collect_gps(const gps_message &gps);

EKF/ekf_helper.cpp

@@ -982,14 +982,6 @@ void Ekf::get_gyro_bias(float bias[3])
 	memcpy(bias, temp, 3 * sizeof(float));
 }
 
-// get the diagonal elements of the covariance matrix
-void Ekf::get_covariances(float *covariances)
-{
-	for (unsigned i = 0; i < _k_num_states; i++) {
-		covariances[i] = P[i][i];
-	}
-}
-
 // get the position and height of the ekf origin in WGS-84 coordinates and time the origin was set
 // return true if the origin is valid
 bool Ekf::get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt)

EKF/estimator_interface.h

@@ -104,8 +104,6 @@ public:
 
 	virtual void get_true_airspeed(float *tas) = 0;
 
-	virtual void get_covariances(float *covariances) = 0;
-
 	// gets the variances for the NED velocity states
 	virtual void get_vel_var(Vector3f &vel_var) = 0;