log ekf2 innovations and innovation variances

src/modules/ekf2/ekf2_main.cpp

@@ -73,6 +73,7 @@
 #include <uORB/topics/control_state.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/estimator_status.h>
+#include <uORB/topics/ekf2_innovations.h>
 
 #include <ecl/EKF/ekf.h>
 
@@ -133,6 +134,7 @@ private:
 	orb_advert_t _control_state_pub;
 	orb_advert_t _vehicle_global_position_pub;
 	orb_advert_t _estimator_status_pub;
+	orb_advert_t _estimator_innovations_pub;
 
 
 	/* Low pass filter for attitude rates */
@@ -178,6 +180,7 @@ Ekf2::Ekf2():
 	_control_state_pub(nullptr),
 	_vehicle_global_position_pub(nullptr),
 	_estimator_status_pub(nullptr),
+	_estimator_innovations_pub(nullptr),
 	_lp_roll_rate(250.0f, 30.0f),
 	_lp_pitch_rate(250.0f, 30.0f),
 	_lp_yaw_rate(250.0f, 20.0f),
@@ -490,6 +493,25 @@ void Ekf2::task_main()
 		} else {
 			orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &status);
 		}
+
+		// publish estimator innovation data
+		struct ekf2_innovations_s innovations = {};
+		innovations.timestamp = hrt_absolute_time();
+		_ekf->get_vel_pos_innov(&innovations.vel_pos_innov[0]);
+		_ekf->get_mag_innov(&innovations.mag_innov[0]);
+		_ekf->get_heading_innov(&innovations.heading_innov);
+
+		_ekf->get_vel_pos_innov_var(&innovations.vel_pos_innov_var[0]);
+		_ekf->get_mag_innov_var(&innovations.mag_innov_var[0]);
+		_ekf->get_heading_innov_var(&innovations.heading_innov_var);
+
+		if (_estimator_innovations_pub == nullptr) {
+			_estimator_innovations_pub = orb_advertise(ORB_ID(ekf2_innovations), &innovations);
+
+		} else {
+			orb_publish(ORB_ID(ekf2_innovations), _estimator_innovations_pub, &innovations);
+		}
+
 	}
 
 	delete ekf2::instance;

src/modules/sdlog2/sdlog2.c

@@ -109,6 +109,7 @@
 #include <uORB/topics/vtol_vehicle_status.h>
 #include <uORB/topics/time_offset.h>
 #include <uORB/topics/mc_att_ctrl_status.h>
+#include <uORB/topics/ekf2_innovations.h>
 
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
@@ -1095,6 +1096,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct time_offset_s time_offset;
 		struct mc_att_ctrl_status_s mc_att_ctrl_status;
 		struct control_state_s ctrl_state;
+		struct ekf2_innovations_s innovations;
 	} buf;
 
 	memset(&buf, 0, sizeof(buf));
@@ -1144,6 +1146,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_TSYN_s log_TSYN;
 			struct log_MACS_s log_MACS;
 			struct log_CTS_s log_CTS;
+			struct log_EST4_s log_INO1;
+			struct log_EST5_s log_INO2;
 		} body;
 	} log_msg = {
 		LOG_PACKET_HEADER_INIT(0)
@@ -1187,6 +1191,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int tsync_sub;
 		int mc_att_ctrl_status_sub;
 		int ctrl_state_sub;
+		int innov_sub;
 	} subs;
 
 	subs.cmd_sub = -1;
@@ -1222,6 +1227,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 	subs.mc_att_ctrl_status_sub = -1;
 	subs.ctrl_state_sub = -1;
 	subs.encoders_sub = -1;
+	subs.innov_sub = -1;
 
 	/* add new topics HERE */
 
@@ -1810,6 +1816,29 @@ int sdlog2_thread_main(int argc, char *argv[])
 			LOGBUFFER_WRITE_AND_COUNT(EST3);
 		}
 
+		/* --- EKF2 INNOVATIONS --- */
+		if (copy_if_updated(ORB_ID(ekf2_innovations), &subs.innov_sub, &buf.innovations)) {
+			log_msg.msg_type = LOG_EST4_MSG;
+			memset(&(log_msg.body.log_INO1.s), 0, sizeof(log_msg.body.log_INO1.s));
+			for (unsigned i = 0; i < 6; i++) {
+				log_msg.body.log_INO1.s[i] = buf.innovations.vel_pos_innov[i];
+				log_msg.body.log_INO1.s[i + 6] = buf.innovations.vel_pos_innov_var[i];
+			}
+			LOGBUFFER_WRITE_AND_COUNT(EST4);
+
+			log_msg.msg_type = LOG_EST5_MSG;
+			memset(&(log_msg.body.log_INO2.s), 0, sizeof(log_msg.body.log_INO2.s));
+			for (unsigned i = 0; i < 3; i++) {
+				log_msg.body.log_INO2.s[i] = buf.innovations.mag_innov[i];
+				log_msg.body.log_INO2.s[i + 3] = buf.innovations.mag_innov_var[i];
+			}
+
+			log_msg.body.log_INO2.s[6] = buf.innovations.heading_innov;
+			log_msg.body.log_INO2.s[7] = buf.innovations.heading_innov_var;
+			LOGBUFFER_WRITE_AND_COUNT(EST5);
+
+		}
+
 		/* --- TECS STATUS --- */
 		if (copy_if_updated(ORB_ID(tecs_status), &subs.tecs_status_sub, &buf.tecs_status)) {
 			log_msg.msg_type = LOG_TECS_MSG;

src/modules/sdlog2/sdlog2_messages.h

@@ -416,6 +416,18 @@ struct log_EST3_s {
     float cov[16];
 };
 
+/* --- EST4 - ESTIMATOR INNOVATIONS --- */
+#define LOG_EST4_MSG 48
+struct log_EST4_s {
+    float s[12];
+};
+
+/* --- EST4 - ESTIMATOR INNOVATIONS --- */
+#define LOG_EST5_MSG 49
+struct log_EST5_s {
+    float s[8];
+};
+
 /* --- TEL0..3 - TELEMETRY STATUS --- */
 #define LOG_TEL0_MSG 36
 #define LOG_TEL1_MSG 37
@@ -553,6 +565,8 @@ static const struct log_format_s log_formats[] = {
 	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(EST4, "ffffffffffff", "VxI,VyI,VzI,PxI,PyI,PzI,VxIV,VyIV,VzIV,PxIV,PyIV,PzIV"),
+	LOG_FORMAT(EST5, "ffffffff", "MAGxI,MAGyI,MAGzI,MAGxIV,MAGyIV,MAGzIV,HeadI,HeadIV"),
 	LOG_FORMAT(PWR, "fffBBBBB",		"Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"),
 	LOG_FORMAT(MOCP, "fffffff",		"QuatW,QuatX,QuatY,QuatZ,X,Y,Z"),
 	LOG_FORMAT(VISN, "ffffffffff",		"X,Y,Z,VX,VY,VZ,QuatW,QuatX,QuatY,QuatZ"),