vehicleIMU: compute and log accel variance

msg/vehicle_imu_status.msg

@@ -20,6 +20,7 @@ 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 temperature_accel
 float32 temperature_gyro

src/modules/sensors/vehicle_imu/VehicleIMU.cpp

@@ -491,6 +491,7 @@ 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
@@ -507,6 +508,12 @@ bool VehicleIMU::Publish()
 				_status.temperature_accel = _accel_temperature / _accel_sum_count;
 				_accel_sum.zero();
 				_accel_temperature = 0;
+
+				// variance accel
+				const Vector3f variance_accel{_accel_squared_error_sum / _accel_sum_count};
+				variance_accel.copyTo(_status.var_accel);
+				_accel_squared_error_sum.zero();
+
 				_accel_sum_count = 0;
 
 				// mean gyro
@@ -595,6 +602,13 @@ 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

@@ -83,6 +83,7 @@ private:
 	void UpdateIntegratorConfiguration();
 	void UpdateAccelVibrationMetrics(const matrix::Vector3f &acceleration);
 	void UpdateGyroVibrationMetrics(const matrix::Vector3f &angular_velocity);
+	void UpdateAccelSquaredErrorSum(const matrix::Vector3f &acceleration);
 
 	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)};
@@ -118,6 +119,7 @@ private:
 	unsigned _accel_last_generation{0};
 	unsigned _gyro_last_generation{0};
 
+	matrix::Vector3f _accel_squared_error_sum{};
 	matrix::Vector3f _accel_sum{};
 	matrix::Vector3f _gyro_sum{};
 	int _accel_sum_count{0};