diff --git a/src/lib/mathlib/math/filter/NotchFilter.hpp b/src/lib/mathlib/math/filter/NotchFilter.hpp index 770fa27a3a3..bce013f996b 100644 --- a/src/lib/mathlib/math/filter/NotchFilter.hpp +++ b/src/lib/mathlib/math/filter/NotchFilter.hpp @@ -37,6 +37,7 @@ * @brief Implementation of a Notch filter. * * @author Mathieu Bresciani + * @author Samuel Garcin */ #pragma once @@ -66,10 +67,11 @@ public: NotchFilter() = default; ~NotchFilter() = default; + void setParameters(float sample_freq, float notch_freq, float bandwidth); /** - * Add a new raw value to the filter + * Add a new raw value to the filter using the Direct form II * * @return retrieve the filtered result */ @@ -85,6 +87,28 @@ public: return output; } + /** + * Add a new raw value to the filter using the Direct Form I + * + * @return retrieve the filtered result + */ + inline T applyDF1(const T &sample) + { + // Direct Form I implementation + const T output = _b0 * sample + _b1 * _delay_element_1 + _b2 * _delay_element_2 - _a1 * _delay_element_output_1 - _a2 * + _delay_element_output_2; + + // shift inputs + _delay_element_2 = _delay_element_1; + _delay_element_1 = sample; + + // shift outputs + _delay_element_output_2 = _delay_element_output_1; + _delay_element_output_1 = output; + + return output; + } + float getNotchFreq() const { return _notch_freq; } float getBandwidth() const { return _bandwidth; } @@ -99,8 +123,26 @@ public: b[2] = _b2; } + /** + * Bypasses the filter update to directly set different filter coefficients. + * Note: the filtered frequency and quality factor saved on the filter lose their + * physical meaning if you use this method to change the filter's coefficients. + * Used for creating clones of a specific filter. + */ + void setCoefficients(float a[2], float b[3]) + { + _a1 = a[0]; + _a2 = a[1]; + _b0 = b[0]; + _b1 = b[1]; + _b2 = b[2]; + } + + T reset(const T &sample); + void update(float sample_freq, float notch_freq, float bandwidth); + protected: float _notch_freq{}; float _bandwidth{}; @@ -115,17 +157,22 @@ protected: T _delay_element_1; T _delay_element_2; + T _delay_element_output_1; + T _delay_element_output_2; }; +/** + * Initialises the filter by setting its parameters and coefficients. + * If using the direct form I (applyDF1) method, allows to dynamically + * update the filtered frequency, refresh rate and quality factor while + * conserving the filter's history + */ template void NotchFilter::setParameters(float sample_freq, float notch_freq, float bandwidth) { _notch_freq = notch_freq; _bandwidth = bandwidth; - _delay_element_1 = {}; - _delay_element_2 = {}; - if (notch_freq <= 0.f) { // no filtering _b0 = 1.0f; @@ -161,6 +208,8 @@ T NotchFilter::reset(const T &sample) _delay_element_1 = dval; _delay_element_2 = dval; + _delay_element_output_1 = {}; + _delay_element_output_2 = {}; return apply(sample); } diff --git a/src/lib/mathlib/math/filter/NotchFilterArray.hpp b/src/lib/mathlib/math/filter/NotchFilterArray.hpp index 15cb62351b6..ffe86c0d054 100644 --- a/src/lib/mathlib/math/filter/NotchFilterArray.hpp +++ b/src/lib/mathlib/math/filter/NotchFilterArray.hpp @@ -37,6 +37,7 @@ * @brief Notch filter with array input/output * * @author Mathieu Bresciani + * @author Samuel Garcin */ #pragma once @@ -50,6 +51,8 @@ class NotchFilterArray : public NotchFilter { using NotchFilter::_delay_element_1; using NotchFilter::_delay_element_2; + using NotchFilter::_delay_element_output_1; + using NotchFilter::_delay_element_output_2; using NotchFilter::_a1; using NotchFilter::_a2; using NotchFilter::_b0; @@ -62,7 +65,7 @@ public: ~NotchFilterArray() = default; /** - * Add new raw values to the filter + * Add new raw values to the filter using the Direct form II. * * @return retrieve the filtered result */ @@ -83,6 +86,36 @@ public: _delay_element_1 = delay_element_0; } } + + /** + * Add new raw values to the filter using the Direct form I. + * + * @return retrieve the filtered result + */ + inline void applyDF1(T samples[], uint8_t num_samples) + { + for (int n = 0; n < num_samples; n++) { + // Direct Form II implementation + const T output = _b0 * samples[n] + _b1 * _delay_element_1 + _b2 * _delay_element_2 - _a1 * _delay_element_output_1 - + _a2 * _delay_element_output_2; + + // don't allow bad values to propagate via the filter + if (!isFinite(output)) { + output = samples[n]; + } + + // shift inputs + _delay_element_2 = _delay_element_1; + _delay_element_1 = samples[n]; + + // shift outputs + _delay_element_output_2 = _delay_element_output_1; + _delay_element_output_1 = output; + + // writes value to array + samples[n] = output; + } + } }; } // namespace math diff --git a/src/lib/mathlib/math/filter/NotchFilterTest.cpp b/src/lib/mathlib/math/filter/NotchFilterTest.cpp index 6eee56c7ce5..f9516b83f46 100644 --- a/src/lib/mathlib/math/filter/NotchFilterTest.cpp +++ b/src/lib/mathlib/math/filter/NotchFilterTest.cpp @@ -80,6 +80,7 @@ TEST_F(NotchFilterTest, simple) TEST_F(NotchFilterTest, filteringLowSide) { // Send a 25Hz sinusoidal signal into a 50Hz notch filter + _notch_float.reset(0.0f); _notch_float.setParameters(_sample_freq, _notch_freq, _bandwidth); const float signal_freq = 25.f; const float omega = 2.f * M_PI_F * signal_freq; @@ -101,9 +102,35 @@ TEST_F(NotchFilterTest, filteringLowSide) } } +TEST_F(NotchFilterTest, filteringLowSideDF1) +{ + // Send a 25Hz sinusoidal signal into a 50Hz notch filter + _notch_float.reset(0.0f); + _notch_float.setParameters(_sample_freq, _notch_freq, _bandwidth); + const float signal_freq = 25.f; + const float omega = 2.f * M_PI_F * signal_freq; + float phase_delay = 11.4f * M_PI_F / 180.f; // Given by simulation + float t = 0.f; + float dt = 1.f / _sample_freq; + float out = 0.f; + + for (int i = 0; i < 1000; i++) { + float input = sinf(omega * t); + float output_expected = sinf(omega * t - phase_delay); + out = _notch_float.applyDF1(input); + t = i * dt; + + // Let some time for the filter to settle + if (i > 30) { + EXPECT_NEAR(out, output_expected, 0.05f); + } + } +} + TEST_F(NotchFilterTest, filteringHighSide) { // Send a 98 sinusoidal signal into a 50Hz notch filter + _notch_float.reset(0.0f); _notch_float.setParameters(_sample_freq, _notch_freq, _bandwidth); const float signal_freq = 98.4f; const float omega = 2.f * M_PI_F * signal_freq; @@ -125,9 +152,35 @@ TEST_F(NotchFilterTest, filteringHighSide) } } +TEST_F(NotchFilterTest, filteringHighSideDF1) +{ + // Send a 98 sinusoidal signal into a 50Hz notch filter + _notch_float.reset(0.0f); + _notch_float.setParameters(_sample_freq, _notch_freq, _bandwidth); + const float signal_freq = 98.4f; + const float omega = 2.f * M_PI_F * signal_freq; + float phase_delay = 11.4f * M_PI_F / 180.f; // Given by simulation + float t = 0.f; + float dt = 1.f / _sample_freq; + float out = 0.f; + + for (int i = 0; i < 1000; i++) { + float input = sinf(omega * t); + float output_expected = sinf(omega * t + phase_delay); + out = _notch_float.applyDF1(input); + t = i * dt; + + // Let some time for the filter to settle + if (i > 30) { + EXPECT_NEAR(out, output_expected, 0.05f); + } + } +} + TEST_F(NotchFilterTest, filterOnNotch) { // Send a 50 sinusoidal signal into a 50Hz notch filter + _notch_float.reset(0.0f); _notch_float.setParameters(_sample_freq, _notch_freq, _bandwidth); const float signal_freq = 50.0f; const float omega = 2.f * M_PI_F * signal_freq; @@ -147,6 +200,42 @@ TEST_F(NotchFilterTest, filterOnNotch) } } +TEST_F(NotchFilterTest, filterOnNotchDF1) +{ + // Send a 50 sinusoidal signal into a 50Hz notch filter + _notch_float.reset(0.0f); + _notch_float.setParameters(_sample_freq, _notch_freq, _bandwidth); + const float signal_freq = 50.0f; + const float omega = 2.f * M_PI_F * signal_freq; + float t = 0.f; + float dt = 1.f / _sample_freq; + float out = 0.f; + + for (int i = 0; i < 1000; i++) { + float input = sinf(omega * t); + out = _notch_float.applyDF1(input); + t = i * dt; + + // Let some time for the filter to settle + if (i > 50) { + EXPECT_NEAR(out, 0.f, 0.1f); + } + } +} + +TEST_F(NotchFilterTest, updateFilter) +{ + _notch_float.reset(0.0f); + _notch_float.setParameters(_sample_freq, _notch_freq, _bandwidth); + float new_notch_freq = 100.f; + float new_bandwidth = 10.f; + _notch_float.setParameters(_sample_freq, new_notch_freq, new_bandwidth); + + EXPECT_EQ(new_notch_freq, _notch_float.getNotchFreq()); + EXPECT_EQ(new_bandwidth, _notch_float.getBandwidth()); + +} + TEST_F(NotchFilterTest, filterVector3f) { // Send three sinusoidal signals (25, 50 and 98.5Hz) into a 50Hz triple notch filter @@ -176,6 +265,35 @@ TEST_F(NotchFilterTest, filterVector3f) } } +TEST_F(NotchFilterTest, filterVector3fDF1) +{ + // Send three sinusoidal signals (25, 50 and 98.5Hz) into a 50Hz triple notch filter + _notch_vector3f.setParameters(_sample_freq, _notch_freq, _bandwidth); + + const Vector3f signal_freq(25.f, 50.f, 98.4f); + const Vector3f omega = 2.f * M_PI_F * signal_freq; + const Vector3f phase_delay = Vector3f(-11.4f, 0.f, 11.4f) * M_PI_F / 180.f; + + float t = 0.f; + float dt = 1.f / _sample_freq; + Vector3f out{}; + + for (int i = 0; i < 1000; i++) { + const Vector3f input(sinf(omega(0) * t), sinf(omega(1) * t), sinf(omega(2) * t)); + const Vector3f arg = omega * t + phase_delay; + const Vector3f output_expected(sinf(arg(0)), 0.f, sinf(arg(2))); + out = _notch_vector3f.applyDF1(input); + t = i * dt; + + // Let some time for the filter to settle + if (i > 50) { + EXPECT_NEAR(out(0), output_expected(0), 0.1f); + EXPECT_NEAR(out(1), output_expected(1), 0.1f); + EXPECT_NEAR(out(2), output_expected(2), 0.1f); + } + } +} + TEST_F(NotchFilterTest, disabled) { const float zero_notch_freq = 0.f; @@ -211,3 +329,21 @@ TEST_F(NotchFilterTest, disabled) EXPECT_EQ(out, input); } } + +TEST_F(NotchFilterTest, setCoefficients) +{ + + float a_new[2] = {1.f, 2.f}; + float b_new[3] = {1.f, 2.f, 3.f}; + float a[3]; + float b[3]; + + _notch_float.setCoefficients(a_new, b_new); + _notch_float.getCoefficients(a, b); + + for (int i = 0; i < 3; i++) { + if (i >= 1) {EXPECT_EQ(a[i], a_new[i - 1]);} //a0 is not part of set function + + EXPECT_EQ(b[i], b_new[i]); + } +}