sensors/vehicle_imu: use WelfordMean for online mean and variance

msg/vehicle_imu_status.msg

@@ -20,7 +20,8 @@ float32 gyro_coning_vibration   # Level of coning vibration in the IMU delta ang
 
 float32[3] mean_accel           # average accelerometer readings since last publication
 float32[3] mean_gyro            # average gyroscope readings since last publication
-float32[3] var_accel            # average accelerometer variance since last publication
+float32[3] var_accel            # accelerometer variance since last publication
+float32[3] var_gyro             # gyroscope variance since last publication
 
 float32 temperature_accel
 float32 temperature_gyro

src/modules/sensors/vehicle_imu/VehicleIMU.cpp

@@ -319,15 +319,25 @@ bool VehicleIMU::UpdateAccel()
 			_status.accel_error_count = accel.error_count;
 		}
 
-		const Vector3f accel_raw{accel.x, accel.y, accel.z};
+
-		_accel_sum += accel_raw;
+		// temperature average
-		_accel_temperature += accel.temperature;
+		if (_accel_temperature_sum_count == 0) {
-		_accel_sum_count++;
+			_accel_temperature_sum = accel.temperature;
+
+		} else {
+			_accel_temperature_sum += accel.temperature;
+		}
+
+		_accel_temperature_sum_count++;
+
 
 		const float dt = (accel.timestamp_sample - _accel_timestamp_sample_last) * 1e-6f;
 		_accel_timestamp_sample_last = accel.timestamp_sample;
 
+		const Vector3f accel_raw{accel.x, accel.y, accel.z};
+		_raw_accel_mean.update(accel_raw);
 		_accel_integrator.put(accel_raw, dt);
+
 		updated = true;
 
 		if (accel.clip_counter[0] > 0 || accel.clip_counter[1] > 0 || accel.clip_counter[2] > 0) {
@@ -465,15 +475,25 @@ bool VehicleIMU::UpdateGyro()
 			_status.gyro_error_count = gyro.error_count;
 		}
 
-		const Vector3f gyro_raw{gyro.x, gyro.y, gyro.z};
+
-		_gyro_sum += gyro_raw;
+		// temperature average
-		_gyro_temperature += gyro.temperature;
+		if (_gyro_temperature_sum_count == 0) {
-		_gyro_sum_count++;
+			_gyro_temperature_sum = gyro.temperature;
+
+		} else {
+			_gyro_temperature_sum += gyro.temperature;
+		}
+
+		_gyro_temperature_sum_count++;
+
 
 		const float dt = (gyro.timestamp_sample - _gyro_timestamp_sample_last) * 1e-6f;
 		_gyro_timestamp_sample_last = gyro.timestamp_sample;
 
+		const Vector3f gyro_raw{gyro.x, gyro.y, gyro.z};
+		_raw_gyro_mean.update(gyro_raw);
 		_gyro_integrator.put(gyro_raw, dt);
+
 		updated = true;
 	}
 
@@ -505,38 +525,38 @@ bool VehicleIMU::Publish()
 			const float accel_dt_inv = 1.e6f / imu.delta_velocity_dt;
 			const Vector3f acceleration{_accel_calibration.Correct(delta_velocity * accel_dt_inv)};
 			UpdateAccelVibrationMetrics(acceleration);
-			UpdateAccelSquaredErrorSum(acceleration);
 			const Vector3f delta_velocity_corrected{acceleration / accel_dt_inv};
 
 			// vehicle_imu_status
 			//  publish before vehicle_imu so that error counts are available synchronously if needed
-			if ((_accel_sum_count > 0) && (_gyro_sum_count > 0)
+			if (_raw_accel_mean.valid() && _raw_gyro_mean.valid()
 			    && (_publish_status || (hrt_elapsed_time(&_status.timestamp) >= 100_ms))) {
 
 				_status.accel_device_id = _accel_calibration.device_id();
-				_status.gyro_device_id = _gyro_calibration.device_id();
 
-				// mean accel
+				// accel mean and variance
-				const Vector3f accel_mean{_accel_calibration.Correct(_accel_sum / _accel_sum_count)};
+				Vector3f(_accel_calibration.rotation() * _raw_accel_mean.mean()).copyTo(_status.mean_accel);
-				accel_mean.copyTo(_status.mean_accel);
+				Vector3f(_accel_calibration.rotation() * _raw_accel_mean.variance()).copyTo(_status.var_accel);
-				_status.temperature_accel = _accel_temperature / _accel_sum_count;
+				_raw_accel_mean.reset();
-				_accel_sum.zero();
-				_accel_temperature = 0;
 
-				// variance accel
+				// accel temperature
-				const Vector3f variance_accel{_accel_squared_error_sum / _accel_sum_count};
+				_status.temperature_accel = _accel_temperature_sum / _accel_temperature_sum_count;
-				variance_accel.copyTo(_status.var_accel);
+				_accel_temperature_sum = NAN;
-				_accel_squared_error_sum.zero();
+				_accel_temperature_sum_count = 0;
 
-				_accel_sum_count = 0;
 
-				// mean gyro
+				_status.gyro_device_id = _gyro_calibration.device_id();
-				const Vector3f gyro_mean{_gyro_calibration.Correct(_gyro_sum / _gyro_sum_count)};
+
-				gyro_mean.copyTo(_status.mean_gyro);
+				// gyro mean and variance
-				_status.temperature_gyro = _gyro_temperature / _gyro_sum_count;
+				Vector3f(_gyro_calibration.rotation() * _raw_gyro_mean.mean()).copyTo(_status.mean_gyro);
-				_gyro_sum.zero();
+				Vector3f(_gyro_calibration.rotation() * _raw_gyro_mean.variance()).copyTo(_status.var_gyro);
-				_gyro_temperature = 0;
+				_raw_gyro_mean.reset();
-				_gyro_sum_count = 0;
+
+				// gyro temperature
+				_status.temperature_gyro = _gyro_temperature_sum / _gyro_temperature_sum_count;
+				_gyro_temperature_sum = NAN;
+				_gyro_temperature_sum_count = 0;
+
 
 				_status.timestamp = hrt_absolute_time();
 				_vehicle_imu_status_pub.publish(_status);
@@ -617,13 +637,6 @@ void VehicleIMU::UpdateIntegratorConfiguration()
 	}
 }
 
-void VehicleIMU::UpdateAccelSquaredErrorSum(const Vector3f &acceleration)
-{
-	// Compute the squared error using the average from last publication for efficiency purposes
-	const Vector3f error{acceleration - Vector3f(_status.mean_accel)};
-	_accel_squared_error_sum += error.emult(error);
-}
-
 void VehicleIMU::UpdateAccelVibrationMetrics(const Vector3f &acceleration)
 {
 	// Accel high frequency vibe = filtered length of (acceleration - acceleration_prev)

src/modules/sensors/vehicle_imu/VehicleIMU.hpp

@@ -86,7 +86,6 @@ private:
 	void UpdateIntegratorConfiguration();
 	void UpdateAccelVibrationMetrics(const matrix::Vector3f &acceleration);
 	void UpdateGyroVibrationMetrics(const matrix::Vector3f &angular_velocity);
-	void UpdateAccelSquaredErrorSum(const matrix::Vector3f &acceleration);
 
 	void SensorCalibrationUpdate();
 
@@ -117,6 +116,9 @@ private:
 
 	hrt_abstime _in_flight_calibration_check_timestamp_last{0};
 
+	math::WelfordMean<matrix::Vector3f> _raw_accel_mean{};
+	math::WelfordMean<matrix::Vector3f> _raw_gyro_mean{};
+
 	math::WelfordMean<matrix::Vector2f> _accel_interval_mean{};
 	math::WelfordMean<matrix::Vector2f> _gyro_interval_mean{};
 
@@ -131,13 +133,11 @@ private:
 	unsigned _accel_last_generation{0};
 	unsigned _gyro_last_generation{0};
 
-	matrix::Vector3f _accel_squared_error_sum{};
+	float _accel_temperature_sum{NAN};
-	matrix::Vector3f _accel_sum{};
+	float _gyro_temperature_sum{NAN};
-	matrix::Vector3f _gyro_sum{};
+
-	int _accel_sum_count{0};
+	int _accel_temperature_sum_count{0};
-	int _gyro_sum_count{0};
+	int _gyro_temperature_sum_count{0};
-	float _accel_temperature{0};
-	float _gyro_temperature{0};
 
 	matrix::Vector3f _acceleration_prev{};     // acceleration from the previous IMU measurement for vibration metrics
 	matrix::Vector3f _angular_velocity_prev{}; // angular velocity from the previous IMU measurement for vibration metrics