Multi-EKF support (ekf2)

- ekf2 can now run in multi-instance mode (currently up to 9 instances)
    - in multi mode all estimates are published to alternate topics (eg estimator_attitude instead of vehicle_attitude)
 - new ekf2 selector runs in multi-instance mode to monitor and compare all instances, selecting a primary (eg N x estimator_attitude => vehicle_attitude)
 - sensors module accel & gyro inconsistency checks are now relative to the mean of all instances, rather than the current primary (when active ekf2 selector is responsible for choosing primary accel & gyro)
 - existing consumers of estimator_status must check estimator_selector_status to select current primary instance status
 - ekf2 single instance mode is still fully supported and the default

Co-authored-by: Paul Riseborough <gncsolns@gmail.com>

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

msg/sensors_status_imu.msg

@@ -5,13 +5,13 @@ uint64 timestamp # time since system start (microseconds)
 
 uint32 accel_device_id_primary       # current primary accel device id for reference
 
-uint32[3] accel_device_ids
-float32[3] accel_inconsistency_m_s_s # magnitude of acceleration difference between IMU instance and primary in (m/s/s).
-bool[3] accel_healthy
+uint32[4] accel_device_ids
+float32[4] accel_inconsistency_m_s_s # magnitude of acceleration difference between IMU instance and mean in (m/s/s).
+bool[4] accel_healthy
 
 
 uint32 gyro_device_id_primary       # current primary gyro device id for reference
 
-uint32[3] gyro_device_ids
-float32[3] gyro_inconsistency_rad_s # magnitude of angular rate difference between IMU instance and primary in (rad/s).
-bool[3] gyro_healthy
+uint32[4] gyro_device_ids
+float32[4] gyro_inconsistency_rad_s # magnitude of angular rate difference between IMU instance and mean in (rad/s).
+bool[4] gyro_healthy

msg/tools/uorb_rtps_message_ids.yaml

@@ -287,6 +287,8 @@ rtps:
     id: 137
   - msg: estimator_optical_flow_vel
     id: 138
+  - msg: estimator_selector_status
+    id: 139
   ########## multi topics: begin ##########
   - msg: actuator_controls_0
     id: 150

msg/vehicle_attitude.msg

@@ -9,3 +9,4 @@ float32[4] delta_q_reset 	# Amount by which quaternion has changed during last r
 uint8 quat_reset_counter	# Quaternion reset counter
 
 # TOPICS vehicle_attitude vehicle_attitude_groundtruth vehicle_vision_attitude
+# TOPICS estimator_attitude

msg/vehicle_global_position.msg

@@ -26,3 +26,4 @@ bool terrain_alt_valid		# Terrain altitude estimate is valid
 bool dead_reckoning		# True if this position is estimated through dead-reckoning
 
 # TOPICS vehicle_global_position vehicle_global_position_groundtruth
+# TOPICS estimator_global_position

msg/vehicle_local_position.msg

@@ -65,3 +65,4 @@ float32 hagl_min			# minimum height above ground level - set to 0 when limiting
 float32 hagl_max			# maximum height above ground level - set to 0 when limiting not required (meters)
 
 # TOPICS vehicle_local_position vehicle_local_position_groundtruth
+# TOPICS estimator_local_position

msg/vehicle_odometry.msg

@@ -63,3 +63,4 @@ float32 yawspeed		# Angular velocity about Z body axis
 float32[21] velocity_covariance
 
 # TOPICS vehicle_odometry vehicle_mocap_odometry vehicle_visual_odometry vehicle_visual_odometry_aligned
+# TOPICS estimator_odometry estimator_visual_odometry_aligned