Multi-EKF support

.ci/Jenkinsfile-hardware

@@ -846,14 +846,14 @@ void statusFTDI() {
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dataman status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "df"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dmesg"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ekf2 status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "free"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "gps status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener adc_report"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener battery_status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener cpuload"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener estimator_sensor"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener estimator_selector_status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener estimator_sensor_bias"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener estimator_status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener logger_status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensor_accel"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensor_accel_fifo"'
@@ -901,6 +901,7 @@ void statusFTDI() {
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb top -1 -a"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ver all"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "work_queue status"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ekf2 status"'
   // stop logger
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "logger off"'
 }
@@ -919,12 +920,12 @@ void statusSEGGER() {
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "dataman status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "df -h"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "dmesg"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "ekf2 status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "free"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "gps status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener adc_report"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener battery_status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener cpuload"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener estimator_selector_status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener estimator_sensor_bias"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener estimator_status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener logger_status"'
@@ -974,6 +975,7 @@ void statusSEGGER() {
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "uorb top -1 -a"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "ver all"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "work_queue status"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "ekf2 status"'
   // stop logger
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "logger off"'
 }

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -141,6 +141,12 @@ then
 	# Speedup SITL startup
 	param set EKF2_REQ_GPS_H 0.5
 
+	# Multi-EKF
+	param set EKF2_MULTI_IMU 3
+	param set SENS_IMU_MODE 0
+	param set EKF2_MULTI_MAG 2
+	param set SENS_MAG_MODE 0
+
 	# By default log from boot until first disarm.
 	param set SDLOG_MODE 1
 	# enable default, estimator replay and vision/avoidance logging profiles

ROMFS/px4fmu_common/init.d/rc.airship_apps

@@ -40,7 +40,7 @@ else
 		# EKF2
 		#
 		param set SYS_MC_EST_GROUP 2
-		ekf2 start
+		ekf2 start &
 	fi
 fi
 

ROMFS/px4fmu_common/init.d/rc.fw_apps

@@ -8,7 +8,7 @@
 #
 # Start the attitude and position estimator.
 #
-ekf2 start
+ekf2 start &
 
 #
 # Start attitude controller.

ROMFS/px4fmu_common/init.d/rc.mc_apps

@@ -40,7 +40,7 @@ else
 		# EKF2
 		#
 		param set SYS_MC_EST_GROUP 2
-		ekf2 start
+		ekf2 start &
 	fi
 fi
 

ROMFS/px4fmu_common/init.d/rc.rover_apps

@@ -8,7 +8,7 @@
 #
 # Start the attitude and position estimator.
 #
-ekf2 start
+ekf2 start &
 #attitude_estimator_q start
 #local_position_estimator start
 

ROMFS/px4fmu_common/init.d/rc.uuv_apps

@@ -9,7 +9,7 @@
 #                       Begin Estimator Group Selection                       #
 ###############################################################################
 
-ekf2 start
+ekf2 start &
 
 ###############################################################################
 #                        End Estimator Group Selection                        #

ROMFS/px4fmu_common/init.d/rc.vehicle_setup

@@ -199,5 +199,5 @@ fi
 if [ $VEHICLE_TYPE = none ]
 then
 	echo "No autostart ID found"
-	ekf2 start
+	ekf2 start &
 fi

ROMFS/px4fmu_common/init.d/rc.vtol_apps

@@ -9,7 +9,7 @@
 #                       Begin Estimator group selection                       #
 ###############################################################################
 
-ekf2 start
+ekf2 start &
 
 ###############################################################################
 #                        End Estimator group selection                        #

Tools/ecl_ekf/analyse_logdata_ekf.py

@@ -14,13 +14,14 @@
 from analysis.post_processing import get_estimator_check_flags
 
 def analyse_ekf(
-        ulog: ULog, check_levels: Dict[str, float], red_thresh: float = 1.0,
-        amb_thresh: float = 0.5, min_flight_duration_seconds: float = 5.0,
+        ulog: ULog, check_levels: Dict[str, float], multi_instance: int = 0,
+        red_thresh: float = 1.0, amb_thresh: float = 0.5, min_flight_duration_seconds: float = 5.0,
         in_air_margin_seconds: float = 5.0, pos_checks_when_sensors_not_fused: bool = False) -> \
         Tuple[str, Dict[str, str], Dict[str, float], Dict[str, float]]:
     """
     :param ulog:
     :param check_levels:
+    :param multi_instance:
     :param red_thresh:
     :param amb_thresh:
     :param min_flight_duration_seconds:
@@ -30,22 +31,19 @@ def analyse_ekf(
     """
 
     try:
-        estimator_states = ulog.get_dataset('estimator_states').data
-        print('found estimator_states data')
+        estimator_states = ulog.get_dataset('estimator_states', multi_instance).data
     except:
-        raise PreconditionError('could not find estimator_states data')
+        raise PreconditionError('could not find estimator_states instance', multi_instance)
 
     try:
-        estimator_status = ulog.get_dataset('estimator_status').data
-        print('found estimator_status data')
+        estimator_status = ulog.get_dataset('estimator_status', multi_instance).data
     except:
-        raise PreconditionError('could not find estimator_status data')
+        raise PreconditionError('could not find estimator_status instance', multi_instance)
 
     try:
-        _ = ulog.get_dataset('estimator_innovations').data
-        print('found estimator_innovations data')
+        _ = ulog.get_dataset('estimator_innovations', multi_instance).data
     except:
-        raise PreconditionError('could not find estimator_innovations data')
+        raise PreconditionError('could not find estimator_innovations instance', multi_instance)
 
     try:
         in_air = InAirDetector(
@@ -71,7 +69,7 @@ def analyse_ekf(
 
     metrics = calculate_ecl_ekf_metrics(
         ulog, innov_flags, innov_fail_checks, sensor_checks, in_air, in_air_no_ground_effects,
-        red_thresh=red_thresh, amb_thresh=amb_thresh)
+        multi_instance, red_thresh=red_thresh, amb_thresh=amb_thresh)
 
     check_status, master_status = perform_ecl_ekf_checks(
         metrics, sensor_checks, innov_fail_checks, check_levels)

Tools/ecl_ekf/analysis/metrics.py

@@ -13,7 +13,7 @@
 def calculate_ecl_ekf_metrics(
         ulog: ULog, innov_flags: Dict[str, float], innov_fail_checks: List[str],
         sensor_checks: List[str], in_air: InAirDetector, in_air_no_ground_effects: InAirDetector,
-        red_thresh: float = 1.0, amb_thresh: float = 0.5) -> Tuple[dict, dict, dict, dict]:
+        multi_instance: int = 0, red_thresh: float = 1.0, amb_thresh: float = 0.5) -> Tuple[dict, dict, dict, dict]:
 
     sensor_metrics = calculate_sensor_metrics(
         ulog, sensor_checks, in_air, in_air_no_ground_effects,
@@ -22,9 +22,9 @@ def calculate_ecl_ekf_metrics(
     innov_fail_metrics = calculate_innov_fail_metrics(
         innov_flags, innov_fail_checks, in_air, in_air_no_ground_effects)
 
-    imu_metrics = calculate_imu_metrics(ulog, in_air_no_ground_effects)
+    imu_metrics = calculate_imu_metrics(ulog, multi_instance, in_air_no_ground_effects)
 
-    estimator_status_data = ulog.get_dataset('estimator_status').data
+    estimator_status_data = ulog.get_dataset('estimator_status', multi_instance).data
 
     # Check for internal filter nummerical faults
     ekf_metrics = {'filter_faults_max': np.amax(estimator_status_data['filter_fault_flags'])}
@@ -44,10 +44,10 @@ def calculate_ecl_ekf_metrics(
 
 def calculate_sensor_metrics(
         ulog: ULog, sensor_checks: List[str], in_air: InAirDetector,
-        in_air_no_ground_effects: InAirDetector, red_thresh: float = 1.0,
-        amb_thresh: float = 0.5) -> Dict[str, float]:
+        in_air_no_ground_effects: InAirDetector, multi_instance: int = 0,
+        red_thresh: float = 1.0, amb_thresh: float = 0.5) -> Dict[str, float]:
 
-    estimator_status_data = ulog.get_dataset('estimator_status').data
+    estimator_status_data = ulog.get_dataset('estimator_status', multi_instance).data
 
     sensor_metrics = dict()
 
@@ -131,10 +131,9 @@ def calculate_innov_fail_metrics(
     return innov_fail_metrics
 
 
-def calculate_imu_metrics(
-        ulog: ULog, in_air_no_ground_effects: InAirDetector) -> dict:
+def calculate_imu_metrics(ulog: ULog, multi_instance, in_air_no_ground_effects: InAirDetector) -> dict:
 
-    estimator_status_data = ulog.get_dataset('estimator_status').data
+    estimator_status_data = ulog.get_dataset('estimator_status', multi_instance).data
 
     imu_metrics = dict()
 
@@ -158,7 +157,7 @@ def calculate_imu_metrics(
                 in_air_no_ground_effects, np.mean)
 
     # IMU bias checks
-    estimator_states_data = ulog.get_dataset('estimator_states').data
+    estimator_states_data = ulog.get_dataset('estimator_states', multi_instance).data
 
     imu_metrics['imu_dang_bias_median'] = np.sqrt(np.sum([np.square(calculate_stat_from_signal(
         estimator_states_data, 'estimator_states', signal, in_air_no_ground_effects, np.median))

Tools/ecl_ekf/plotting/pdf_report.py

@@ -17,7 +17,7 @@
 from analysis.detectors import PreconditionError
 import analysis.data_version_handler as dvh
 
-def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
+def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str) -> None:
     """
     creates a pdf report of the ekf analysis.
     :param ulog:
@@ -29,20 +29,18 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
     # save the plots to PDF
 
     try:
-        estimator_status = ulog.get_dataset('estimator_status').data
-        print('found estimator_status data')
+        estimator_status = ulog.get_dataset('estimator_status', multi_instance).data
     except:
-        raise PreconditionError('could not find estimator_status data')
+        raise PreconditionError('could not find estimator_status instance', multi_instance)
 
     try:
-        estimator_states = ulog.get_dataset('estimator_states').data
-        print('found estimator_states data')
+        estimator_states = ulog.get_dataset('estimator_states', multi_instance).data
     except:
-        raise PreconditionError('could not find estimator_states data')
+        raise PreconditionError('could not find estimator_states instance', multi_instance)
 
     try:
-        estimator_innovations = ulog.get_dataset('estimator_innovations').data
-        estimator_innovation_variances = ulog.get_dataset('estimator_innovation_variances').data
+        estimator_innovations = ulog.get_dataset('estimator_innovations', multi_instance).data
+        estimator_innovation_variances = ulog.get_dataset('estimator_innovation_variances', multi_instance).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
@@ -65,7 +63,7 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
             for key in innovation_data:
                 innovation_data[key] = innovation_data[key][0:innovation_data_min_length]
 
-        print('found innovation data (merged estimator_innovations + estimator_innovation_variances)')
+        print('found innovation data (merged estimator_innovations + estimator_innovation_variances) instance', multi_instance)
 
     except:
         raise PreconditionError('could not find innovation data')

Tools/ecl_ekf/process_logdata_ekf.py

@@ -90,6 +90,21 @@ def process_logdata_ekf(
     except:
         raise PreconditionError('could not open {:s}'.format(filename))
 
+    ekf_instances = 1
+
+    try:
+        estimator_selector_status = ulog.get_dataset('estimator_selector_status',).data
+        print('found estimator_selector_status (multi-ekf) data')
+
+        for instances_available in estimator_selector_status['instances_available']:
+            if instances_available > ekf_instances:
+                ekf_instances = instances_available
+
+        print(ekf_instances, 'ekf instances')
+
+    except:
+        print('could not find estimator_selector_status data')
+
     try:
         # get the dictionary of fail and warning test thresholds from a csv file
         with open(check_level_dict_filename, 'r') as file:
@@ -100,30 +115,35 @@ def process_logdata_ekf(
         raise PreconditionError('could not find {:s}'.format(check_level_dict_filename))
 
     in_air_margin = 5.0 if sensor_safety_margins else 0.0
-    # perform the ekf analysis
-    master_status, check_status, metrics, airtime_info = analyse_ekf(
-        ulog, check_levels, red_thresh=1.0, amb_thresh=0.5, min_flight_duration_seconds=5.0,
-        in_air_margin_seconds=in_air_margin)
-
-    test_results = create_results_table(
-        check_table_filename, master_status, check_status, metrics, airtime_info)
-
-    # write metadata to a .csv file
-    with open('{:s}.mdat.csv'.format(filename), "w") as file:
-
-        file.write("name,value,description\n")
-
-        # loop through the test results dictionary and write each entry on a separate row, with data comma separated
-        # save data in alphabetical order
-        key_list = list(test_results.keys())
-        key_list.sort()
-        for key in key_list:
-            file.write(key + "," + str(test_results[key][0]) + "," + test_results[key][1] + "\n")
-    print('Test results written to {:s}.mdat.csv'.format(filename))
-
-    if plot:
-        create_pdf_report(ulog, '{:s}.pdf'.format(filename))
-        print('Plots saved to {:s}.pdf'.format(filename))
+
+    for multi_instance in range(ekf_instances):
+
+        print('\nestimator instance:', multi_instance)
+
+        # perform the ekf analysis
+        master_status, check_status, metrics, airtime_info = analyse_ekf(
+            ulog, check_levels, multi_instance, red_thresh=1.0, amb_thresh=0.5, min_flight_duration_seconds=5.0,
+            in_air_margin_seconds=in_air_margin)
+
+        test_results = create_results_table(
+            check_table_filename, master_status, check_status, metrics, airtime_info)
+
+        # write metadata to a .csv file
+        with open('{:s}-{:d}.mdat.csv'.format(filename, multi_instance), "w") as file:
+
+            file.write("name,value,description\n")
+
+            # loop through the test results dictionary and write each entry on a separate row, with data comma separated
+            # save data in alphabetical order
+            key_list = list(test_results.keys())
+            key_list.sort()
+            for key in key_list:
+                file.write(key + "," + str(test_results[key][0]) + "," + test_results[key][1] + "\n")
+        print('Test results written to {:s}-{:d}.mdat.csv'.format(filename, multi_instance))
+
+        if plot:
+            create_pdf_report(ulog, multi_instance, '{:s}-{:d}.pdf'.format(filename, multi_instance))
+            print('Plots saved to {:s}-{:d}.pdf'.format(filename, multi_instance))
 
     return test_results
 

msg/CMakeLists.txt

@@ -57,6 +57,7 @@ set(msg_files
 	esc_status.msg
 	estimator_innovations.msg
 	estimator_optical_flow_vel.msg
+	estimator_selector_status.msg
 	estimator_sensor_bias.msg
 	estimator_states.msg
 	estimator_status.msg

msg/estimator_selector_status.msg

@@ -0,0 +1,22 @@
+uint64 timestamp		# time since system start (microseconds)
+
+uint8 primary_instance
+
+uint8 instances_available
+
+uint32 instance_changed_count
+uint64 last_instance_change
+
+uint32 accel_device_id
+uint32 baro_device_id
+uint32 gyro_device_id
+uint32 mag_device_id
+
+float32[9] combined_test_ratio
+float32[9] relative_test_ratio
+bool[9] healthy
+
+float32[4] accumulated_gyro_error
+float32[4] accumulated_accel_error
+bool gyro_fault_detected
+bool accel_fault_detected