From a4dd162fb3d33f3ddcec6265615c319d03457851 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 29 Jul 2022 21:24:16 -0400 Subject: [PATCH] sensors/vehicle_angular_velocity: use linear regression to determine angular acceleration (aka slope filter) --- .../VehicleAngularVelocity.cpp | 79 +++++++++++++++---- .../VehicleAngularVelocity.hpp | 6 ++ 2 files changed, 69 insertions(+), 16 deletions(-) diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index 00dc3d906a..2807bcc34b 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -825,7 +825,8 @@ void VehicleAngularVelocity::Run() sensor_gyro_fifo_s sensor_fifo_data; while (_sensor_gyro_fifo_sub.update(&sensor_fifo_data)) { - const float inverse_dt_s = 1e6f / sensor_fifo_data.dt; + const float dt_s = sensor_fifo_data.dt * 1e-6f; + const float inverse_dt_s = 1.f / dt_s; const int N = sensor_fifo_data.samples; static constexpr int FIFO_SIZE_MAX = sizeof(sensor_fifo_data.x) / sizeof(sensor_fifo_data.x[0]); @@ -845,17 +846,65 @@ void VehicleAngularVelocity::Run() // save last filtered sample angular_velocity_uncalibrated(axis) = FilterAngularVelocity(axis, data, N); + + // calculate linear regression intermediate terms + for (int n = 0; n < N; n++) { + const float gyro_rad_s = data[n]; + + const float t = (n + 1) * dt_s; + + _gyro_sum[axis] += gyro_rad_s; + _time_sum[axis] += t; + _gyro_time_sum[axis] += t * gyro_rad_s; // sum xy + _time_squared_sum[axis] += t * t; + } + angular_acceleration_uncalibrated(axis) = FilterAngularAcceleration(axis, inverse_dt_s, data, N); } + _gyro_sum_count += N; + // Publish if (!_sensor_gyro_fifo_sub.updated()) { - if (CalibrateAndPublish(sensor_fifo_data.timestamp_sample, - angular_velocity_uncalibrated, - angular_acceleration_uncalibrated)) { - perf_end(_cycle_perf); - return; + if (sensor_fifo_data.timestamp_sample >= _last_publish + _publish_interval_min_us) { + + // use linear regression to determine slope of angular velocity + if (_gyro_sum_count >= 5 && _gyro_sum_count < 20) { + int n = _gyro_sum_count; + + for (int axis = 0; axis < 3; axis++) { + + float sum_x = _time_sum[axis]; + float sum_y = _gyro_sum[axis]; + float sum_xx = _time_squared_sum[axis]; + float sum_xy = _gyro_time_sum[axis]; + + // slope (derivative of angular velocity) + float m = (n * sum_xy - sum_x * sum_y) / (n * sum_xx - sum_x * sum_x); + //float b = (sum_y - m * sum_x) / n; + + angular_acceleration_uncalibrated(axis) = m; + } + } + + // reset + _gyro_sum_count = 0; + + for (int axis = 0; axis < 3; axis++) { + _gyro_sum[axis] = 0; + _time_sum[axis] = 0; + _gyro_time_sum[axis] = 0; + _time_squared_sum[axis] = 0; + } + + if (CalibrateAndPublish(sensor_fifo_data.timestamp_sample, + angular_velocity_uncalibrated, + angular_acceleration_uncalibrated)) { + + perf_end(_cycle_perf); + return; + } } } } @@ -916,27 +965,25 @@ bool VehicleAngularVelocity::CalibrateAndPublish(const hrt_abstime ×tamp_sa const Vector3f &angular_velocity_uncalibrated, const Vector3f &angular_acceleration_uncalibrated) { + // Angular acceleration: rotate sensor frame to board, scale raw data to SI, apply any additional configured rotation + _angular_acceleration = _calibration.rotation() * angular_acceleration_uncalibrated; + + // Angular velocity: rotate sensor frame to board, scale raw data to SI, apply calibration, and remove in-run estimated bias + _angular_velocity = _calibration.Correct(angular_velocity_uncalibrated) - _bias; + if (timestamp_sample >= _last_publish + _publish_interval_min_us) { - // Publish vehicle_angular_acceleration + // publish vehicle_angular_acceleration vehicle_angular_acceleration_s angular_acceleration; angular_acceleration.timestamp_sample = timestamp_sample; - - // Angular acceleration: rotate sensor frame to board, scale raw data to SI, apply any additional configured rotation - _angular_acceleration = _calibration.rotation() * angular_acceleration_uncalibrated; _angular_acceleration.copyTo(angular_acceleration.xyz); - angular_acceleration.timestamp = hrt_absolute_time(); _vehicle_angular_acceleration_pub.publish(angular_acceleration); - // Publish vehicle_angular_velocity + // publish vehicle_angular_velocity vehicle_angular_velocity_s angular_velocity; angular_velocity.timestamp_sample = timestamp_sample; - - // Angular velocity: rotate sensor frame to board, scale raw data to SI, apply calibration, and remove in-run estimated bias - _angular_velocity = _calibration.Correct(angular_velocity_uncalibrated) - _bias; _angular_velocity.copyTo(angular_velocity.xyz); - angular_velocity.timestamp = hrt_absolute_time(); _vehicle_angular_velocity_pub.publish(angular_velocity); diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp index 27eae95c84..f8af654540 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -173,6 +173,12 @@ private: // angular acceleration filter AlphaFilter _lp_filter_acceleration[3] {}; + float _gyro_sum[3] {}; + float _time_sum[3] {}; + float _gyro_time_sum[3] {}; + float _time_squared_sum[3] {}; + int _gyro_sum_count{0}; + uint32_t _selected_sensor_device_id{0}; bool _reset_filters{true};