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>

src/modules/sensors/sensors.cpp

@@ -536,7 +536,8 @@ void Sensors::InitializeVehicleIMU()
 			if (accel.device_id > 0 && gyro.device_id > 0) {
 				// if the sensors module is responsible for voting (SENS_IMU_MODE 1) then run every VehicleIMU in the same WQ
 				//   otherwise each VehicleIMU runs in a corresponding INSx WQ
-				const px4::wq_config_t &wq_config = px4::wq_configurations::nav_and_controllers;
+				const bool multi_mode = (_param_sens_imu_mode.get() == 0);
+				const px4::wq_config_t &wq_config = multi_mode ? px4::ins_instance_to_wq(i) : px4::wq_configurations::INS0;
 
 				VehicleIMU *imu = new VehicleIMU(i, i, i, wq_config);
 

src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp

@@ -65,7 +65,8 @@ bool VehicleAcceleration::Start()
 	}
 
 	if (!SensorSelectionUpdate(true)) {
-		ScheduleDelayed(10_ms);
+		_selected_sensor_sub_index = 0;
+		_sensor_sub.registerCallback();
 	}
 
 	return true;
@@ -137,6 +138,15 @@ void VehicleAcceleration::CheckFilters()
 
 void VehicleAcceleration::SensorBiasUpdate(bool force)
 {
+	// find corresponding estimated sensor bias
+	if (_estimator_selector_status_sub.updated()) {
+		estimator_selector_status_s estimator_selector_status;
+
+		if (_estimator_selector_status_sub.copy(&estimator_selector_status)) {
+			_estimator_sensor_bias_sub.ChangeInstance(estimator_selector_status.primary_instance);
+		}
+	}
+
 	if (_estimator_sensor_bias_sub.updated() || force) {
 		estimator_sensor_bias_s bias;
 

src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp

@@ -44,6 +44,7 @@
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/estimator_selector_status.h>
 #include <uORB/topics/estimator_sensor_bias.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_accel.h>
@@ -56,7 +57,6 @@ namespace sensors
 class VehicleAcceleration : public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
-
 	VehicleAcceleration();
 	~VehicleAcceleration() override;
 
@@ -77,6 +77,7 @@ private:
 
 	uORB::Publication<vehicle_acceleration_s> _vehicle_acceleration_pub{ORB_ID(vehicle_acceleration)};
 
+	uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
 	uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)};
 	uORB::Subscription _params_sub{ORB_ID(parameter_update)};
 

src/modules/sensors/vehicle_air_data/VehicleAirData.cpp

@@ -137,7 +137,7 @@ void VehicleAirData::Run()
 
 		if (!_advertised[uorb_index]) {
 			// use data's timestamp to throttle advertisement checks
-			if (hrt_elapsed_time(&_last_data[uorb_index].timestamp) > 1_s) {
+			if ((_last_data[uorb_index].timestamp == 0) || (hrt_elapsed_time(&_last_data[uorb_index].timestamp) > 1_s)) {
 				if (_sensor_sub[uorb_index].advertised()) {
 					if (uorb_index > 0) {
 						/* the first always exists, but for each further sensor, add a new validator */
@@ -151,12 +151,17 @@ void VehicleAirData::Run()
 					// force temperature correction update
 					SensorCorrectionsUpdate(true);
 
+					if (_selected_sensor_sub_index < 0) {
+						_sensor_sub[uorb_index].registerCallback();
+					}
+
 				} else {
 					_last_data[uorb_index].timestamp = hrt_absolute_time();
 				}
 			}
+		}
 
-		} else {
+		if (_advertised[uorb_index]) {
 			updated[uorb_index] = _sensor_sub[uorb_index].update(&_last_data[uorb_index]);
 
 			if (updated[uorb_index]) {
@@ -204,7 +209,8 @@ void VehicleAirData::Run()
 		_baro_sum_count++;
 
 		if ((_param_sens_baro_rate.get() > 0)
-		    && hrt_elapsed_time(&_last_publication_timestamp) >= (1e6f / _param_sens_baro_rate.get())) {
+		    && ((_last_publication_timestamp == 0)
+			|| (hrt_elapsed_time(&_last_publication_timestamp) >= (1e6f / _param_sens_baro_rate.get())))) {
 
 			const float pressure = _baro_sum / _baro_sum_count;
 			const hrt_abstime timestamp_sample = _baro_timestamp_sum / _baro_sum_count;

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp

@@ -68,7 +68,8 @@ bool VehicleAngularVelocity::Start()
 	}
 
 	if (!SensorSelectionUpdate(true)) {
-		ScheduleDelayed(10_ms);
+		_selected_sensor_sub_index = 0;
+		_sensor_sub.registerCallback();
 	}
 
 	return true;
@@ -157,6 +158,15 @@ void VehicleAngularVelocity::CheckFilters()
 
 void VehicleAngularVelocity::SensorBiasUpdate(bool force)
 {
+	// find corresponding estimated sensor bias
+	if (_estimator_selector_status_sub.updated()) {
+		estimator_selector_status_s estimator_selector_status;
+
+		if (_estimator_selector_status_sub.copy(&estimator_selector_status)) {
+			_estimator_sensor_bias_sub.ChangeInstance(estimator_selector_status.primary_instance);
+		}
+	}
+
 	if (_estimator_sensor_bias_sub.updated() || force) {
 		estimator_sensor_bias_s bias;
 
@@ -321,6 +331,7 @@ void VehicleAngularVelocity::PrintStatus()
 	PX4_INFO("selected sensor: %d (%d), rate: %.1f Hz",
 		 _selected_sensor_device_id, _selected_sensor_sub_index, (double)_update_rate_hz);
 	PX4_INFO("estimated bias: [%.4f %.4f %.4f]", (double)_bias(0), (double)_bias(1), (double)_bias(2));
+
 	_calibration.PrintStatus();
 }
 

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp

@@ -45,6 +45,7 @@
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/estimator_selector_status.h>
 #include <uORB/topics/estimator_sensor_bias.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_gyro.h>
@@ -58,7 +59,6 @@ namespace sensors
 class VehicleAngularVelocity : public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
-
 	VehicleAngularVelocity();
 	~VehicleAngularVelocity() override;
 
@@ -80,6 +80,7 @@ private:
 	uORB::Publication<vehicle_angular_acceleration_s> _vehicle_angular_acceleration_pub{ORB_ID(vehicle_angular_acceleration)};
 	uORB::Publication<vehicle_angular_velocity_s> _vehicle_angular_velocity_pub{ORB_ID(vehicle_angular_velocity)};
 
+	uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
 	uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)};
 	uORB::Subscription _params_sub{ORB_ID(parameter_update)};
 

src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp

@@ -199,7 +199,7 @@ void VehicleMagnetometer::Run()
 
 		if (!_advertised[uorb_index]) {
 			// use data's timestamp to throttle advertisement checks
-			if (hrt_elapsed_time(&_last_data[uorb_index].timestamp) > 1_s) {
+			if ((_last_data[uorb_index].timestamp == 0) || (hrt_elapsed_time(&_last_data[uorb_index].timestamp) > 1_s)) {
 				if (_sensor_sub[uorb_index].advertised()) {
 					if (uorb_index > 0) {
 						/* the first always exists, but for each further sensor, add a new validator */
@@ -217,12 +217,18 @@ void VehicleMagnetometer::Run()
 						}
 					}
 
+					if (_selected_sensor_sub_index < 0) {
+						_sensor_sub[uorb_index].registerCallback();
+					}
+
 				} else {
 					_last_data[uorb_index].timestamp = hrt_absolute_time();
 				}
 			}
 
-		} else {
+		}
+
+		if (_advertised[uorb_index]) {
 			sensor_mag_s report;
 
 			while (_sensor_sub[uorb_index].update(&report)) {
@@ -339,8 +345,8 @@ void VehicleMagnetometer::Run()
 
 void VehicleMagnetometer::Publish(uint8_t instance, bool multi)
 {
-	if ((_param_sens_mag_rate.get() > 0)
-	    && hrt_elapsed_time(&_last_publication_timestamp[instance]) >= (1e6f / _param_sens_mag_rate.get())) {
+	if ((_param_sens_mag_rate.get() > 0) && (_last_publication_timestamp[instance] ||
+			(hrt_elapsed_time(&_last_publication_timestamp[instance]) >= (1e6f / _param_sens_mag_rate.get())))) {
 
 		const Vector3f magnetometer_data = _mag_sum[instance] / _mag_sum_count[instance];
 		const hrt_abstime timestamp_sample = _timestamp_sample_sum[instance] / _mag_sum_count[instance];

src/modules/sensors/voted_sensors_update.cpp

@@ -374,14 +374,19 @@ void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw)
 	status.accel_device_id_primary = _selection.accel_device_id;
 	status.gyro_device_id_primary = _selection.gyro_device_id;
 
+	static_assert(MAX_SENSOR_COUNT == (sizeof(sensors_status_imu_s::accel_inconsistency_m_s_s) / sizeof(
+			sensors_status_imu_s::accel_inconsistency_m_s_s[0])), "check sensors_status_imu accel_inconsistency_m_s_s size");
+	static_assert(MAX_SENSOR_COUNT == (sizeof(sensors_status_imu_s::gyro_inconsistency_rad_s) / sizeof(
+			sensors_status_imu_s::gyro_inconsistency_rad_s[0])), "check sensors_status_imu accel_inconsistency_m_s_s size");
+
 	for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
-		if (_accel_device_id[i] != 0) {
+		if ((_accel_device_id[i] != 0) && (_accel.priority[i] > 0)) {
 			status.accel_device_ids[i] = _accel_device_id[i];
 			status.accel_inconsistency_m_s_s[i] = _accel_diff[i].norm();
 			status.accel_healthy[i] = (_accel.voter.get_sensor_state(i) == DataValidator::ERROR_FLAG_NO_ERROR);
 		}
 
-		if (_gyro_device_id[i] != 0) {
+		if ((_gyro_device_id[i] != 0) && (_gyro.priority[i] > 0)) {
 			status.gyro_device_ids[i] = _gyro_device_id[i];
 			status.gyro_inconsistency_rad_s[i] = _gyro_diff[i].norm();
 			status.gyro_healthy[i] = (_gyro.voter.get_sensor_state(i) == DataValidator::ERROR_FLAG_NO_ERROR);
@@ -403,32 +408,50 @@ void VotedSensorsUpdate::setRelativeTimestamps(sensor_combined_s &raw)
 
 void VotedSensorsUpdate::calcAccelInconsistency()
 {
-	const Vector3f primary_accel{_last_sensor_data[_accel.last_best_vote].accelerometer_m_s2};
+	Vector3f accel_mean{};
+	Vector3f accel_all[MAX_SENSOR_COUNT] {};
+	uint8_t accel_count = 0;
 
-	// Check each sensor against the primary
 	for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) {
-		// check that the sensor we are checking against is not the same as the primary
-		if (_accel.advertised[sensor_index] && (_accel.priority[sensor_index] > 0)
-		    && (_accel_device_id[sensor_index] != _selection.accel_device_id)) {
+		if ((_accel_device_id[sensor_index] != 0) && (_accel.priority[sensor_index] > 0)) {
+			accel_count++;
+			accel_all[sensor_index] = Vector3f{_last_sensor_data[sensor_index].accelerometer_m_s2};
+			accel_mean += accel_all[sensor_index];
+		}
+	}
 
-			const Vector3f current_accel{_last_sensor_data[sensor_index].accelerometer_m_s2};
-			_accel_diff[sensor_index] = 0.95f * _accel_diff[sensor_index] + 0.05f * (primary_accel - current_accel);
+	if (accel_count > 0) {
+		accel_mean /= accel_count;
+
+		for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) {
+			if ((_accel_device_id[sensor_index] != 0) && (_accel.priority[sensor_index] > 0)) {
+				_accel_diff[sensor_index] = 0.95f * _accel_diff[sensor_index] + 0.05f * (accel_all[sensor_index] - accel_mean);
+			}
 		}
 	}
 }
 
 void VotedSensorsUpdate::calcGyroInconsistency()
 {
-	const Vector3f primary_gyro{_last_sensor_data[_gyro.last_best_vote].gyro_rad};
+	Vector3f gyro_mean{};
+	Vector3f gyro_all[MAX_SENSOR_COUNT] {};
+	uint8_t gyro_count = 0;
 
-	// Check each sensor against the primary
 	for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) {
-		// check that the sensor we are checking against is not the same as the primary
-		if (_gyro.advertised[sensor_index] && (_gyro.priority[sensor_index] > 0)
-		    && (_gyro_device_id[sensor_index] != _selection.gyro_device_id)) {
+		if ((_gyro_device_id[sensor_index] != 0) && (_gyro.priority[sensor_index] > 0)) {
+			gyro_count++;
+			gyro_all[sensor_index] = Vector3f{_last_sensor_data[sensor_index].gyro_rad};
+			gyro_mean += gyro_all[sensor_index];
+		}
+	}
 
-			const Vector3f current_gyro{_last_sensor_data[sensor_index].gyro_rad};
-			_gyro_diff[sensor_index] = 0.95f * _gyro_diff[sensor_index] + 0.05f * (primary_gyro - current_gyro);
+	if (gyro_count > 0) {
+		gyro_mean /= gyro_count;
+
+		for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) {
+			if ((_gyro_device_id[sensor_index] != 0) && (_gyro.priority[sensor_index] > 0)) {
+				_gyro_diff[sensor_index] = 0.95f * _gyro_diff[sensor_index] + 0.05f * (gyro_all[sensor_index] - gyro_mean);
+			}
 		}
 	}
 }

src/modules/sensors/voted_sensors_update.h

@@ -142,12 +142,12 @@ private:
 	bool checkFailover(SensorData &sensor, const char *sensor_name);
 
 	/**
-	 * Calculates the magnitude in m/s/s of the largest difference between the primary and any other accel sensor
+	 * Calculates the magnitude in m/s/s of the largest difference between each accelerometer vector and the mean of all vectors
 	 */
 	void calcAccelInconsistency();
 
 	/**
-	 * Calculates the magnitude in rad/s of the largest difference between the primary and any other gyro sensor
+	 * Calculates the magnitude in rad/s of the largest difference between each gyro vector and the mean of all vectors
 	 */
 	void calcGyroInconsistency();