[ECL] Moving towards multi-sensor-per-same-measurement fusion

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 @@ def get_airtime(self, dataset) -> list:
                                              (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 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,16 +114,16 @@ 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()
         data_plot.close()
 
         # 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
@@ -155,8 +155,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/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
@@ -328,4 +327,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 ##########