Add gyroscope notch filter - IMU_GYRO_NF_(FREQ|BW)

* NotchFilter: add NotchFilter template and test for float type * NotchFilterTest: add test for Vector3f notch filter * PX4Gyroscope: add notch filter with IMU_GYRO_NF_(FREQ|BW) parameters to set the notch center frequency and bandwidth
2026-10-17 20:53:35 +08:00 · 2020-01-06 16:53:36 -05:00
parent 031680a520
commit 5adf23a6a8
7 changed files with 519 additions and 3 deletions
@@ -37,3 +37,5 @@ px4_add_library(mathlib
 	math/filter/LowPassFilter2p.cpp
 	math/filter/LowPassFilter2pVector3f.cpp
 )
+
+px4_add_unit_gtest(SRC math/filter/NotchFilterTest.cpp)
@@ -0,0 +1,168 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file NotchFilter.hpp
+ *
+ * @brief Implementation of a Notch filter.
+ *
+ * @author Mathieu Bresciani <brescianimathieu@gmail.com>
+ */
+
+#pragma once
+
+#include <px4_platform_common/defines.h>
+#include <cmath>
+#include <float.h>
+#include <matrix/math.hpp>
+
+namespace math
+{
+
+inline bool isFinite(const float &value)
+{
+	return PX4_ISFINITE(value);
+}
+
+inline bool isFinite(const matrix::Vector3f &value)
+{
+	return PX4_ISFINITE(value(0)) && PX4_ISFINITE(value(1)) && PX4_ISFINITE(value(2));
+}
+
+template<typename T>
+class NotchFilter
+{
+public:
+	NotchFilter() = default;
+	~NotchFilter() = default;
+
+	void setParameters(float sample_freq, float notch_freq, float bandwidth);
+
+	/**
+	 * Add a new raw value to the filter
+	 *
+	 * @return retrieve the filtered result
+	 */
+	inline T apply(const T &sample)
+	{
+		// Direct Form II implementation
+		const T delay_element_0{sample - _delay_element_1 *_a1 - _delay_element_2 * _a2};
+		const T output{delay_element_0 *_b0 + _delay_element_1 *_b1 + _delay_element_2 * _b2};
+
+		_delay_element_2 = _delay_element_1;
+		_delay_element_1 = delay_element_0;
+
+		return output;
+	}
+
+	float getNotchFreq() const { return _notch_freq; }
+	float getBandwidth() const { return _bandwidth; }
+
+	// Used in unit test only
+	void getCoefficients(float a[3], float b[3]) const
+	{
+		a[0] = 1.f;
+		a[1] = _a1;
+		a[2] = _a2;
+		b[0] = _b0;
+		b[1] = _b1;
+		b[2] = _b2;
+	}
+
+	T reset(const T &sample);
+
+protected:
+	float _notch_freq{};
+	float _bandwidth{};
+
+	// All the coefficients are normalized by a0, so a0 becomes 1 here
+	float _a1{};
+	float _a2{};
+
+	float _b0{};
+	float _b1{};
+	float _b2{};
+
+	T _delay_element_1;
+	T _delay_element_2;
+};
+
+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;
+		_b1 = 0.0f;
+		_b2 = 0.0f;
+
+		_a1 = 0.0f;
+		_a2 = 0.0f;
+
+		return;
+	}
+
+	const float alpha = tanf(M_PI_F * bandwidth / sample_freq);
+	const float beta = -cosf(2.f * M_PI_F * notch_freq / sample_freq);
+	const float a0_inv = 1.f / (alpha + 1.f);
+
+	_b0 = a0_inv;
+	_b1 = 2.f * beta * a0_inv;
+	_b2 = a0_inv;
+
+	_a1 = _b1;
+	_a2 = (1.f - alpha) * a0_inv;
+}
+
+template<typename T>
+T NotchFilter<T>::reset(const T &sample)
+{
+	T dval = sample;
+
+	if (fabsf(_b0 + _b1 + _b2) > FLT_EPSILON) {
+		dval = dval / (_b0 + _b1 + _b2);
+	}
+
+	_delay_element_1 = dval;
+	_delay_element_2 = dval;
+
+	return apply(sample);
+}
+
+} // namespace math
@@ -0,0 +1,88 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file NotchFilter.hpp
+ *
+ * @brief Notch filter with array input/output
+ *
+ * @author Mathieu Bresciani <brescianimathieu@gmail.com>
+ */
+
+#pragma once
+
+#include "NotchFilter.hpp"
+
+namespace math
+{
+template<typename T>
+class NotchFilterArray : public NotchFilter<T>
+{
+	using NotchFilter<T>::_delay_element_1;
+	using NotchFilter<T>::_delay_element_2;
+	using NotchFilter<T>::_a1;
+	using NotchFilter<T>::_a2;
+	using NotchFilter<T>::_b0;
+	using NotchFilter<T>::_b1;
+	using NotchFilter<T>::_b2;
+
+public:
+
+	NotchFilterArray() = default;
+	~NotchFilterArray() = default;
+
+	/**
+	 * Add new raw values to the filter
+	 *
+	 * @return retrieve the filtered result
+	 */
+	inline void apply(T samples[], uint8_t num_samples)
+	{
+		for (int n = 0; n < num_samples; n++) {
+			// Direct Form II implementation
+			T delay_element_0{samples[n] - _delay_element_1 *_a1 - _delay_element_2 * _a2};
+
+			// don't allow bad values to propagate via the filter
+			if (!isFinite(delay_element_0)) {
+				delay_element_0 = samples[n];
+			}
+
+			samples[n] = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2;
+
+			_delay_element_2 = _delay_element_1;
+			_delay_element_1 = delay_element_0;
+		}
+	}
+};
+
+} // namespace math
@@ -0,0 +1,213 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * Test code for the Notch filter
+ * Run this test only using make tests TESTFILTER=NotchFilter
+ */
+
+#include <gtest/gtest.h>
+#include <matrix/matrix/math.hpp>
+
+#include "NotchFilter.hpp"
+
+using namespace math;
+using matrix::Vector3f;
+
+class NotchFilterTest : public ::testing::Test
+{
+public:
+	NotchFilter<float> _notch_float;
+	NotchFilter<Vector3f> _notch_vector3f;
+	const float _sample_freq = 1000.f;
+	const float _notch_freq = 50.f;
+	const float _bandwidth = 15.f;
+
+	const float _epsilon_near = 1e-6f;
+};
+
+TEST_F(NotchFilterTest, simple)
+{
+	_notch_float.setParameters(_sample_freq, _notch_freq, _bandwidth);
+	EXPECT_EQ(_notch_float.getBandwidth(), _bandwidth);
+	EXPECT_EQ(_notch_float.getNotchFreq(), _notch_freq);
+
+	float a[3];
+	float b[3];
+
+	// a and b coefficients computed from an external python script
+	const float b_expected[3] = {0.95496499f, -1.81645136f, 0.95496499f};
+	const float a_expected[3] = {1.f, -1.81645136f, 0.90992999f};
+
+	_notch_float.getCoefficients(a, b);
+
+	for (int i = 0; i < 3; i++) {
+		EXPECT_NEAR(a[i], a_expected[i], _epsilon_near);
+		EXPECT_NEAR(b[i], b_expected[i], _epsilon_near);
+	}
+}
+
+TEST_F(NotchFilterTest, filteringLowSide)
+{
+	// Send a 25Hz sinusoidal signal into a 50Hz notch filter
+	_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.apply(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.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.apply(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.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.apply(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, filterVector3f)
+{
+	// 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.apply(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;
+	_notch_float.setParameters(_sample_freq, zero_notch_freq, _bandwidth);
+
+	float a[3];
+	float b[3];
+
+	// The filter should just be a static gain of 1
+	const float b_expected[3] = {1.f, 0.f, 0.f};
+	const float a_expected[3] = {1.f, 0.f, 0.f};
+
+	_notch_float.getCoefficients(a, b);
+
+	for (int i = 0; i < 3; i++) {
+		EXPECT_EQ(a[i], a_expected[i]);
+		EXPECT_EQ(b[i], b_expected[i]);
+	}
+
+	// Run the filter with a 1Hz sinusoid
+	const float signal_freq = 1.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 < 10; i++) {
+		float input = sinf(omega * t);
+		out = _notch_float.apply(input);
+		t = i * dt;
+
+		// The filter should not do anything to the input
+		EXPECT_EQ(out, input);
+	}
+}