From 825c11fc8a31cc2c30c1156344a5f9de44a81c5c Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 20 Dec 2023 17:37:12 +0100 Subject: [PATCH] atune: use butterworth 1st order hpf Equations from: R. Allred, Digital Filters for Everyone --- src/lib/system_identification/system_identification.cpp | 4 ++-- src/lib/system_identification/system_identification.hpp | 4 ++-- .../mc_autotune_attitude_control.cpp | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/lib/system_identification/system_identification.cpp b/src/lib/system_identification/system_identification.cpp index cf8437d292..c95bea6bac 100644 --- a/src/lib/system_identification/system_identification.cpp +++ b/src/lib/system_identification/system_identification.cpp @@ -79,8 +79,8 @@ void SystemIdentification::updateFilters(float u, float y) const float u_lpf = _u_lpf.apply(u); const float y_lpf = _y_lpf.apply(y); - _u_hpf = _alpha_hpf * _u_hpf + _alpha_hpf * (u_lpf - _u_prev); - _y_hpf = _alpha_hpf * _y_hpf + _alpha_hpf * (y_lpf - _y_prev); + _u_hpf = u_lpf - _u_prev - (_gamma_hpf - 1.f) * _u_hpf; + _y_hpf = y_lpf - _y_prev - (_gamma_hpf - 1.f) * _y_hpf; _u_prev = u_lpf; _y_prev = y_lpf; diff --git a/src/lib/system_identification/system_identification.hpp b/src/lib/system_identification/system_identification.hpp index a370db80c5..10712ed7e0 100644 --- a/src/lib/system_identification/system_identification.hpp +++ b/src/lib/system_identification/system_identification.hpp @@ -70,7 +70,7 @@ public: _u_lpf.set_cutoff_frequency(sample_freq, cutoff); _y_lpf.set_cutoff_frequency(sample_freq, cutoff); } - void setHpfCutoffFrequency(float sample_freq, float cutoff) { _alpha_hpf = sample_freq / (sample_freq + 2.f * M_PI_F * cutoff); } + void setHpfCutoffFrequency(float sample_freq, float cutoff) { _gamma_hpf = tanf(M_PI_F * cutoff / sample_freq); } void setForgettingFactor(float time_constant, float dt) { _rls.setForgettingFactor(time_constant, dt); } void setFitnessLpfTimeConstant(float time_constant, float dt) @@ -88,7 +88,7 @@ private: math::LowPassFilter2p _y_lpf{400.f, 30.f}; //TODO: replace by HighPassFilter class - float _alpha_hpf{0.f}; + float _gamma_hpf{0.f}; float _u_hpf{0.f}; float _y_hpf{0.f}; diff --git a/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp b/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp index d1f1d52efc..c0fd90a5cc 100644 --- a/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp +++ b/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp @@ -241,7 +241,7 @@ void McAutotuneAttitudeControl::checkFilters() const float filter_rate_hz = 1.f / _filter_dt; _sys_id.setLpfCutoffFrequency(filter_rate_hz, _param_imu_gyro_cutoff.get()); - _sys_id.setHpfCutoffFrequency(filter_rate_hz, .5f); + _sys_id.setHpfCutoffFrequency(filter_rate_hz, .1f); // Set the model sampling time depending on the gyro cutoff frequency // as this is a good indicator of the maximum control loop bandwidth