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>

boards/holybro/kakutef7/default.px4board

@@ -41,8 +41,6 @@ CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
 CONFIG_SYSTEMCMDS_MIXER=y
 CONFIG_SYSTEMCMDS_PARAM=y
-CONFIG_SYSTEMCMDS_PERF=y
 CONFIG_SYSTEMCMDS_PWM=y
 CONFIG_SYSTEMCMDS_REBOOT=y
-CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_VER=y

msg/estimator_sensor_bias.msg

@@ -13,15 +13,18 @@ float32[3] gyro_bias            # gyroscope in-run bias in body frame (rad/s)
 float32 gyro_bias_limit         # magnitude of maximum gyroscope in-run bias in body frame (rad/s)
 float32[3] gyro_bias_variance
 bool gyro_bias_valid
+bool gyro_bias_stable		# true when the gyro bias estimate is stable enough to use for calibration
 
 uint32 accel_device_id          # unique device ID for the sensor that does not change between power cycles
 float32[3] accel_bias           # accelerometer in-run bias in body frame (m/s^2)
 float32 accel_bias_limit        # magnitude of maximum accelerometer in-run bias in body frame (m/s^2)
 float32[3] accel_bias_variance
 bool accel_bias_valid
+bool accel_bias_stable		# true when the accel bias estimate is stable enough to use for calibration
 
 uint32 mag_device_id            # unique device ID for the sensor that does not change between power cycles
 float32[3] mag_bias             # magnetometer in-run bias in body frame (Gauss)
 float32 mag_bias_limit          # magnitude of maximum magnetometer in-run bias in body frame (Gauss)
 float32[3] mag_bias_variance
 bool mag_bias_valid
+bool mag_bias_stable		# true when the mag bias estimate is stable enough to use for calibration

msg/vehicle_imu.msg

@@ -16,4 +16,5 @@ uint8 CLIPPING_Y = 2
 uint8 CLIPPING_Z = 4
 uint8 delta_velocity_clipping   # bitfield indicating if there was any accelerometer clipping (per axis) during the integration time frame
 
-uint8 calibration_count         # Calibration changed counter. Monotonically increases whenever calibration changes.
+uint8 accel_calibration_count  	# Calibration changed counter. Monotonically increases whenever accelermeter calibration changes.
+uint8 gyro_calibration_count   	# Calibration changed counter. Monotonically increases whenever rate gyro calibration changes.

src/lib/sensor_calibration/Accelerometer.hpp

@@ -84,6 +84,12 @@ public:
 		return _rotation * matrix::Vector3f{(data - _thermal_offset - _offset).emult(_scale)};
 	}
 
+	// Compute sensor offset from bias (board frame)
+	matrix::Vector3f BiasCorrectedSensorOffset(const matrix::Vector3f &bias) const
+	{
+		return (_rotation.I() * bias).edivide(_scale) + _thermal_offset + _offset;
+	}
+
 	bool ParametersSave();
 	void ParametersUpdate();
 

src/lib/sensor_calibration/Gyroscope.hpp

@@ -88,6 +88,12 @@ public:
 		return (_rotation.I() * corrected_data) + _thermal_offset + _offset;
 	}
 
+	// Compute sensor offset from bias (board frame)
+	matrix::Vector3f BiasCorrectedSensorOffset(const matrix::Vector3f &bias) const
+	{
+		return (_rotation.I() * bias) + _thermal_offset + _offset;
+	}
+
 	bool ParametersSave();
 	void ParametersUpdate();
 

src/modules/ekf2/EKF2.cpp

@@ -341,20 +341,45 @@ void EKF2::Run()
 		if ((_device_id_accel == 0) || (_device_id_gyro == 0)) {
 			_device_id_accel = imu.accel_device_id;
 			_device_id_gyro = imu.gyro_device_id;
-			_imu_calibration_count = imu.calibration_count;
+			_accel_calibration_count = imu.accel_calibration_count;
+			_gyro_calibration_count = imu.gyro_calibration_count;
 
-		} else if ((imu.calibration_count > _imu_calibration_count)
+		} else {
-			   || (imu.accel_device_id != _device_id_accel)
+			bool reset_actioned = false;
-			   || (imu.gyro_device_id != _device_id_gyro)) {
 
-			PX4_DEBUG("%d - resetting IMU bias", _instance);
+			if ((imu.accel_calibration_count != _accel_calibration_count)
-			_device_id_accel = imu.accel_device_id;
+			    || (imu.accel_device_id != _device_id_accel)) {
-			_device_id_gyro = imu.gyro_device_id;
 
-			_ekf.resetImuBias();
+				PX4_DEBUG("%d - resetting accelerometer bias", _instance);
-			_imu_calibration_count = imu.calibration_count;
+				_device_id_accel = imu.accel_device_id;
 
-			SelectImuStatus();
+				_ekf.resetAccelBias();
+				_accel_calibration_count = imu.accel_calibration_count;
+
+				// reset bias learning
+				_accel_cal = {};
+
+				reset_actioned = true;
+			}
+
+			if ((imu.gyro_calibration_count != _gyro_calibration_count)
+			    || (imu.gyro_device_id != _device_id_gyro)) {
+
+				PX4_DEBUG("%d - resetting rate gyro bias", _instance);
+				_device_id_gyro = imu.gyro_device_id;
+
+				_ekf.resetGyroBias();
+				_gyro_calibration_count = imu.gyro_calibration_count;
+
+				// reset bias learning
+				_gyro_cal = {};
+
+				reset_actioned = true;
+			}
+
+			if (reset_actioned) {
+				SelectImuStatus();
+			}
 		}
 
 	} else {
@@ -385,14 +410,26 @@ void EKF2::Run()
 
 			if (_sensor_selection_sub.copy(&sensor_selection)) {
 				if (_device_id_accel != sensor_selection.accel_device_id) {
-					_ekf.resetAccelBias();
+
 					_device_id_accel = sensor_selection.accel_device_id;
+
+					_ekf.resetAccelBias();
+
+					// reset bias learning
+					_accel_cal = {};
+
 					SelectImuStatus();
 				}
 
 				if (_device_id_gyro != sensor_selection.gyro_device_id) {
-					_ekf.resetGyroBias();
+
 					_device_id_gyro = sensor_selection.gyro_device_id;
+
+					_ekf.resetGyroBias();
+
+					// reset bias learning
+					_gyro_cal = {};
+
 					SelectImuStatus();
 				}
 			}
@@ -452,6 +489,7 @@ void EKF2::Run()
 			vehicle_land_detected_s vehicle_land_detected;
 
 			if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) {
+				const bool was_in_air = _ekf.control_status_flags().in_air;
 				_ekf.set_in_air_status(!vehicle_land_detected.landed);
 
 				if (_armed && (_param_ekf2_gnd_eff_dz.get() > 0.f)) {
@@ -463,6 +501,13 @@ void EKF2::Run()
 				} else {
 					_ekf.set_gnd_effect_flag(false);
 				}
+
+				// reset learned sensor calibrations on takeoff
+				if (_ekf.control_status_flags().in_air && !was_in_air) {
+					_accel_cal = {};
+					_gyro_cal = {};
+					_mag_cal = {};
+				}
 			}
 		}
 
@@ -500,7 +545,6 @@ void EKF2::Run()
 			PublishLocalPosition(now);
 			PublishOdometry(now, imu_sample_new);
 			PublishGlobalPosition(now);
-			PublishSensorBias(now);
 			PublishWindEstimate(now);
 
 			// publish status/logging messages
@@ -515,7 +559,10 @@ void EKF2::Run()
 			PublishInnovationVariances(now);
 			PublishYawEstimatorStatus(now);
 
+			UpdateAccelCalibration(now);
+			UpdateGyroCalibration(now);
 			UpdateMagCalibration(now);
+			PublishSensorBias(now);
 
 		} else {
 			// ekf no update
@@ -1013,7 +1060,7 @@ void EKF2::PublishSensorBias(const hrt_abstime &timestamp)
 
 	const Vector3f gyro_bias{_ekf.getGyroBias()};
 	const Vector3f accel_bias{_ekf.getAccelBias()};
-	const Vector3f mag_bias{_mag_cal_last_bias};
+	const Vector3f mag_bias{_ekf.getMagBias()};
 
 	// only publish on change
 	if ((gyro_bias - _last_gyro_bias_published).longerThan(0.001f)
@@ -1026,8 +1073,8 @@ void EKF2::PublishSensorBias(const hrt_abstime &timestamp)
 			gyro_bias.copyTo(bias.gyro_bias);
 			bias.gyro_bias_limit = math::radians(20.f); // 20 degrees/s see Ekf::constrainStates()
 			_ekf.getGyroBiasVariance().copyTo(bias.gyro_bias_variance);
-			bias.gyro_bias_valid = true;
+			bias.gyro_bias_valid = true;  // TODO
-
+			bias.gyro_bias_stable = _gyro_cal.cal_available;
 			_last_gyro_bias_published = gyro_bias;
 		}
 
@@ -1036,8 +1083,8 @@ void EKF2::PublishSensorBias(const hrt_abstime &timestamp)
 			accel_bias.copyTo(bias.accel_bias);
 			bias.accel_bias_limit = _params->acc_bias_lim;
 			_ekf.getAccelBiasVariance().copyTo(bias.accel_bias_variance);
-			bias.accel_bias_valid = !_ekf.fault_status_flags().bad_acc_bias;
+			bias.accel_bias_valid = true;  // TODO
-
+			bias.accel_bias_stable = _accel_cal.cal_available;
 			_last_accel_bias_published = accel_bias;
 		}
 
@@ -1045,9 +1092,9 @@ void EKF2::PublishSensorBias(const hrt_abstime &timestamp)
 			bias.mag_device_id = _device_id_mag;
 			mag_bias.copyTo(bias.mag_bias);
 			bias.mag_bias_limit = 0.5f; // 0.5 Gauss see Ekf::constrainStates()
-			_mag_cal_last_bias_variance.copyTo(bias.mag_bias_variance);
+			_ekf.getMagBiasVariance().copyTo(bias.mag_bias_variance);
-			bias.mag_bias_valid = _mag_cal_available;
+			bias.mag_bias_valid = true; // TODO
-
+			bias.mag_bias_stable = _mag_cal.cal_available;
 			_last_mag_bias_published = mag_bias;
 		}
 
@@ -1646,9 +1693,7 @@ void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps)
 			_mag_calibration_count = magnetometer.calibration_count;
 
 			// reset magnetometer bias learning
-			_mag_cal_total_time_us = 0;
+			_mag_cal = {};
-			_mag_cal_last_us = 0;
-			_mag_cal_available = false;
 		}
 
 		_ekf.setMagData(magSample{magnetometer.timestamp_sample, Vector3f{magnetometer.magnetometer_ga}});
@@ -1725,34 +1770,99 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps)
 	}
 }
 
+void EKF2::UpdateAccelCalibration(const hrt_abstime &timestamp)
+{
+	// Check if conditions are OK for learning of accelerometer bias values
+	// the EKF is operating in the correct mode and there are no filter faults
+	if (_ekf.control_status_flags().in_air && (_ekf.fault_status().value == 0)
+	    && !(_param_ekf2_aid_mask.get() & MASK_INHIBIT_ACC_BIAS)) {
+
+		if (_accel_cal.last_us != 0) {
+			_accel_cal.total_time_us += timestamp - _accel_cal.last_us;
+
+			// Start checking accel bias estimates when we have accumulated sufficient calibration time
+			if (_accel_cal.total_time_us > 30_s) {
+				_accel_cal.last_bias = _ekf.getAccelBias();
+				_accel_cal.last_bias_variance = _ekf.getAccelBiasVariance();
+				_accel_cal.cal_available = true;
+			}
+		}
+
+		_accel_cal.last_us = timestamp;
+
+	} else {
+		// conditions are NOT OK for learning accelerometer bias, reset timestamp
+		// but keep the accumulated calibration time
+		_accel_cal.last_us = 0;
+
+		if (_ekf.fault_status().value != 0) {
+			// if a filter fault has occurred, assume previous learning was invalid and do not
+			// count it towards total learning time.
+			_accel_cal.total_time_us = 0;
+		}
+	}
+}
+
+void EKF2::UpdateGyroCalibration(const hrt_abstime &timestamp)
+{
+	// Check if conditions are OK for learning of gyro bias values
+	// the EKF is operating in the correct mode and there are no filter faults
+	if (_ekf.control_status_flags().in_air && (_ekf.fault_status().value == 0)) {
+
+		if (_gyro_cal.last_us != 0) {
+			_gyro_cal.total_time_us += timestamp - _gyro_cal.last_us;
+
+			// Start checking gyro bias estimates when we have accumulated sufficient calibration time
+			if (_gyro_cal.total_time_us > 30_s) {
+				_gyro_cal.last_bias = _ekf.getGyroBias();
+				_gyro_cal.last_bias_variance = _ekf.getGyroBiasVariance();
+				_gyro_cal.cal_available = true;
+			}
+		}
+
+		_gyro_cal.last_us = timestamp;
+
+	} else {
+		// conditions are NOT OK for learning gyro bias, reset timestamp
+		// but keep the accumulated calibration time
+		_gyro_cal.last_us = 0;
+
+		if (_ekf.fault_status().value != 0) {
+			// if a filter fault has occurred, assume previous learning was invalid and do not
+			// count it towards total learning time.
+			_gyro_cal.total_time_us = 0;
+		}
+	}
+}
+
 void EKF2::UpdateMagCalibration(const hrt_abstime &timestamp)
 {
 	// Check if conditions are OK for learning of magnetometer bias values
 	// the EKF is operating in the correct mode and there are no filter faults
 	if (_ekf.control_status_flags().in_air && _ekf.control_status_flags().mag_3D && (_ekf.fault_status().value == 0)) {
 
-		if (_mag_cal_last_us != 0) {
+		if (_mag_cal.last_us != 0) {
-			_mag_cal_total_time_us += timestamp - _mag_cal_last_us;
+			_mag_cal.total_time_us += timestamp - _mag_cal.last_us;
 
 			// Start checking mag bias estimates when we have accumulated sufficient calibration time
-			if (_mag_cal_total_time_us > 30_s) {
+			if (_mag_cal.total_time_us > 30_s) {
-				_mag_cal_last_bias = _ekf.getMagBias();
+				_mag_cal.last_bias = _ekf.getMagBias();
-				_mag_cal_last_bias_variance = _ekf.getMagBiasVariance();
+				_mag_cal.last_bias_variance = _ekf.getMagBiasVariance();
-				_mag_cal_available = true;
+				_mag_cal.cal_available = true;
 			}
 		}
 
-		_mag_cal_last_us = timestamp;
+		_mag_cal.last_us = timestamp;
 
 	} else {
 		// conditions are NOT OK for learning magnetometer bias, reset timestamp
 		// but keep the accumulated calibration time
-		_mag_cal_last_us = 0;
+		_mag_cal.last_us = 0;
 
 		if (_ekf.fault_status().value != 0) {
 			// if a filter fault has occurred, assume previous learning was invalid and do not
 			// count it towards total learning time.
-			_mag_cal_total_time_us = 0;
+			_mag_cal.total_time_us = 0;
 		}
 	}
 

src/modules/ekf2/EKF2.hpp

@@ -166,8 +166,11 @@ private:
 	void UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps);
 	void UpdateImuStatus();
 
+	void UpdateAccelCalibration(const hrt_abstime &timestamp);
+	void UpdateGyroCalibration(const hrt_abstime &timestamp);
 	void UpdateMagCalibration(const hrt_abstime &timestamp);
 
+
 	/*
 	 * Calculate filtered WGS84 height from estimated AMSL height
 	 */
@@ -198,17 +201,22 @@ private:
 	perf_counter_t _msg_missed_odometry_perf{nullptr};
 	perf_counter_t _msg_missed_optical_flow_perf{nullptr};
 
-	// Used to check, save and use learned magnetometer biases
-	hrt_abstime _mag_cal_last_us{0};	///< last time the EKF was operating a mode that estimates magnetomer biases (uSec)
-	hrt_abstime _mag_cal_total_time_us{0};	///< accumulated calibration time since the last save
-
-	Vector3f _mag_cal_last_bias{};	///< last valid XYZ magnetometer bias estimates (Gauss)
-	Vector3f _mag_cal_last_bias_variance{};	///< variances for the last valid magnetometer XYZ bias estimates (Gauss**2)
-	bool _mag_cal_available{false};	///< true when an unsaved valid calibration for the XYZ magnetometer bias is available
-
 	// Used to control saving of mag declination to be used on next startup
 	bool _mag_decl_saved = false;	///< true when the magnetic declination has been saved
 
+	// Used to check, save and use learned accel/gyro/mag biases
+	struct InFlightCalibration {
+		hrt_abstime last_us{0};         ///< last time the EKF was operating a mode that estimates accelerometer biases (uSec)
+		hrt_abstime total_time_us{0};   ///< accumulated calibration time since the last save
+		Vector3f last_bias{};           ///< last valid XYZ accelerometer bias estimates (Gauss)
+		Vector3f last_bias_variance{};  ///< variances for the last valid accelerometer XYZ bias estimates (m/s**2)**2
+		bool cal_available{false};      ///< true when an unsaved valid calibration for the XYZ accelerometer bias is available
+	};
+
+	InFlightCalibration _accel_cal{};
+	InFlightCalibration _gyro_cal{};
+	InFlightCalibration _mag_cal{};
+
 	bool _had_valid_terrain{false};			///< true if at any time there was a valid terrain estimate
 
 	uint64_t _gps_time_usec{0};
@@ -216,7 +224,8 @@ private:
 	uint64_t _gps_alttitude_ellipsoid_previous_timestamp{0}; ///< storage for previous timestamp to compute dt
 	float   _wgs84_hgt_offset = 0;  ///< height offset between AMSL and WGS84
 
-	uint8_t _imu_calibration_count{0};
+	uint8_t _accel_calibration_count{0};
+	uint8_t _gyro_calibration_count{0};
 	uint8_t _mag_calibration_count{0};
 
 	uint32_t _device_id_accel{0};
@@ -227,6 +236,11 @@ private:
 	Vector3f _last_accel_bias_published{};
 	Vector3f _last_gyro_bias_published{};
 	Vector3f _last_mag_bias_published{};
+
+	Vector3f _last_accel_calibration_published{};
+	Vector3f _last_gyro_calibration_published{};
+	Vector3f _last_mag_calibration_published{};
+
 	float _last_baro_bias_published{};
 
 	float _airspeed_scale_factor{1.0f}; ///< scale factor correction applied to airspeed measurements

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
+						   && (estimator_sensor_bias.mag_device_id != 0) &&
-						   && (bias_variance.min() > min_var_allowed) && (bias_variance.max() < max_var_allowed);
+						   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