forked from Archive/PX4-Autopilot
sensors/vehicle_imu: use WelfordMean for online mean and variance
This commit is contained in:
parent
a8c3bcca32
commit
e18cf3da3e
|
@ -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_accel # average accelerometer readings since last publication
|
||||||
float32[3] mean_gyro # average gyroscope 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_accel
|
||||||
float32 temperature_gyro
|
float32 temperature_gyro
|
||||||
|
|
|
@ -319,15 +319,25 @@ bool VehicleIMU::UpdateAccel()
|
||||||
_status.accel_error_count = accel.error_count;
|
_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;
|
const float dt = (accel.timestamp_sample - _accel_timestamp_sample_last) * 1e-6f;
|
||||||
_accel_timestamp_sample_last = accel.timestamp_sample;
|
_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);
|
_accel_integrator.put(accel_raw, dt);
|
||||||
|
|
||||||
updated = true;
|
updated = true;
|
||||||
|
|
||||||
if (accel.clip_counter[0] > 0 || accel.clip_counter[1] > 0 || accel.clip_counter[2] > 0) {
|
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;
|
_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;
|
const float dt = (gyro.timestamp_sample - _gyro_timestamp_sample_last) * 1e-6f;
|
||||||
_gyro_timestamp_sample_last = gyro.timestamp_sample;
|
_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);
|
_gyro_integrator.put(gyro_raw, dt);
|
||||||
|
|
||||||
updated = true;
|
updated = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -505,38 +525,38 @@ bool VehicleIMU::Publish()
|
||||||
const float accel_dt_inv = 1.e6f / imu.delta_velocity_dt;
|
const float accel_dt_inv = 1.e6f / imu.delta_velocity_dt;
|
||||||
const Vector3f acceleration{_accel_calibration.Correct(delta_velocity * accel_dt_inv)};
|
const Vector3f acceleration{_accel_calibration.Correct(delta_velocity * accel_dt_inv)};
|
||||||
UpdateAccelVibrationMetrics(acceleration);
|
UpdateAccelVibrationMetrics(acceleration);
|
||||||
UpdateAccelSquaredErrorSum(acceleration);
|
|
||||||
const Vector3f delta_velocity_corrected{acceleration / accel_dt_inv};
|
const Vector3f delta_velocity_corrected{acceleration / accel_dt_inv};
|
||||||
|
|
||||||
// vehicle_imu_status
|
// vehicle_imu_status
|
||||||
// publish before vehicle_imu so that error counts are available synchronously if needed
|
// 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))) {
|
&& (_publish_status || (hrt_elapsed_time(&_status.timestamp) >= 100_ms))) {
|
||||||
|
|
||||||
_status.accel_device_id = _accel_calibration.device_id();
|
_status.accel_device_id = _accel_calibration.device_id();
|
||||||
|
|
||||||
|
// accel mean and variance
|
||||||
|
Vector3f(_accel_calibration.rotation() * _raw_accel_mean.mean()).copyTo(_status.mean_accel);
|
||||||
|
Vector3f(_accel_calibration.rotation() * _raw_accel_mean.variance()).copyTo(_status.var_accel);
|
||||||
|
_raw_accel_mean.reset();
|
||||||
|
|
||||||
|
// accel temperature
|
||||||
|
_status.temperature_accel = _accel_temperature_sum / _accel_temperature_sum_count;
|
||||||
|
_accel_temperature_sum = NAN;
|
||||||
|
_accel_temperature_sum_count = 0;
|
||||||
|
|
||||||
|
|
||||||
_status.gyro_device_id = _gyro_calibration.device_id();
|
_status.gyro_device_id = _gyro_calibration.device_id();
|
||||||
|
|
||||||
// mean accel
|
// gyro mean and variance
|
||||||
const Vector3f accel_mean{_accel_calibration.Correct(_accel_sum / _accel_sum_count)};
|
Vector3f(_gyro_calibration.rotation() * _raw_gyro_mean.mean()).copyTo(_status.mean_gyro);
|
||||||
accel_mean.copyTo(_status.mean_accel);
|
Vector3f(_gyro_calibration.rotation() * _raw_gyro_mean.variance()).copyTo(_status.var_gyro);
|
||||||
_status.temperature_accel = _accel_temperature / _accel_sum_count;
|
_raw_gyro_mean.reset();
|
||||||
_accel_sum.zero();
|
|
||||||
_accel_temperature = 0;
|
|
||||||
|
|
||||||
// variance accel
|
// gyro temperature
|
||||||
const Vector3f variance_accel{_accel_squared_error_sum / _accel_sum_count};
|
_status.temperature_gyro = _gyro_temperature_sum / _gyro_temperature_sum_count;
|
||||||
variance_accel.copyTo(_status.var_accel);
|
_gyro_temperature_sum = NAN;
|
||||||
_accel_squared_error_sum.zero();
|
_gyro_temperature_sum_count = 0;
|
||||||
|
|
||||||
_accel_sum_count = 0;
|
|
||||||
|
|
||||||
// mean gyro
|
|
||||||
const Vector3f gyro_mean{_gyro_calibration.Correct(_gyro_sum / _gyro_sum_count)};
|
|
||||||
gyro_mean.copyTo(_status.mean_gyro);
|
|
||||||
_status.temperature_gyro = _gyro_temperature / _gyro_sum_count;
|
|
||||||
_gyro_sum.zero();
|
|
||||||
_gyro_temperature = 0;
|
|
||||||
_gyro_sum_count = 0;
|
|
||||||
|
|
||||||
_status.timestamp = hrt_absolute_time();
|
_status.timestamp = hrt_absolute_time();
|
||||||
_vehicle_imu_status_pub.publish(_status);
|
_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)
|
void VehicleIMU::UpdateAccelVibrationMetrics(const Vector3f &acceleration)
|
||||||
{
|
{
|
||||||
// Accel high frequency vibe = filtered length of (acceleration - acceleration_prev)
|
// Accel high frequency vibe = filtered length of (acceleration - acceleration_prev)
|
||||||
|
|
|
@ -86,7 +86,6 @@ private:
|
||||||
void UpdateIntegratorConfiguration();
|
void UpdateIntegratorConfiguration();
|
||||||
void UpdateAccelVibrationMetrics(const matrix::Vector3f &acceleration);
|
void UpdateAccelVibrationMetrics(const matrix::Vector3f &acceleration);
|
||||||
void UpdateGyroVibrationMetrics(const matrix::Vector3f &angular_velocity);
|
void UpdateGyroVibrationMetrics(const matrix::Vector3f &angular_velocity);
|
||||||
void UpdateAccelSquaredErrorSum(const matrix::Vector3f &acceleration);
|
|
||||||
|
|
||||||
void SensorCalibrationUpdate();
|
void SensorCalibrationUpdate();
|
||||||
|
|
||||||
|
@ -117,6 +116,9 @@ private:
|
||||||
|
|
||||||
hrt_abstime _in_flight_calibration_check_timestamp_last{0};
|
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> _accel_interval_mean{};
|
||||||
math::WelfordMean<matrix::Vector2f> _gyro_interval_mean{};
|
math::WelfordMean<matrix::Vector2f> _gyro_interval_mean{};
|
||||||
|
|
||||||
|
@ -131,13 +133,11 @@ private:
|
||||||
unsigned _accel_last_generation{0};
|
unsigned _accel_last_generation{0};
|
||||||
unsigned _gyro_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 _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
|
matrix::Vector3f _angular_velocity_prev{}; // angular velocity from the previous IMU measurement for vibration metrics
|
||||||
|
|
Loading…
Reference in New Issue