Merge pull request #2459 from wingtra/estimator_LP_rates_log_covariances

EKF Enhancments: LP filters on rates and covariance added to logging

msg/estimator_status.msg

@@ -4,3 +4,4 @@ float32 n_states		# Number of states effectively used
 uint8 nan_flags			# Bitmask to indicate NaN states
 uint8 health_flags		# Bitmask to indicate sensor health states (vel, pos, hgt)
 uint8 timeout_flags		# Bitmask to indicate timeout flags (vel, pos, hgt)
+float32[28] covariances	# Diagonal Elements of Covariance Matrix

src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h

@@ -66,6 +66,8 @@
 #include <drivers/drv_mag.h>
 #include <drivers/drv_baro.h>
 
+#include <mathlib/math/filter/LowPassFilter2p.hpp>
+
 #include <geo/geo.h>
 #include <systemlib/perf_counter.h>
 
@@ -258,6 +260,11 @@ private:
 
     AttPosEKF                   *_ekf;
 
+    /* Low pass filter for attitude rates */
+    math::LowPassFilter2p _LP_att_P;
+    math::LowPassFilter2p _LP_att_Q;
+    math::LowPassFilter2p _LP_att_R;
+
 private:
     /**
      * Update our local parameter cache.

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@@ -200,9 +200,13 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
     _newRangeData(false),
 
     _mavlink_fd(-1),
-    _parameters {},
+    _parameters{},
-    _parameter_handles {},
+    _parameter_handles{},
-    _ekf(nullptr)
+    _ekf(nullptr),
+
+    _LP_att_P(100.0f, 10.0f),
+    _LP_att_Q(100.0f, 10.0f),
+    _LP_att_R(100.0f, 10.0f)
 {
 	_last_run = hrt_absolute_time();
 
@@ -474,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];
 		}
 
 
@@ -819,9 +828,9 @@ void AttitudePositionEstimatorEKF::publishAttitude()
 	_att.pitch = euler(1);
 	_att.yaw = euler(2);
 
-	_att.rollspeed = _ekf->angRate.x - _ekf->states[10] / _ekf->dtIMUfilt;
+	_att.rollspeed = _LP_att_P.apply(_ekf->angRate.x) - _ekf->states[10] / _ekf->dtIMUfilt;
-	_att.pitchspeed = _ekf->angRate.y - _ekf->states[11] / _ekf->dtIMUfilt;
+	_att.pitchspeed = _LP_att_Q.apply(_ekf->angRate.y) - _ekf->states[11] / _ekf->dtIMUfilt;
-	_att.yawspeed = _ekf->angRate.z - _ekf->states[12] / _ekf->dtIMUfilt;
+	_att.yawspeed = _LP_att_R.apply(_ekf->angRate.z) - _ekf->states[12] / _ekf->dtIMUfilt;
 
 	// gyro offsets
 	_att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMUfilt;

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:
 
     /**

src/modules/sdlog2/sdlog2.c

@@ -1126,6 +1126,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_TEL_s log_TEL;
 			struct log_EST0_s log_EST0;
 			struct log_EST1_s log_EST1;
+			struct log_EST2_s log_EST2;
+			struct log_EST3_s log_EST3;
 			struct log_PWR_s log_PWR;
 			struct log_VICN_s log_VICN;
 			struct log_VISN_s log_VISN;
@@ -1845,6 +1847,18 @@ int sdlog2_thread_main(int argc, char *argv[])
 			memset(&(log_msg.body.log_EST1.s), 0, sizeof(log_msg.body.log_EST1.s));
 			memcpy(&(log_msg.body.log_EST1.s), buf.estimator_status.states + maxcopy0, maxcopy1);
 			LOGBUFFER_WRITE_AND_COUNT(EST1);
+
+			log_msg.msg_type = LOG_EST2_MSG;
+			unsigned maxcopy2 = (sizeof(buf.estimator_status.covariances) < sizeof(log_msg.body.log_EST2.cov)) ? sizeof(buf.estimator_status.covariances) : sizeof(log_msg.body.log_EST2.cov);
+			memset(&(log_msg.body.log_EST2.cov), 0, sizeof(log_msg.body.log_EST2.cov));
+			memcpy(&(log_msg.body.log_EST2.cov), buf.estimator_status.covariances, maxcopy2);
+			LOGBUFFER_WRITE_AND_COUNT(EST2);
+
+			log_msg.msg_type = LOG_EST3_MSG;
+			unsigned maxcopy3 = ((sizeof(buf.estimator_status.covariances) - maxcopy2) < sizeof(log_msg.body.log_EST3.cov)) ? (sizeof(buf.estimator_status.covariances) - maxcopy2) : sizeof(log_msg.body.log_EST3.cov);
+			memset(&(log_msg.body.log_EST3.cov), 0, sizeof(log_msg.body.log_EST3.cov));
+			memcpy(&(log_msg.body.log_EST3.cov), buf.estimator_status.covariances + maxcopy2, maxcopy3);
+			LOGBUFFER_WRITE_AND_COUNT(EST3);
 		}
 
 		/* --- TECS STATUS --- */

src/modules/sdlog2/sdlog2_messages.h

@@ -402,11 +402,23 @@ struct log_EST1_s {
 	float s[16];
 };
 
+/* --- EST2 - ESTIMATOR STATUS --- */
+#define LOG_EST2_MSG 34
+struct log_EST2_s {
+    float cov[12];
+};
+
+/* --- EST3 - ESTIMATOR STATUS --- */
+#define LOG_EST3_MSG 35
+struct log_EST3_s {
+    float cov[16];
+};
+
 /* --- TEL0..3 - TELEMETRY STATUS --- */
-#define LOG_TEL0_MSG 34
+#define LOG_TEL0_MSG 36
-#define LOG_TEL1_MSG 35
+#define LOG_TEL1_MSG 37
-#define LOG_TEL2_MSG 36
+#define LOG_TEL2_MSG 38
-#define LOG_TEL3_MSG 37
+#define LOG_TEL3_MSG 39
 struct log_TEL_s {
 	uint8_t rssi;
 	uint8_t remote_rssi;
@@ -419,7 +431,7 @@ struct log_TEL_s {
 };
 
 /* --- VISN - VISION POSITION --- */
-#define LOG_VISN_MSG 38
+#define LOG_VISN_MSG 40
 struct log_VISN_s {
 	float x;
 	float y;
@@ -434,7 +446,7 @@ struct log_VISN_s {
 };
 
 /* --- ENCODERS - ENCODER DATA --- */
-#define LOG_ENCD_MSG 39
+#define LOG_ENCD_MSG 41
 struct log_ENCD_s {
 	int64_t cnt0;
 	float vel0;
@@ -443,22 +455,22 @@ struct log_ENCD_s {
 };
 
 /* --- AIR SPEED SENSORS - DIFF. PRESSURE --- */
-#define LOG_AIR1_MSG 41
+#define LOG_AIR1_MSG 42
 
 /* --- VTOL - VTOL VEHICLE STATUS */
-#define LOG_VTOL_MSG 42
+#define LOG_VTOL_MSG 43
 struct log_VTOL_s {
 	float airspeed_tot;
 };
 
 /* --- TIMESYNC - TIME SYNCHRONISATION OFFSET */
-#define LOG_TSYN_MSG 43
+#define LOG_TSYN_MSG 44
 struct log_TSYN_s {
 	uint64_t time_offset;
 };
 
 /* --- MACS - MULTIROTOR ATTITUDE CONTROLLER STATUS */
-#define LOG_MACS_MSG 44
+#define LOG_MACS_MSG 45
 struct log_MACS_s {
 	float roll_rate_integ;
 	float pitch_rate_integ;
@@ -522,6 +534,8 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT_S(TEL3, TEL, "BBBBHHBQ",		"RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
 	LOG_FORMAT(EST0, "ffffffffffffBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,nStat,fNaN,fHealth,fTOut"),
 	LOG_FORMAT(EST1, "ffffffffffffffff",	"s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"),
+	LOG_FORMAT(EST2, "ffffffffffff",    "P0,P1,P2,P3,P4,P5,P6,P7,P8,P9,P10,P11"),
+	LOG_FORMAT(EST3, "ffffffffffffffff",    "P12,P13,P14,P15,P16,P17,P18,P19,P20,P21,P22,P23,P24,P25,P26,P27"),
 	LOG_FORMAT(PWR, "fffBBBBB",		"Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"),
 	LOG_FORMAT(VICN, "ffffff",		"X,Y,Z,Roll,Pitch,Yaw"),
 	LOG_FORMAT(VISN, "ffffffffff",		"X,Y,Z,VX,VY,VZ,QuatX,QuatY,QuatZ,QuatW"),