save significant IMU bias changes learned by the EKF

* ekf2: make publishing of learned accel biases more robust
* ekf2: reset accel bias if calibration updated
* msg: add separate accel and gyro calibration counters
* ekf2: use separate accel and gyro calibration counters
* ekf2: rework logic to reset biases when calibration counters increment
* sensors: add saving of learned accel biases
* ekf2: generalized saving accel/gyro/mag in flight sensor calibration
* boards: holybro kakutef7 disable systemcmds/perf and systemcmds/top to save flash

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

src/modules/sensors/vehicle_imu/VehicleIMU.cpp

@@ -146,6 +146,15 @@ void VehicleIMU::Run()
 
 	ParametersUpdate();
 
+	// check vehicle status for changes to armed state
+	if (_vehicle_control_mode_sub.updated()) {
+		vehicle_control_mode_s vehicle_control_mode;
+
+		if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) {
+			_armed = vehicle_control_mode.flag_armed;
+		}
+	}
+
 	if (!_accel_calibration.enabled() || !_gyro_calibration.enabled()) {
 		return;
 	}
@@ -210,15 +219,20 @@ void VehicleIMU::Run()
 
 				_gyro_update_latency_mean.update(Vector2f{time_run_s - _gyro_timestamp_sample_last * 1e-6f, time_run_s - _gyro_timestamp_last * 1e-6f});
 
-				return;
+				break;
 			}
 		}
 
 		// finish if there are no more updates, but didn't publish
 		if (!updated) {
-			return;
+			break;
 		}
 	}
+
+	if (hrt_elapsed_time(&_in_flight_calibration_check_timestamp_last) > 1_s) {
+		SensorCalibrationUpdate();
+		_in_flight_calibration_check_timestamp_last = hrt_absolute_time();
+	}
 }
 
 bool VehicleIMU::UpdateAccel()
@@ -537,7 +551,8 @@ bool VehicleIMU::Publish()
 			delta_angle_corrected.copyTo(imu.delta_angle);
 			delta_velocity_corrected.copyTo(imu.delta_velocity);
 			imu.delta_velocity_clipping = _delta_velocity_clipping;
-			imu.calibration_count = _accel_calibration.calibration_count() + _gyro_calibration.calibration_count();
+			imu.accel_calibration_count = _accel_calibration.calibration_count();
+			imu.gyro_calibration_count = _gyro_calibration.calibration_count();
 			imu.timestamp = hrt_absolute_time();
 			_vehicle_imu_pub.publish(imu);
 
@@ -650,4 +665,178 @@ void VehicleIMU::PrintStatus()
 	_gyro_calibration.PrintStatus();
 }
 
+void VehicleIMU::SensorCalibrationUpdate()
+{
+	// State variance assumed for accelerometer bias storage.
+	// This is a reference variance used to calculate the fraction of learned accelerometer bias that will be used to update the stored value.
+	// Larger values cause a larger fraction of the learned biases to be used.
+	static constexpr float max_var_allowed = 1e-3f;
+	static constexpr float max_var_ratio = 1e2f;
+
+	if (_armed) {
+		for (int i = 0; i < _estimator_sensor_bias_subs.size(); i++) {
+			estimator_sensor_bias_s estimator_sensor_bias;
+
+			if (_estimator_sensor_bias_subs[i].update(&estimator_sensor_bias)) {
+				// find corresponding accel bias
+				if (_accel_calibration.device_id() == estimator_sensor_bias.accel_device_id) {
+					const Vector3f bias{estimator_sensor_bias.accel_bias};
+					const Vector3f bias_variance{estimator_sensor_bias.accel_bias_variance};
+
+					const bool valid = (hrt_elapsed_time(&estimator_sensor_bias.timestamp) < 1_s) &&
+							   (estimator_sensor_bias.accel_device_id != 0) &&
+							   estimator_sensor_bias.accel_bias_stable &&
+							   (bias_variance.max() < max_var_allowed) &&
+							   (bias_variance.max() < max_var_ratio * bias_variance.min());
+
+					if (valid) {
+						_accel_learned_calibration[i].offset = _accel_calibration.BiasCorrectedSensorOffset(bias);
+						_accel_learned_calibration[i].bias_variance = bias_variance;
+						_accel_learned_calibration[i].valid = true;
+						_accel_cal_available = true;
+
+					} else {
+						_accel_learned_calibration[i].valid = false;
+					}
+				}
+
+				// find corresponding gyro calibration
+				if (_gyro_calibration.device_id() == estimator_sensor_bias.gyro_device_id) {
+					const Vector3f bias{estimator_sensor_bias.gyro_bias};
+					const Vector3f bias_variance{estimator_sensor_bias.gyro_bias_variance};
+
+					const bool valid = (hrt_elapsed_time(&estimator_sensor_bias.timestamp) < 1_s) &&
+							   (estimator_sensor_bias.gyro_device_id != 0) &&
+							   estimator_sensor_bias.gyro_bias_valid &&
+							   estimator_sensor_bias.gyro_bias_stable &&
+							   (bias_variance.max() < max_var_allowed) &&
+							   (bias_variance.max() < max_var_ratio * bias_variance.min());
+
+					if (valid) {
+						_gyro_learned_calibration[i].offset = _gyro_calibration.BiasCorrectedSensorOffset(bias);
+						_gyro_learned_calibration[i].bias_variance = bias_variance;
+						_gyro_learned_calibration[i].valid = true;
+						_gyro_cal_available = true;
+
+					} else {
+						_gyro_learned_calibration[i].valid = false;
+					}
+				}
+			}
+		}
+
+	}
+
+
+	// not armed and accel cal available
+	if (!_armed && _accel_cal_available) {
+
+		bool initialised = false;
+		Vector3f bias_variance {};
+		Vector3f bias_estimate {};
+
+		// apply all valid saved offsets
+		for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+			if (_accel_learned_calibration[i].valid) {
+				if (!initialised) {
+					bias_variance = _accel_learned_calibration[i].bias_variance;
+					bias_estimate = _accel_learned_calibration[i].offset;
+					initialised = true;
+
+				} else {
+					for (int axis_index = 0; axis_index < 3; axis_index++) {
+						const float sum_of_variances = _accel_learned_calibration[i].bias_variance(axis_index) + bias_variance(axis_index);
+						const float k1 = bias_variance(axis_index) / sum_of_variances;
+						const float k2 = _accel_learned_calibration[i].bias_variance(axis_index) / sum_of_variances;
+						bias_estimate(axis_index) = k2 * bias_estimate(axis_index) + k1 * _accel_learned_calibration[i].offset(axis_index);
+						bias_variance(axis_index) *= k2;
+					}
+				}
+			}
+		}
+
+		if (initialised && bias_estimate.longerThan(0.05f)) {
+			const Vector3f accel_cal_orig{_accel_calibration.offset()};
+			Vector3f accel_cal_offset{_accel_calibration.offset()};
+
+			for (int axis_index = 0; axis_index < 3; axis_index++) {
+				accel_cal_offset(axis_index) += bias_estimate(axis_index);
+			}
+
+			if (_accel_calibration.set_offset(accel_cal_offset)) {
+
+				PX4_INFO("accel %d (%" PRIu32 ") offset committed: [%.2f %.2f %.2f]->[%.2f %.2f %.2f] (full [%.2f %.2f %.2f])",
+					 _instance, _accel_calibration.device_id(),
+					 (double)accel_cal_orig(0), (double)accel_cal_orig(1), (double)accel_cal_orig(2),
+					 (double)accel_cal_offset(0), (double)accel_cal_offset(1), (double)accel_cal_offset(2),
+					 (double)bias_estimate(0), (double)bias_estimate(1), (double)bias_estimate(2));
+
+				_accel_calibration.ParametersSave();
+			}
+		}
+
+		// reset
+		_accel_cal_available = false;
+
+		for (auto &learned_cal : _accel_learned_calibration) {
+			learned_cal = {};
+		}
+	}
+
+
+	// not armed and gyro cal available
+	if (!_armed && _gyro_cal_available) {
+		bool initialised = false;
+		Vector3f bias_variance {};
+		Vector3f bias_estimate {};
+
+		// apply all valid saved offsets
+		for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+			if (_gyro_learned_calibration[i].valid) {
+				if (!initialised) {
+					bias_variance = _gyro_learned_calibration[i].bias_variance;
+					bias_estimate = _gyro_learned_calibration[i].offset;
+					initialised = true;
+
+				} else {
+					for (int axis_index = 0; axis_index < 3; axis_index++) {
+						const float sum_of_variances = _gyro_learned_calibration[i].bias_variance(axis_index) + bias_variance(axis_index);
+						const float k1 = bias_variance(axis_index) / sum_of_variances;
+						const float k2 = _gyro_learned_calibration[i].bias_variance(axis_index) / sum_of_variances;
+						bias_estimate(axis_index) = k2 * bias_estimate(axis_index) + k1 * _gyro_learned_calibration[i].offset(axis_index);
+						bias_variance(axis_index) *= k2;
+					}
+				}
+			}
+		}
+
+		if (initialised && bias_estimate.longerThan(0.05f)) {
+			const Vector3f gyro_cal_orig{_gyro_calibration.offset()};
+			Vector3f gyro_cal_offset{_gyro_calibration.offset()};
+
+			for (int axis_index = 0; axis_index < 3; axis_index++) {
+				gyro_cal_offset(axis_index) += bias_estimate(axis_index);
+			}
+
+			if (_gyro_calibration.set_offset(gyro_cal_offset)) {
+
+				PX4_INFO("gyro %d (%" PRIu32 ") offset committed: [%.2f %.2f %.2f]->[%.2f %.2f %.2f] (full [%.2f %.2f %.2f])",
+					 _instance, _gyro_calibration.device_id(),
+					 (double)gyro_cal_orig(0), (double)gyro_cal_orig(1), (double)gyro_cal_orig(2),
+					 (double)gyro_cal_offset(0), (double)gyro_cal_offset(1), (double)gyro_cal_offset(2),
+					 (double)bias_estimate(0), (double)bias_estimate(1), (double)bias_estimate(2));
+
+				_gyro_calibration.ParametersSave();
+			}
+		}
+
+		// reset
+		_gyro_cal_available = false;
+
+		for (auto &learned_cal : _gyro_learned_calibration) {
+			learned_cal = {};
+		}
+	}
+}
+
 } // namespace sensors

src/modules/sensors/vehicle_imu/VehicleIMU.hpp

@@ -47,10 +47,13 @@
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
 #include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/estimator_sensor_bias.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_accel.h>
 #include <uORB/topics/sensor_gyro.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_imu.h>
 #include <uORB/topics/vehicle_imu_status.h>
 
@@ -85,14 +88,21 @@ private:
 	void UpdateGyroVibrationMetrics(const matrix::Vector3f &angular_velocity);
 	void UpdateAccelSquaredErrorSum(const matrix::Vector3f &acceleration);
 
+	void SensorCalibrationUpdate();
+
 	uORB::PublicationMulti<vehicle_imu_s> _vehicle_imu_pub{ORB_ID(vehicle_imu)};
 	uORB::PublicationMulti<vehicle_imu_status_s> _vehicle_imu_status_pub{ORB_ID(vehicle_imu_status)};
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
+	// Used to check, save and use learned magnetometer biases
+	uORB::SubscriptionMultiArray<estimator_sensor_bias_s> _estimator_sensor_bias_subs{ORB_ID::estimator_sensor_bias};
+
 	uORB::Subscription _sensor_accel_sub;
 	uORB::SubscriptionCallbackWorkItem _sensor_gyro_sub;
 
+	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
+
 	calibration::Accelerometer _accel_calibration{};
 	calibration::Gyroscope _gyro_calibration{};
 
@@ -105,6 +115,8 @@ private:
 	hrt_abstime _gyro_timestamp_sample_last{0};
 	hrt_abstime _gyro_timestamp_last{0};
 
+	hrt_abstime _in_flight_calibration_check_timestamp_last{0};
+
 	math::WelfordMean<matrix::Vector2f> _accel_interval_mean{};
 	math::WelfordMean<matrix::Vector2f> _gyro_interval_mean{};
 
@@ -147,6 +159,21 @@ private:
 
 	const uint8_t _instance;
 
+	bool _armed{false};
+
+	bool _accel_cal_available{false};
+	bool _gyro_cal_available{false};
+
+	struct InFlightCalibration {
+		matrix::Vector3f offset{};
+		matrix::Vector3f bias_variance{};
+		bool valid{false};
+	};
+
+	InFlightCalibration _accel_learned_calibration[ORB_MULTI_MAX_INSTANCES] {};
+	InFlightCalibration _gyro_learned_calibration[ORB_MULTI_MAX_INSTANCES] {};
+
+
 	perf_counter_t _accel_generation_gap_perf{perf_alloc(PC_COUNT, MODULE_NAME": accel data gap")};
 	perf_counter_t _gyro_generation_gap_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro data gap")};
 

src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp

@@ -160,8 +160,10 @@ void VehicleMagnetometer::MagCalibrationUpdate()
 				const Vector3f bias_variance{estimator_sensor_bias.mag_bias_variance};
 
 				const bool valid = (hrt_elapsed_time(&estimator_sensor_bias.timestamp) < 1_s)
-						   && (estimator_sensor_bias.mag_device_id != 0) && estimator_sensor_bias.mag_bias_valid
-						   && (bias_variance.min() > min_var_allowed) && (bias_variance.max() < max_var_allowed);
+						   && (estimator_sensor_bias.mag_device_id != 0) &&
+						   estimator_sensor_bias.mag_bias_valid &&
+						   estimator_sensor_bias.mag_bias_stable &&
+						   (bias_variance.min() > min_var_allowed) && (bias_variance.max() < max_var_allowed);
 
 				if (valid) {
 					// find corresponding mag calibration