Notch filter Direct Form I implementation that support dynamic change of frequencies

* New NotchFilter methods ahead of RPMFilter implementation.
 * Added Direct Form I implementation that support dynamic change of frequencies.
 * Added update method to update frequency on an existing filter.
 * Added setCoefficients method to easily and efficiently create clones of a filter.
 * LowSide, HighSide, Onnotch and array tests testing the applyDF1 method.

src/lib/mathlib/math/filter/NotchFilter.hpp

@@ -37,6 +37,7 @@
  * @brief Implementation of a Notch filter.
  *
  * @author Mathieu Bresciani <brescianimathieu@gmail.com>
+ * @author Samuel Garcin <samuel.garcin@wecorpindustries.com>
  */
 
 #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<typename T>
 void NotchFilter<T>::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<T>::reset(const T &sample)
 
 	_delay_element_1 = dval;
 	_delay_element_2 = dval;
+	_delay_element_output_1 = {};
+	_delay_element_output_2 = {};
 
 	return apply(sample);
 }

src/lib/mathlib/math/filter/NotchFilterArray.hpp

@@ -37,6 +37,7 @@
  * @brief Notch filter with array input/output
  *
  * @author Mathieu Bresciani <brescianimathieu@gmail.com>
+ * @author Samuel Garcin <samuel.garcin@wecorpindustries.com>
  */
 
 #pragma once
@@ -50,6 +51,8 @@ class NotchFilterArray : public NotchFilter<T>
 {
 	using NotchFilter<T>::_delay_element_1;
 	using NotchFilter<T>::_delay_element_2;
+	using NotchFilter<T>::_delay_element_output_1;
+	using NotchFilter<T>::_delay_element_output_2;
 	using NotchFilter<T>::_a1;
 	using NotchFilter<T>::_a2;
 	using NotchFilter<T>::_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

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]);
+	}
+}