From 32642b78bf8775a6666ffc6f93622723b7abdc5b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Wed, 3 Jun 2020 11:52:13 +0200 Subject: [PATCH] gyro filtering: apply IMU_GYRO_CUTOFF also to D-term Greatly improves the noise on the d-term on a Kopis 2. Apparently the order of filtering vs differentiating matters. Also disables the DGYRO filter by default, as the D-term now already has a default filter of 30Hz applied. --- .../VehicleAngularVelocity.cpp | 17 ++++++++++------- .../imu_gyro_parameters.c | 6 +++--- 2 files changed, 13 insertions(+), 10 deletions(-) diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index 1b2b176a8e..d2fa7a441f 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -282,19 +282,22 @@ void VehicleAngularVelocity::Run() // correct for in-run bias errors const Vector3f angular_velocity_raw = _corrections.Correct(val) - _bias; - // Differentiate angular velocity (after notch filter) + // Gyro filtering: + // - Apply general notch filter (IMU_GYRO_NF_FREQ) + // - Apply general low-pass filter (IMU_GYRO_CUTOFF) + // - Differentiate & apply specific angular acceleration (D-term) low-pass (IMU_DGYRO_CUTOFF) + const Vector3f angular_velocity_notched{_notch_filter_velocity.apply(angular_velocity_raw)}; - const Vector3f angular_acceleration_raw = (angular_velocity_notched - _angular_velocity_prev) / dt; - _angular_velocity_prev = angular_velocity_notched; + const Vector3f angular_velocity{_lp_filter_velocity.apply(angular_velocity_notched)}; + + const Vector3f angular_acceleration_raw = (angular_velocity - _angular_velocity_prev) / dt; + _angular_velocity_prev = angular_velocity; _angular_acceleration_prev = angular_acceleration_raw; + const Vector3f angular_acceleration{_lp_filter_acceleration.apply(angular_acceleration_raw)}; CheckFilters(); - // Filter: apply low-pass - const Vector3f angular_acceleration{_lp_filter_acceleration.apply(angular_acceleration_raw)}; - const Vector3f angular_velocity{_lp_filter_velocity.apply(angular_velocity_notched)}; - // publish once all new samples are processed sensor_updated = _sensor_sub[_selected_sensor_sub_index].updated(); diff --git a/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c b/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c index c8cf25a989..670c101e8b 100644 --- a/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c +++ b/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c @@ -70,7 +70,7 @@ PARAM_DEFINE_FLOAT(IMU_GYRO_NF_BW, 20.0f); * * The cutoff frequency for the 2nd order butterworth filter on the primary gyro. * This only affects the angular velocity sent to the controllers, not the estimators. -* Doesn't apply to the angular acceleration (D-Term filter), see IMU_DGYRO_CUTOFF. +* It applies also to the angular acceleration (D-Term filter), see IMU_DGYRO_CUTOFF. * * A value of 0 disables the filter. * @@ -109,7 +109,7 @@ PARAM_DEFINE_INT32(IMU_GYRO_RATEMAX, 0); * the time derivative of the measured angular velocity, also known as * the D-term filter in the rate controller. The D-term uses the derivative of * the rate and thus is the most susceptible to noise. Therefore, using -* a D-term filter allows to decrease the driver-level filtering, which +* a D-term filter allows to increase IMU_GYRO_CUTOFF, which * leads to reduced control latency and permits to increase the P gains. * * A value of 0 disables the filter. @@ -120,4 +120,4 @@ PARAM_DEFINE_INT32(IMU_GYRO_RATEMAX, 0); * @reboot_required true * @group Sensors */ -PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 30.0f); +PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 0.0f);