added covariances to estimator_status and logging

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@@ -478,8 +478,13 @@ int AttitudePositionEstimatorEKF::check_filter_state()
 		size_t max_states = (sizeof(rep.states) / sizeof(rep.states[0]));
 		rep.n_states = (ekf_n_states < max_states) ? ekf_n_states : max_states;
 
+		// Copy diagonal elemnts of covariance matrix
+		float covariances[28];
+		_ekf->get_covariance(covariances);
+		
 		for (size_t i = 0; i < rep.n_states; i++) {
 			rep.states[i] = ekf_report.states[i];
+			rep.covariances[i] = covariances[i];
 		}
 
 

src/modules/ekf_att_pos_estimator/estimator_22states.cpp

@@ -3337,3 +3337,10 @@ void AttPosEKF::setIsFixedWing(const bool fixedWing)
 {
     _isFixedWing = fixedWing;
 }
+
+void AttPosEKF::get_covariance(float c[EKF_STATE_ESTIMATES])
+{
+    for (unsigned int i = 0; i < EKF_STATE_ESTIMATES; i++) {
+        c[i] = P[i][i];
+    }
+}

src/modules/ekf_att_pos_estimator/estimator_22states.h

@@ -379,6 +379,8 @@ public:
      */
     void ZeroVariables();
 
+    void get_covariance(float c[28]);
+
 protected:
 
     /**