ECL: Clean velPos logging, deprecate ekf2_innovations msg

Tools/ecl_ekf/analyse_logdata_ekf.py

@@ -36,10 +36,10 @@ def analyse_ekf(
         raise PreconditionError('could not find estimator_status data')
 
     try:
-        _ = ulog.get_dataset('ekf2_innovations').data
-        print('found ekf2_innovation data')
+        _ = ulog.get_dataset('estimator_innovations').data
+        print('found estimator_innovations data')
     except:
-        raise PreconditionError('could not find ekf2_innovation data')
+        raise PreconditionError('could not find estimator_innovations data')
 
     try:
         in_air = InAirDetector(
@@ -133,6 +133,3 @@ def find_checks_that_apply(
 
 
 
-
-
-

Tools/ecl_ekf/analysis/data_version_handler.py

@@ -0,0 +1,48 @@
+#! /usr/bin/env python3
+"""
+function collection for handling different versions of log files
+"""
+from pyulog import ULog
+
+from analysis.detectors import PreconditionError
+
+def get_output_tracking_error_message(ulog: ULog) -> str:
+    """
+    return the name of the message containing the output_tracking_error
+    :param ulog:
+    :return: str
+    """
+    for elem in  ulog.data_list:
+        if elem.name == "ekf2_innovations":
+            return "ekf2_innovations"
+        if elem.name == "estimator_innovations":
+            return "estimator_status"
+
+    raise PreconditionError("Could not detect the message containing the output tracking error")
+
+def get_innovation_message(ulog: ULog, topic: str = 'innovation') -> str:
+    """
+    return the name of the innovation message (old: ekf2_innovations; new: estimator_innovations)
+    :param ulog:
+    :return: str
+    """
+    if topic == 'innovation':
+        for elem in  ulog.data_list:
+            if elem.name == "ekf2_innovations":
+                return "ekf2_innovations"
+            if elem.name == "estimator_innovations":
+                return "estimator_innovations"
+    if topic == 'innovation_variance':
+        for elem in  ulog.data_list:
+            if elem.name == "ekf2_innovations":
+                return "ekf2_innovations"
+            if elem.name == "estimator_innovations":
+                return "estimator_innovations"
+    if topic == 'innovation_test_ratio':
+        for elem in  ulog.data_list:
+            if elem.name == "ekf2_innovations":
+                return "ekf2_innovations"
+            if elem.name == "estimator_innovations":
+                return "estimator_innovations"
+
+    raise PreconditionError("Could not detect the message")

Tools/ecl_ekf/analysis/detectors.py

@@ -179,4 +179,4 @@ class InAirDetector(object):
                                              (data['timestamp'] - self._ulog.start_timestamp) /
                                              1.0e6 < self._in_air[i].landing))[0])
 
-        return airtime
+        return airtime

Tools/ecl_ekf/analysis/metrics.py

@@ -134,8 +134,6 @@ def calculate_innov_fail_metrics(
 def calculate_imu_metrics(
         ulog: ULog, in_air_no_ground_effects: InAirDetector) -> dict:
 
-    ekf2_innovation_data = ulog.get_dataset('ekf2_innovations').data
-
     estimator_status_data = ulog.get_dataset('estimator_status').data
 
     imu_metrics = dict()
@@ -145,7 +143,7 @@ def calculate_imu_metrics(
                            ('output_tracking_error[1]', 'output_obs_vel_err_median'),
                            ('output_tracking_error[2]', 'output_obs_pos_err_median')]:
         imu_metrics[result] = calculate_stat_from_signal(
-            ekf2_innovation_data, 'ekf2_innovations', signal, in_air_no_ground_effects, np.median)
+            estimator_status_data, 'estimator_status', signal, in_air_no_ground_effects, np.median)
 
     # calculates peak and mean for IMU vibration checks
     for signal, result in [('vibe[0]', 'imu_coning'),

Tools/ecl_ekf/analysis/post_processing.py

@@ -42,6 +42,17 @@ def get_control_mode_flags(estimator_status: dict) -> dict:
     # 12 - true when local position data from external vision is being fused
     # 13 - true when yaw data from external vision measurements is being fused
     # 14 - true when height data from external vision measurements is being fused
+    # 15 - true when synthetic sideslip measurements are being fused
+    # 16 - true true when the mag field does not match the expected strength
+    # 17 - true true when the vehicle is operating as a fixed wing vehicle
+    # 18 - true when the magnetometer has been declared faulty and is no longer being used
+    # 19 - true true when airspeed measurements are being fused
+    # 20 - true true when protection from ground effect induced static pressure rise is active
+    # 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough
+    # 22 - true when yaw (not ground course) data from a GPS receiver is being fused
+    # 23 - true when the in-flight mag field alignment has been completed
+    # 24 - true when local earth frame velocity data from external vision measurements are being fused
+    # 25 - true when we are using a synthesized measurement for the magnetometer Z component
     control_mode['tilt_aligned'] = ((2 ** 0 & estimator_status['control_mode_flags']) > 0) * 1
     control_mode['yaw_aligned'] = ((2 ** 1 & estimator_status['control_mode_flags']) > 0) * 1
     control_mode['using_gps'] = ((2 ** 2 & estimator_status['control_mode_flags']) > 0) * 1
@@ -57,9 +68,19 @@ def get_control_mode_flags(estimator_status: dict) -> dict:
     control_mode['using_evpos'] = ((2 ** 12 & estimator_status['control_mode_flags']) > 0) * 1
     control_mode['using_evyaw'] = ((2 ** 13 & estimator_status['control_mode_flags']) > 0) * 1
     control_mode['using_evhgt'] = ((2 ** 14 & estimator_status['control_mode_flags']) > 0) * 1
+    control_mode['fuse_beta'] = ((2 ** 15 & estimator_status['control_mode_flags']) > 0) * 1
+    control_mode['mag_field_disturbed'] = ((2 ** 16 & estimator_status['control_mode_flags']) > 0) * 1
+    control_mode['fixed_wing'] = ((2 ** 17 & estimator_status['control_mode_flags']) > 0) * 1
+    control_mode['mag_fault'] = ((2 ** 18 & estimator_status['control_mode_flags']) > 0) * 1
+    control_mode['fuse_aspd'] = ((2 ** 19 & estimator_status['control_mode_flags']) > 0) * 1
+    control_mode['gnd_effect'] = ((2 ** 20 & estimator_status['control_mode_flags']) > 0) * 1
+    control_mode['rng_stuck'] = ((2 ** 21 & estimator_status['control_mode_flags']) > 0) * 1
+    control_mode['gps_yaw'] = ((2 ** 22 & estimator_status['control_mode_flags']) > 0) * 1
+    control_mode['mag_aligned_in_flight'] = ((2 ** 23 & estimator_status['control_mode_flags']) > 0) * 1
+    control_mode['ev_vel'] = ((2 ** 24 & estimator_status['control_mode_flags']) > 0) * 1
+    control_mode['synthetic_mag_z'] = ((2 ** 25 & estimator_status['control_mode_flags']) > 0) * 1
     return control_mode
 
-
 def get_innovation_check_flags(estimator_status: dict) -> dict:
     """
     :param estimator_status:

Tools/ecl_ekf/plotting/pdf_report.py

@@ -15,6 +15,7 @@ from analysis.post_processing import magnetic_field_estimates_from_status, get_e
 from plotting.data_plots import TimeSeriesPlot, InnovationPlot, ControlModeSummaryPlot, \
     CheckFlagsPlot
 from analysis.detectors import PreconditionError
+import analysis.data_version_handler as dvh
 
 def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
     """
@@ -34,10 +35,15 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
         raise PreconditionError('could not find estimator_status data')
 
     try:
-        ekf2_innovations = ulog.get_dataset('ekf2_innovations').data
-        print('found ekf2_innovation data')
+        estimator_innovations = ulog.get_dataset('estimator_innovations').data
+        estimator_innovation_variances = ulog.get_dataset('estimator_innovation_variances').data
+        innovation_data = estimator_innovations
+        for key in estimator_innovation_variances:
+            # append 'var' to the field name such that we can distingush between innov and innov_var
+            innovation_data.update({str('var_'+key): estimator_innovation_variances[key]})
+        print('found innovation data')
     except:
-        raise PreconditionError('could not find ekf2_innovation data')
+        raise PreconditionError('could not find innovation data')
 
     try:
         sensor_preflight = ulog.get_dataset('sensor_preflight').data
@@ -67,8 +73,8 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
 
         # vertical velocity and position innovations
         data_plot = InnovationPlot(
-            ekf2_innovations, [('vel_pos_innov[2]', 'vel_pos_innov_var[2]'),
-                               ('vel_pos_innov[5]', 'vel_pos_innov_var[5]')],
+            innovation_data, [('gps_vpos', 'var_gps_vpos'),
+                               ('gps_vvel', 'var_gps_vvel')],
             x_labels=['time (sec)', 'time (sec)'],
             y_labels=['Down Vel (m/s)', 'Down Pos (m)'], plot_title='Vertical Innovations',
             pdf_handle=pdf_pages)
@@ -77,8 +83,8 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
 
         # horizontal velocity innovations
         data_plot = InnovationPlot(
-            ekf2_innovations, [('vel_pos_innov[0]', 'vel_pos_innov_var[0]'),
-                               ('vel_pos_innov[1]','vel_pos_innov_var[1]')],
+            innovation_data, [('gps_hvel[0]', 'var_gps_hvel[0]'),
+                              ('gps_hvel[1]', 'var_gps_hvel[1]')],
             x_labels=['time (sec)', 'time (sec)'],
             y_labels=['North Vel (m/s)', 'East Vel (m/s)'],
             plot_title='Horizontal Velocity  Innovations', pdf_handle=pdf_pages)
@@ -87,8 +93,8 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
 
         # horizontal position innovations
         data_plot = InnovationPlot(
-            ekf2_innovations, [('vel_pos_innov[3]', 'vel_pos_innov_var[3]'), ('vel_pos_innov[4]',
-                                                                              'vel_pos_innov_var[4]')],
+            innovation_data, [('gps_hpos[0]', 'var_gps_hpos[0]'),
+                              ('gps_hpos[1]', 'var_gps_hpos[1]')],
             x_labels=['time (sec)', 'time (sec)'],
             y_labels=['North Pos (m)', 'East Pos (m)'], plot_title='Horizontal Position Innovations',
             pdf_handle=pdf_pages)
@@ -97,8 +103,9 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
 
         # magnetometer innovations
         data_plot = InnovationPlot(
-            ekf2_innovations, [('mag_innov[0]', 'mag_innov_var[0]'),
-           ('mag_innov[1]', 'mag_innov_var[1]'), ('mag_innov[2]', 'mag_innov_var[2]')],
+            innovation_data, [('mag_field[0]', 'var_mag_field[0]'),
+                              ('mag_field[1]', 'var_mag_field[1]'),
+                              ('mag_field[2]', 'var_mag_field[2]')],
             x_labels=['time (sec)', 'time (sec)', 'time (sec)'],
             y_labels=['X (Gauss)', 'Y (Gauss)', 'Z (Gauss)'], plot_title='Magnetometer Innovations',
             pdf_handle=pdf_pages)
@@ -107,7 +114,7 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
 
         # magnetic heading innovations
         data_plot = InnovationPlot(
-            ekf2_innovations, [('heading_innov', 'heading_innov_var')],
+            innovation_data, [('heading', 'var_heading')],
             x_labels=['time (sec)'], y_labels=['Heading (rad)'],
             plot_title='Magnetic Heading Innovations', pdf_handle=pdf_pages)
         data_plot.save()
@@ -115,8 +122,8 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
 
         # air data innovations
         data_plot = InnovationPlot(
-            ekf2_innovations,
-            [('airspeed_innov', 'airspeed_innov_var'), ('beta_innov', 'beta_innov_var')],
+            innovation_data, [('airspeed', 'var_airspeed'),
+                              ('beta', 'var_beta')],
             x_labels=['time (sec)', 'time (sec)'],
             y_labels=['innovation (m/sec)', 'innovation (rad)'],
             sub_titles=['True Airspeed Innovations', 'Synthetic Sideslip Innovations'],
@@ -126,8 +133,8 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
 
         # optical flow innovations
         data_plot = InnovationPlot(
-            ekf2_innovations, [('flow_innov[0]', 'flow_innov_var[0]'), ('flow_innov[1]',
-                                                                        'flow_innov_var[1]')],
+            innovation_data, [('flow[0]', 'var_flow[0]'),
+                              ('flow[1]', 'var_flow[1]')],
             x_labels=['time (sec)', 'time (sec)'],
             y_labels=['X (rad/sec)', 'Y (rad/sec)'],
             plot_title='Optical Flow Innovations', pdf_handle=pdf_pages)
@@ -252,12 +259,12 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
 
         # Plot the EKF output observer tracking errors
         scaled_innovations = {
-            'output_tracking_error[0]': 1000. * ekf2_innovations['output_tracking_error[0]'],
-            'output_tracking_error[1]': ekf2_innovations['output_tracking_error[1]'],
-            'output_tracking_error[2]': ekf2_innovations['output_tracking_error[2]']
+            'output_tracking_error[0]': 1000. * estimator_status['output_tracking_error[0]'],
+            'output_tracking_error[1]': estimator_status['output_tracking_error[1]'],
+            'output_tracking_error[2]': estimator_status['output_tracking_error[2]']
             }
         data_plot = CheckFlagsPlot(
-            1e-6 * ekf2_innovations['timestamp'], scaled_innovations,
+            1e-6 * estimator_status['timestamp'], scaled_innovations,
             [['output_tracking_error[0]'], ['output_tracking_error[1]'],
              ['output_tracking_error[2]']], x_label='time (sec)',
             y_labels=['angles (mrad)', 'velocity (m/s)', 'position (m)'],
@@ -350,4 +357,4 @@ def detect_airtime(control_mode, status_time):
             in_air_duration = float('NaN')
     else:
         in_air_duration = float('NaN')
-    return b_finishes_in_air, b_starts_in_air, in_air_duration, in_air_transition_time, on_ground_transition_time
+    return b_finishes_in_air, b_starts_in_air, in_air_duration, in_air_transition_time, on_ground_transition_time

msg/CMakeLists.txt

@@ -55,13 +55,13 @@ set(msg_files
 	debug_vect.msg
 	differential_pressure.msg
 	distance_sensor.msg
-	ekf2_innovations.msg		# TODO: remove as soon as https://github.com/PX4/Firmware/pull/13127 is rebased over this
 	ekf2_timestamps.msg
 	ekf_gps_drift.msg
 	ekf_gps_position.msg
 	esc_report.msg
 	esc_status.msg
 	estimator_status.msg
+	estimator_innovations.msg
 	follow_target.msg
 	geofence_result.msg
 	gps_dump.msg
@@ -159,8 +159,7 @@ set(msg_files
 	)
 
 set(deprecated_msgs
-	# TODO: uncomment as soon as https://github.com/PX4/Firmware/pull/13127 is rebased over this
-	# ekf2_innovations.msg	# 2019-11-22, Updated estimator interface and logging; replaced by 'estimator_innovations'.
+	ekf2_innovations.msg	# 2019-11-22, Updated estimator interface and logging; replaced by 'estimator_innovations'.
 	)
 
 foreach(msg IN LISTS deprecated_msgs)

msg/ekf2_innovations.msg

@@ -1,19 +0,0 @@
-uint64 timestamp		# time since system start (microseconds)
-float32[6] vel_pos_innov	# velocity and position innovations
-float32[3] mag_innov 		# earth magnetic field innovations
-float32 heading_innov 		# heading innovation
-float32 airspeed_innov		# airspeed innovation
-float32 beta_innov		# synthetic sideslip innovation
-float32[2] flow_innov   	# flow innovation
-float32 hagl_innov      	# innovation from the terrain estimator for the height above ground level measurement  (m)
-float32[6] vel_pos_innov_var 	# velocity and position innovation variances
-float32[3] mag_innov_var	# earth magnetic field innovation variance
-float32 heading_innov_var 	# heading innovation variance
-float32 airspeed_innov_var	# Airspeed innovation variance
-float32 beta_innov_var		# synthetic sideslip innovation variance
-float32[2] flow_innov_var	# flow innovation variance
-float32 hagl_innov_var          # innovation variance from the terrain estimator for the height above ground level measurement (m^2)
-float32[3] output_tracking_error # return a vector containing the output predictor angular, velocity and position tracking error magnitudes (rad), (m/s), (m)
-float32[2] drag_innov		# drag specific force innovation (m/s/s)
-float32[2] drag_innov_var	# drag specific force innovation variance (m/s/s)**2
-float32[2] aux_vel_innov	# auxiliary NE velocity innovations from landing target measurement (m/s)

msg/estimator_innovations.msg

@@ -0,0 +1,43 @@
+uint64 timestamp		# time since system start (microseconds)
+
+# GPS
+float32[2] gps_hvel	# horizontal GPS velocity innovation (m/sec) and innovation variance ((m/sec)**2)
+float32    gps_vvel	# vertical GPS velocity innovation (m/sec) and innovation variance ((m/sec)**2)
+float32[2] gps_hpos	# horizontal GPS position innovation (m) and innovation variance (m**2)
+float32    gps_vpos	# vertical GPS position innovation (m) and innovation variance (m**2)
+
+# External Vision
+float32[2] ev_hvel	# horizontal external vision velocity innovation (m/sec) and innovation variance ((m/sec)**2)
+float32    ev_vvel	# vertical external vision velocity innovation (m/sec) and innovation variance ((m/sec)**2)
+float32[2] ev_hpos	# horizontal external vision position innovation (m) and innovation variance (m**2)
+float32    ev_vpos	# vertical external vision position innovation (m) and innovation variance (m**2)
+
+# Fake Position and Velocity
+float32[2] fake_hvel	# fake horizontal velocity innovation (m/s) and innovation variance ((m/s)**2)
+float32    fake_vvel	# fake vertical velocity innovation (m/s) and innovation variance ((m/s)**2)
+float32[2] fake_hpos	# fake horizontal position innovation (m) and innovation variance (m**2)
+float32    fake_vpos	# fake vertical position innovation (m) and innovation variance (m**2)
+
+# Height sensors
+float32 rng_vpos	# range sensor height innovation (m) and innovation variance (m**2)
+float32 baro_vpos	# barometer height innovation (m) and innovation variance (m**2)
+
+# Auxiliary velocity
+float32[2] aux_hvel	# horizontal auxiliar velocity innovation from landing target measurement (m/sec) and innovation variance ((m/sec)**2)
+float32    aux_vvel	# vertical auxiliar velocity innovation from landing target measurement (m/sec) and innovation variance ((m/sec)**2)
+
+# Optical flow
+float32[2] flow		# flow innvoation (rad/sec) and innovation variance ((rad/sec)**2)
+
+# Various
+float32 heading		# heading innovation (rad) and innovation variance (rad**2)
+float32[3] mag_field	# earth magnetic field innovation (Gauss) and innovation variance (Gauss**2)
+float32[2] drag		# drag specific force innovation (m/sec**2) and innovation variance ((m/sec)**2)
+float32 airspeed	# airspeed innovation (m/sec) and innovation variance ((m/sec)**2)
+float32 beta		# synthetic sideslip innovation (rad) and innovation variance (rad**2)
+float32 hagl		# height of ground innovation (m) and innovation variance (m**2)
+
+# The innovation test ratios are scalar values. In case the field is a vector,
+# the test ratio will be put in the first component of the vector.
+
+# TOPICS estimator_innovations estimator_innovation_variances estimator_innovation_test_ratios

msg/estimator_status.msg

@@ -9,6 +9,8 @@ float32[3] vibe			# IMU vibration metrics in the following array locations
 
 float32[24] covariances	# Diagonal Elements of Covariance Matrix
 
+float32[3] output_tracking_error # return a vector containing the output predictor angular, velocity and position tracking error magnitudes (rad), (m/s), (m)
+
 uint16 gps_check_fail_flags     # Bitmask to indicate status of GPS checks - see definition below
 # bits are true when corresponding test has failed
 uint8 GPS_CHECK_FAIL_GPS_FIX = 0		# 0 : insufficient fix type (no 3D solution)

msg/tools/uorb_rtps_message_ids.yaml

@@ -45,8 +45,7 @@ rtps:
   - msg: distance_sensor
     id: 17
     send: true
-  # TODO: replace with 'estimator_innovations' as https://github.com/PX4/Firmware/pull/13127 is rebased over this
-  - msg: ekf2_innovations
+  - msg: estimator_innovations
     id: 18
   - msg: ekf2_timestamps
     id: 19
@@ -336,4 +335,10 @@ rtps:
   - msg: vehicle_visual_odometry_aligned
     id: 169
     alias: vehicle_odometry
+  - msg: estimator_innovation_variances
+    id: 170
+    alias: estimator_innovations
+  - msg: estimator_innovation_test_ratios
+    id: 171
+    alias: estimator_innovations
   ########## multi topics: end ##########

src/modules/ekf2/Utility/PreFlightChecker.cpp

@@ -38,7 +38,7 @@
 
 #include "PreFlightChecker.hpp"
 
-void PreFlightChecker::update(const float dt, const ekf2_innovations_s &innov)
+void PreFlightChecker::update(const float dt, const estimator_innovations_s &innov)
 {
 	const float alpha = InnovationLpf::computeAlphaFromDtAndTauInv(dt, _innov_lpf_tau_inv);
 
@@ -48,14 +48,14 @@ void PreFlightChecker::update(const float dt, const ekf2_innovations_s &innov)
 	_has_height_failed = preFlightCheckHeightFailed(innov, alpha);
 }
 
-bool PreFlightChecker::preFlightCheckHeadingFailed(const ekf2_innovations_s &innov, const float alpha)
+bool PreFlightChecker::preFlightCheckHeadingFailed(const estimator_innovations_s &innov, const float alpha)
 {
 	const float heading_test_limit = selectHeadingTestLimit();
 	const float heading_innov_spike_lim = 2.0f * heading_test_limit;
 
-	const float heading_innov_lpf = _filter_heading_innov.update(innov.heading_innov, alpha, heading_innov_spike_lim);
+	const float heading_innov_lpf = _filter_heading_innov.update(innov.heading, alpha, heading_innov_spike_lim);
 
-	return checkInnovFailed(innov.heading_innov, heading_innov_lpf, heading_test_limit);
+	return checkInnovFailed(innov.heading, heading_innov_lpf, heading_test_limit);
 }
 
 float PreFlightChecker::selectHeadingTestLimit()
@@ -69,12 +69,13 @@ float PreFlightChecker::selectHeadingTestLimit()
 	       : _heading_innov_test_lim; // less restrictive test limit
 }
 
-bool PreFlightChecker::preFlightCheckHorizVelFailed(const ekf2_innovations_s &innov, const float alpha)
+bool PreFlightChecker::preFlightCheckHorizVelFailed(const estimator_innovations_s &innov, const float alpha)
 {
 	bool has_failed = false;
 
-	if (_is_using_gps_aiding || _is_using_ev_pos_aiding) {
-		const Vector2f vel_ne_innov = Vector2f(innov.vel_pos_innov);
+	if (_is_using_gps_aiding || _is_using_ev_vel_aiding) {
+		const Vector2f vel_ne_innov = Vector2f(fmaxf(fabsf(innov.gps_hvel[0]), fabsf(innov.ev_hvel[0])),
+						       fmaxf(fabsf(innov.gps_hvel[1]), fabsf(innov.ev_hvel[1])));
 		Vector2f vel_ne_innov_lpf;
 		vel_ne_innov_lpf(0) = _filter_vel_n_innov.update(vel_ne_innov(0), alpha, _vel_innov_spike_lim);
 		vel_ne_innov_lpf(1) = _filter_vel_n_innov.update(vel_ne_innov(1), alpha, _vel_innov_spike_lim);
@@ -82,7 +83,7 @@ bool PreFlightChecker::preFlightCheckHorizVelFailed(const ekf2_innovations_s &in
 	}
 
 	if (_is_using_flow_aiding) {
-		const Vector2f flow_innov = Vector2f(innov.flow_innov);
+		const Vector2f flow_innov = Vector2f(innov.flow);
 		Vector2f flow_innov_lpf;
 		flow_innov_lpf(0) = _filter_flow_x_innov.update(flow_innov(0), alpha, _flow_innov_spike_lim);
 		flow_innov_lpf(1) = _filter_flow_x_innov.update(flow_innov(1), alpha, _flow_innov_spike_lim);
@@ -92,16 +93,16 @@ bool PreFlightChecker::preFlightCheckHorizVelFailed(const ekf2_innovations_s &in
 	return has_failed;
 }
 
-bool PreFlightChecker::preFlightCheckVertVelFailed(const ekf2_innovations_s &innov, const float alpha)
+bool PreFlightChecker::preFlightCheckVertVelFailed(const estimator_innovations_s &innov, const float alpha)
 {
-	const float vel_d_innov = innov.vel_pos_innov[2];
+	const float vel_d_innov = fmaxf(fabsf(innov.gps_vvel), fabs(innov.ev_vvel));
 	const float vel_d_innov_lpf = _filter_vel_d_innov.update(vel_d_innov, alpha, _vel_innov_spike_lim);
 	return checkInnovFailed(vel_d_innov, vel_d_innov_lpf, _vel_innov_test_lim);
 }
 
-bool PreFlightChecker::preFlightCheckHeightFailed(const ekf2_innovations_s &innov, const float alpha)
+bool PreFlightChecker::preFlightCheckHeightFailed(const estimator_innovations_s &innov, const float alpha)
 {
-	const float hgt_innov = innov.vel_pos_innov[5];
+	const float hgt_innov = innov.gps_vpos;    // HACK: height innovation independent of sensor is published on gps_vpos
 	const float hgt_innov_lpf = _filter_hgt_innov.update(hgt_innov, alpha, _hgt_innov_spike_lim);
 	return checkInnovFailed(hgt_innov, hgt_innov_lpf, _hgt_innov_test_lim);
 }
@@ -122,6 +123,7 @@ void PreFlightChecker::reset()
 	_is_using_gps_aiding = false;
 	_is_using_flow_aiding = false;
 	_is_using_ev_pos_aiding = false;
+	_is_using_ev_vel_aiding = false;
 	_has_heading_failed = false;
 	_has_horiz_vel_failed = false;
 	_has_vert_vel_failed = false;

src/modules/ekf2/Utility/PreFlightChecker.hpp

@@ -42,7 +42,7 @@
 #pragma once
 
 #include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/ekf2_innovations.h>
+#include <uORB/topics/estimator_innovations.h>
 
 #include <matrix/matrix/math.hpp>
 
@@ -66,7 +66,7 @@ public:
 	 * @param dt the sampling time
 	 * @param innov the ekf2_innovation_s struct containing the current innovations
 	 */
-	void update(float dt, const ekf2_innovations_s &innov);
+	void update(float dt, const estimator_innovations_s &innov);
 
 	/*
 	 * If set to true, the checker will use a less conservative heading innovation check
@@ -76,6 +76,7 @@ public:
 	void setUsingGpsAiding(bool val) { _is_using_gps_aiding = val; }
 	void setUsingFlowAiding(bool val) { _is_using_flow_aiding = val; }
 	void setUsingEvPosAiding(bool val) { _is_using_ev_pos_aiding = val; }
+	void setUsingEvVelAiding(bool val) { _is_using_ev_vel_aiding = val; }
 
 	bool hasHeadingFailed() const { return _has_heading_failed; }
 	bool hasHorizVelFailed() const { return _has_horiz_vel_failed; }
@@ -127,12 +128,12 @@ public:
 	static constexpr float sq(float var) { return var * var; }
 
 private:
-	bool preFlightCheckHeadingFailed(const ekf2_innovations_s &innov, float alpha);
+	bool preFlightCheckHeadingFailed(const estimator_innovations_s &innov, float alpha);
 	float selectHeadingTestLimit();
 
-	bool preFlightCheckHorizVelFailed(const ekf2_innovations_s &innov, float alpha);
-	bool preFlightCheckVertVelFailed(const ekf2_innovations_s &innov, float alpha);
-	bool preFlightCheckHeightFailed(const ekf2_innovations_s &innov, float alpha);
+	bool preFlightCheckHorizVelFailed(const estimator_innovations_s &innov, float alpha);
+	bool preFlightCheckVertVelFailed(const estimator_innovations_s &innov, float alpha);
+	bool preFlightCheckHeightFailed(const estimator_innovations_s &innov, float alpha);
 
 	void resetPreFlightChecks();
 
@@ -145,6 +146,7 @@ private:
 	bool _is_using_gps_aiding{};
 	bool _is_using_flow_aiding{};
 	bool _is_using_ev_pos_aiding{};
+	bool _is_using_ev_vel_aiding{};
 
 	// Low-pass filters for innovation pre-flight checks
 	InnovationLpf _filter_vel_n_innov;	///< Preflight low pass filter N axis velocity innovations (m/sec)

src/modules/ekf2/ekf2_main.cpp

@@ -56,7 +56,7 @@
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/distance_sensor.h>
-#include <uORB/topics/ekf2_innovations.h>
+#include <uORB/topics/estimator_innovations.h>
 #include <uORB/topics/ekf2_timestamps.h>
 #include <uORB/topics/ekf_gps_position.h>
 #include <uORB/topics/estimator_status.h>
@@ -120,7 +120,7 @@ private:
 	PreFlightChecker _preflt_checker;
 	void runPreFlightChecks(float dt, const filter_control_status_u &control_status,
 				const vehicle_status_s &vehicle_status,
-				const ekf2_innovations_s &innov);
+				const estimator_innovations_s &innov);
 	void resetPreFlightChecks();
 
 	template<typename Param>
@@ -267,7 +267,9 @@ private:
 	vehicle_land_detected_s		_vehicle_land_detected{};
 	vehicle_status_s		_vehicle_status{};
 
-	uORB::Publication<ekf2_innovations_s>			_estimator_innovations_pub{ORB_ID(ekf2_innovations)};
+	uORB::Publication<estimator_innovations_s>			_estimator_innovations_pub{ORB_ID(estimator_innovations)};
+	uORB::Publication<estimator_innovations_s>			_estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)};
+	uORB::Publication<estimator_innovations_s>			_estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)};
 	uORB::Publication<ekf2_timestamps_s>			_ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)};
 	uORB::Publication<ekf_gps_drift_s>			_ekf_gps_drift_pub{ORB_ID(ekf_gps_drift)};
 	uORB::Publication<ekf_gps_position_s>			_blended_gps_pub{ORB_ID(ekf_gps_position)};
@@ -1484,16 +1486,17 @@ void Ekf2::Run()
 			_ekf.get_state_delayed(status.states);
 			status.n_states = 24;
 			_ekf.covariances_diagonal().copyTo(status.covariances);
+			_ekf.get_output_tracking_error(&status.output_tracking_error[0]);
 			_ekf.get_gps_check_status(&status.gps_check_fail_flags);
 			// only report enabled GPS check failures (the param indexes are shifted by 1 bit, because they don't include
 			// the GPS Fix bit, which is always checked)
 			status.gps_check_fail_flags &= ((uint16_t)_params->gps_check_mask << 1) | 1;
 			status.control_mode_flags = control_status.value;
 			_ekf.get_filter_fault_status(&status.filter_fault_flags);
-			_ekf.get_innovation_test_status(&status.innovation_check_flags, &status.mag_test_ratio,
-							&status.vel_test_ratio, &status.pos_test_ratio,
-							&status.hgt_test_ratio, &status.tas_test_ratio,
-							&status.hagl_test_ratio, &status.beta_test_ratio);
+			_ekf.get_innovation_test_status(status.innovation_check_flags, status.mag_test_ratio,
+							status.vel_test_ratio, status.pos_test_ratio,
+							status.hgt_test_ratio, status.tas_test_ratio,
+							status.hagl_test_ratio, status.beta_test_ratio);
 
 			status.pos_horiz_accuracy = _vehicle_local_position_pub.get().eph;
 			status.pos_vert_accuracy = _vehicle_local_position_pub.get().epv;
@@ -1602,28 +1605,51 @@ void Ekf2::Run()
 
 			{
 				// publish estimator innovation data
-				ekf2_innovations_s innovations;
+				estimator_innovations_s innovations;
 				innovations.timestamp = now;
-				_ekf.get_vel_pos_innov(&innovations.vel_pos_innov[0]);
-				_ekf.get_aux_vel_innov(&innovations.aux_vel_innov[0]);
-				_ekf.get_mag_innov(&innovations.mag_innov[0]);
-				_ekf.get_heading_innov(&innovations.heading_innov);
-				_ekf.get_airspeed_innov(&innovations.airspeed_innov);
-				_ekf.get_beta_innov(&innovations.beta_innov);
-				_ekf.get_flow_innov(&innovations.flow_innov[0]);
-				_ekf.get_hagl_innov(&innovations.hagl_innov);
-				_ekf.get_drag_innov(&innovations.drag_innov[0]);
-
-				_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);
-				_ekf.get_airspeed_innov_var(&innovations.airspeed_innov_var);
-				_ekf.get_beta_innov_var(&innovations.beta_innov_var);
-				_ekf.get_flow_innov_var(&innovations.flow_innov_var[0]);
-				_ekf.get_hagl_innov_var(&innovations.hagl_innov_var);
-				_ekf.get_drag_innov_var(&innovations.drag_innov_var[0]);
-
-				_ekf.get_output_tracking_error(&innovations.output_tracking_error[0]);
+				_ekf.getGpsVelPosInnov(&innovations.gps_hvel[0], innovations.gps_vvel, &innovations.gps_hpos[0],
+						       innovations.gps_vpos);
+				_ekf.getEvVelPosInnov(&innovations.ev_hvel[0], innovations.ev_vvel, &innovations.ev_hpos[0], innovations.ev_vpos);
+				_ekf.getAuxVelInnov(&innovations.aux_hvel[0]);
+				_ekf.getFlowInnov(&innovations.flow[0]);
+				_ekf.getHeadingInnov(innovations.heading);
+				_ekf.getMagInnov(innovations.mag_field);
+				_ekf.getDragInnov(&innovations.drag[0]);
+				_ekf.getAirspeedInnov(innovations.airspeed);
+				_ekf.getBetaInnov(innovations.beta);
+				_ekf.getHaglInnov(innovations.hagl);
+
+				// publish estimator innovation variance data
+				estimator_innovations_s innovation_var;
+				innovation_var.timestamp = now;
+				_ekf.getGpsVelPosInnovVar(&innovation_var.gps_hvel[0], innovation_var.gps_vvel, &innovation_var.gps_hpos[0],
+							  innovation_var.gps_vpos);
+				_ekf.getEvVelPosInnovVar(&innovation_var.ev_hvel[0], innovation_var.ev_vvel, &innovation_var.ev_hpos[0],
+							 innovation_var.ev_vpos);
+				_ekf.getAuxVelInnovVar(&innovation_var.aux_hvel[0]);
+				_ekf.getFlowInnovVar(&innovation_var.flow[0]);
+				_ekf.getHeadingInnovVar(innovation_var.heading);
+				_ekf.getMagInnovVar(&innovation_var.mag_field[0]);
+				_ekf.getDragInnovVar(&innovation_var.drag[0]);
+				_ekf.getAirspeedInnovVar(innovation_var.airspeed);
+				_ekf.getBetaInnovVar(innovation_var.beta);
+				_ekf.getHaglInnovVar(innovation_var.hagl);
+
+				// publish estimator innovation test ratio data
+				estimator_innovations_s test_ratios;
+				test_ratios.timestamp = now;
+				_ekf.getGpsVelPosInnovRatio(test_ratios.gps_hvel[0], test_ratios.gps_vvel, test_ratios.gps_hpos[0],
+							    test_ratios.gps_vpos);
+				_ekf.getEvVelPosInnovRatio(test_ratios.ev_hvel[0], test_ratios.ev_vvel, test_ratios.ev_hpos[0],
+							   test_ratios.ev_vpos);
+				_ekf.getAuxVelInnovRatio(test_ratios.aux_hvel[0]);
+				_ekf.getFlowInnovRatio(test_ratios.flow[0]);
+				_ekf.getHeadingInnovRatio(test_ratios.heading);
+				_ekf.getMagInnovRatio(test_ratios.mag_field[0]);
+				_ekf.getDragInnovRatio(&test_ratios.drag[0]);
+				_ekf.getAirspeedInnovRatio(test_ratios.airspeed);
+				_ekf.getBetaInnovRatio(test_ratios.beta);
+				_ekf.getHaglInnovRatio(test_ratios.hagl);
 
 				// calculate noise filtered velocity innovations which are used for pre-flight checking
 				if (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
@@ -1635,6 +1661,9 @@ void Ekf2::Run()
 				}
 
 				_estimator_innovations_pub.publish(innovations);
+				_estimator_innovation_variances_pub.publish(innovation_var);
+				_estimator_innovation_test_ratios_pub.publish(test_ratios);
+
 			}
 		}
 
@@ -1667,7 +1696,7 @@ void Ekf2::fillGpsMsgWithVehicleGpsPosData(gps_message &msg, const vehicle_gps_p
 void Ekf2::runPreFlightChecks(const float dt,
 			      const filter_control_status_u &control_status,
 			      const vehicle_status_s &vehicle_status,
-			      const ekf2_innovations_s &innov)
+			      const estimator_innovations_s &innov)
 {
 	const bool can_observe_heading_in_flight = (vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING);
 
@@ -1675,6 +1704,7 @@ void Ekf2::runPreFlightChecks(const float dt,
 	_preflt_checker.setUsingGpsAiding(control_status.flags.gps);
 	_preflt_checker.setUsingFlowAiding(control_status.flags.opt_flow);
 	_preflt_checker.setUsingEvPosAiding(control_status.flags.ev_pos);
+	_preflt_checker.setUsingEvVelAiding(control_status.flags.ev_vel);
 
 	_preflt_checker.update(dt, innov);
 }
@@ -1736,10 +1766,10 @@ bool Ekf2::publish_wind_estimate(const hrt_abstime &timestamp)
 		float wind_var[2];
 		_ekf.get_wind_velocity(velNE_wind);
 		_ekf.get_wind_velocity_var(wind_var);
-		_ekf.get_airspeed_innov(&wind_estimate.tas_innov);
-		_ekf.get_airspeed_innov_var(&wind_estimate.tas_innov_var);
-		_ekf.get_beta_innov(&wind_estimate.beta_innov);
-		_ekf.get_beta_innov_var(&wind_estimate.beta_innov_var);
+		_ekf.getAirspeedInnov(wind_estimate.tas_innov);
+		_ekf.getAirspeedInnovVar(wind_estimate.tas_innov_var);
+		_ekf.getBetaInnov(wind_estimate.beta_innov);
+		_ekf.getBetaInnovVar(wind_estimate.beta_innov_var);
 		wind_estimate.timestamp = timestamp;
 		wind_estimate.windspeed_north = velNE_wind[0];
 		wind_estimate.windspeed_east = velNE_wind[1];

src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

@@ -493,6 +493,8 @@ void BlockLocalPositionEstimator::Run()
 		publishEstimatorStatus();
 		_pub_innov.get().timestamp = _timeStamp;
 		_pub_innov.update();
+		_pub_innov_var.get().timestamp = _timeStamp;
+		_pub_innov_var.update();
 
 		if ((_estimatorInitialized & EST_XY) && (_map_ref.init_done || _param_lpe_fake_origin.get())) {
 			publishGlobalPos();

src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp

@@ -34,7 +34,7 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/estimator_status.h>
-#include <uORB/topics/ekf2_innovations.h>
+#include <uORB/topics/estimator_innovations.h>
 
 using namespace matrix;
 using namespace control;
@@ -285,7 +285,8 @@ private:
 	uORB::PublicationData<vehicle_global_position_s> _pub_gpos{ORB_ID(vehicle_global_position)};
 	uORB::PublicationData<vehicle_odometry_s> _pub_odom{ORB_ID(vehicle_odometry)};
 	uORB::PublicationData<estimator_status_s> _pub_est_status{ORB_ID(estimator_status)};
-	uORB::PublicationData<ekf2_innovations_s> _pub_innov{ORB_ID(ekf2_innovations)};
+	uORB::PublicationData<estimator_innovations_s> _pub_innov{ORB_ID(estimator_innovations)};
+	uORB::PublicationData<estimator_innovations_s> _pub_innov_var{ORB_ID(estimator_innovation_variances)};
 
 	// map projection
 	struct map_projection_reference_s _map_ref;

src/modules/local_position_estimator/sensors/flow.cpp

@@ -172,10 +172,10 @@ void BlockLocalPositionEstimator::flowCorrect()
 	Matrix<float, n_y_flow, n_y_flow> S = C * m_P * C.transpose() + R;
 
 	// publish innovations
-	_pub_innov.get().flow_innov[0] = r(0);
-	_pub_innov.get().flow_innov[1] = r(1);
-	_pub_innov.get().flow_innov_var[0] = S(0, 0);
-	_pub_innov.get().flow_innov_var[1] = S(1, 1);
+	_pub_innov.get().flow[0] = r(0);
+	_pub_innov.get().flow[1] = r(1);
+	_pub_innov_var.get().flow[0] = S(0, 0);
+	_pub_innov_var.get().flow[1] = S(1, 1);
 
 	// residual covariance, (inverse)
 	Matrix<float, n_y_flow, n_y_flow> S_I = inv<float, n_y_flow>(S);

src/modules/local_position_estimator/sensors/gps.cpp

@@ -189,10 +189,20 @@ void BlockLocalPositionEstimator::gpsCorrect()
 	Matrix<float, n_y_gps, n_y_gps> S = C * m_P * C.transpose() + R;
 
 	// publish innovations
-	for (size_t i = 0; i < 6; i++) {
-		_pub_innov.get().vel_pos_innov[i] = r(i);
-		_pub_innov.get().vel_pos_innov_var[i] = S(i, i);
-	}
+	_pub_innov.get().gps_hvel[0] = r(3);
+	_pub_innov.get().gps_hvel[1] = r(4);
+	_pub_innov.get().gps_vvel    = r(5);
+	_pub_innov.get().gps_hpos[0] = r(0);
+	_pub_innov.get().gps_hpos[1] = r(1);
+	_pub_innov.get().gps_vpos    = r(2);
+
+	// publish innovation variances
+	_pub_innov_var.get().gps_hvel[0] = S(3, 3);
+	_pub_innov_var.get().gps_hvel[1] = S(4, 4);
+	_pub_innov_var.get().gps_vvel    = S(5, 5);
+	_pub_innov_var.get().gps_hpos[0] = S(0, 0);
+	_pub_innov_var.get().gps_hpos[1] = S(1, 1);
+	_pub_innov_var.get().gps_vpos    = S(2, 2);
 
 	// residual covariance, (inverse)
 	Matrix<float, n_y_gps, n_y_gps> S_I = inv<float, n_y_gps>(S);

src/modules/local_position_estimator/sensors/land.cpp

@@ -61,8 +61,8 @@ void BlockLocalPositionEstimator::landCorrect()
 	// residual
 	Matrix<float, n_y_land, n_y_land> S_I = inv<float, n_y_land>((C * m_P * C.transpose()) + R);
 	Vector<float, n_y_land> r = y - C * _x;
-	_pub_innov.get().hagl_innov = r(Y_land_agl);
-	_pub_innov.get().hagl_innov_var = R(Y_land_agl, Y_land_agl);
+	_pub_innov.get().hagl = r(Y_land_agl);
+	_pub_innov_var.get().hagl = R(Y_land_agl, Y_land_agl);
 
 	// fault detection
 	float beta = (r.transpose() * (S_I * r))(0, 0);

src/modules/local_position_estimator/sensors/lidar.cpp

@@ -92,8 +92,8 @@ void BlockLocalPositionEstimator::lidarCorrect()
 	Matrix<float, n_y_lidar, n_y_lidar> S = C * m_P * C.transpose() + R;
 
 	// publish innovations
-	_pub_innov.get().hagl_innov = r(0);
-	_pub_innov.get().hagl_innov_var = S(0, 0);
+	_pub_innov.get().hagl = r(0);
+	_pub_innov_var.get().hagl = S(0, 0);
 
 	// residual covariance, (inverse)
 	Matrix<float, n_y_lidar, n_y_lidar> S_I = inv<float, n_y_lidar>(S);

src/modules/local_position_estimator/sensors/mocap.cpp

@@ -145,15 +145,20 @@ void BlockLocalPositionEstimator::mocapCorrect()
 	Matrix<float, n_y_mocap, n_y_mocap> S = C * m_P * C.transpose() + R;
 
 	// publish innovations
-	for (size_t i = 0; i < 3; i++) {
-		_pub_innov.get().vel_pos_innov[i] = r(i);
-		_pub_innov.get().vel_pos_innov_var[i] = S(i, i);
-	}
-
-	for (size_t i = 3; i < 6; i++) {
-		_pub_innov.get().vel_pos_innov[i] = 0;
-		_pub_innov.get().vel_pos_innov_var[i] = 1;
-	}
+	_pub_innov.get().ev_hvel[0] = NAN;
+	_pub_innov.get().ev_hvel[1] = NAN;
+	_pub_innov.get().ev_vvel    = NAN;
+	_pub_innov.get().ev_hpos[0] = r(0);
+	_pub_innov.get().ev_hpos[1] = r(1);
+	_pub_innov.get().ev_vpos    = r(2);
+
+	// publish innovation variances
+	_pub_innov_var.get().ev_hvel[0] = NAN;
+	_pub_innov_var.get().ev_hvel[1] = NAN;
+	_pub_innov_var.get().ev_vvel    = NAN;
+	_pub_innov_var.get().ev_hpos[0] = S(0, 0);
+	_pub_innov_var.get().ev_hpos[1] = S(1, 1);
+	_pub_innov_var.get().ev_vpos    = S(2, 2);
 
 	// residual covariance, (inverse)
 	Matrix<float, n_y_mocap, n_y_mocap> S_I = inv<float, n_y_mocap>(S);

src/modules/local_position_estimator/sensors/sonar.cpp

@@ -112,8 +112,8 @@ void BlockLocalPositionEstimator::sonarCorrect()
 	Matrix<float, n_y_sonar, n_y_sonar> S = C * m_P * C.transpose() + R;
 
 	// publish innovations
-	_pub_innov.get().hagl_innov = r(0);
-	_pub_innov.get().hagl_innov_var = S(0, 0);
+	_pub_innov.get().hagl = r(0);
+	_pub_innov_var.get().hagl = S(0, 0);
 
 	// residual covariance, (inverse)
 	Matrix<float, n_y_sonar, n_y_sonar> S_I = inv<float, n_y_sonar>(S);

src/modules/local_position_estimator/sensors/vision.cpp

@@ -162,15 +162,20 @@ void BlockLocalPositionEstimator::visionCorrect()
 	Matrix<float, n_y_vision, n_y_vision> S = C * m_P * C.transpose() + R;
 
 	// publish innovations
-	for (size_t i = 0; i < 3; i++) {
-		_pub_innov.get().vel_pos_innov[i] = r(i, 0);
-		_pub_innov.get().vel_pos_innov_var[i] = S(i, i);
-	}
-
-	for (size_t i = 3; i < 6; i++) {
-		_pub_innov.get().vel_pos_innov[i] = 0;
-		_pub_innov.get().vel_pos_innov_var[i] = 1;
-	}
+	_pub_innov.get().ev_hvel[0] = NAN;
+	_pub_innov.get().ev_hvel[1] = NAN;
+	_pub_innov.get().ev_vvel    = NAN;
+	_pub_innov.get().ev_hpos[0] = r(0, 0);
+	_pub_innov.get().ev_hpos[1] = r(1, 0);
+	_pub_innov.get().ev_vpos    = r(2, 0);
+
+	// publish innovation variances
+	_pub_innov_var.get().ev_hvel[0] = NAN;
+	_pub_innov_var.get().ev_hvel[1] = NAN;
+	_pub_innov_var.get().ev_vvel    = NAN;
+	_pub_innov_var.get().ev_hpos[0] = S(0, 0);
+	_pub_innov_var.get().ev_hpos[1] = S(1, 1);
+	_pub_innov_var.get().ev_vpos    = S(2, 2);
 
 	// residual covariance, (inverse)
 	Matrix<float, n_y_vision, n_y_vision> S_I = inv<float, n_y_vision>(S);

src/modules/logger/logger.cpp

@@ -516,7 +516,9 @@ void Logger::add_default_topics()
 	add_topic("camera_trigger");
 	add_topic("camera_trigger_secondary");
 	add_topic("cpuload");
-	add_topic("ekf2_innovations", 200);
+	add_topic("estimator_innovations", 200);
+	add_topic("estimator_innovation_variances", 200);
+	add_topic("estimator_innovation_test_ratios", 200);
 	add_topic("ekf_gps_drift");
 	add_topic("esc_status", 250);
 	add_topic("estimator_status", 200);