From 5adf23a6a8ee6c2e7a809b2fbc9b872adb9f5548 Mon Sep 17 00:00:00 2001 From: Mathieu Bresciani Date: Mon, 6 Jan 2020 22:53:36 +0100 Subject: [PATCH] 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 --- src/lib/drivers/gyroscope/PX4Gyroscope.cpp | 16 +- src/lib/drivers/gyroscope/PX4Gyroscope.hpp | 5 + src/lib/mathlib/CMakeLists.txt | 2 + src/lib/mathlib/math/filter/NotchFilter.hpp | 168 ++++++++++++++ .../mathlib/math/filter/NotchFilterArray.hpp | 88 ++++++++ .../mathlib/math/filter/NotchFilterTest.cpp | 213 ++++++++++++++++++ src/modules/sensors/sensor_params.c | 30 +++ 7 files changed, 519 insertions(+), 3 deletions(-) create mode 100644 src/lib/mathlib/math/filter/NotchFilter.hpp create mode 100644 src/lib/mathlib/math/filter/NotchFilterArray.hpp create mode 100644 src/lib/mathlib/math/filter/NotchFilterTest.cpp diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp index 59ab98178e..c65f232147 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp @@ -54,6 +54,7 @@ PX4Gyroscope::PX4Gyroscope(uint32_t device_id, uint8_t priority, enum Rotation r // set software low pass filter for controllers updateParams(); ConfigureFilter(_param_imu_gyro_cutoff.get()); + ConfigureNotchFilter(_param_imu_gyro_nf_freq.get(), _param_imu_gyro_nf_bw.get()); } PX4Gyroscope::~PX4Gyroscope() @@ -105,6 +106,7 @@ PX4Gyroscope::set_sample_rate(uint16_t rate) _sample_rate = rate; ConfigureFilter(_filter.get_cutoff_freq()); + ConfigureNotchFilter(_notch_filter.getNotchFreq(), _notch_filter.getBandwidth()); } void @@ -137,9 +139,9 @@ PX4Gyroscope::update(hrt_abstime timestamp, float x, float y, float z) // Apply range scale and the calibrating offset/scale const Vector3f val_calibrated{((raw * _scale) - _calibration_offset)}; - // Filtered values - const Vector3f val_filtered{_filter.apply(val_calibrated)}; - + // Filtered values: apply notch and then low-pass + Vector3f val_filtered{_notch_filter.apply(val_calibrated)}; + val_filtered = _filter.apply(val_filtered); // publish control data (filtered) immediately bool publish_control = true; @@ -428,6 +430,12 @@ PX4Gyroscope::ConfigureFilter(float cutoff_freq) _filterArrayZ.set_cutoff_frequency(_sample_rate, cutoff_freq); } +void +PX4Gyroscope::ConfigureNotchFilter(float notch_freq, float bandwidth) +{ + _notch_filter.setParameters(_sample_rate, notch_freq, bandwidth); +} + void PX4Gyroscope::UpdateVibrationMetrics(const Vector3f &delta_angle) { @@ -448,6 +456,8 @@ PX4Gyroscope::print_status() PX4_INFO(GYRO_BASE_DEVICE_PATH " device instance: %d", _class_device_instance); PX4_INFO("sample rate: %d Hz", _sample_rate); PX4_INFO("filter cutoff: %.3f Hz", (double)_filter.get_cutoff_freq()); + PX4_INFO("notch filter freq: %.3f Hz\tbandwidth: %.3f Hz", (double)_notch_filter.getNotchFreq(), + (double)_notch_filter.getBandwidth()); PX4_INFO("calibration offset: %.5f %.5f %.5f", (double)_calibration_offset(0), (double)_calibration_offset(1), (double)_calibration_offset(2)); diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp index e5b2573f24..f50b00a13c 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp @@ -40,6 +40,7 @@ #include #include #include +#include #include #include #include @@ -89,6 +90,7 @@ public: private: void ConfigureFilter(float cutoff_freq); + void ConfigureNotchFilter(float notch_freq, float bandwidth); void ResetIntegrator(); void UpdateVibrationMetrics(const matrix::Vector3f &delta_angle); @@ -98,6 +100,7 @@ private: uORB::PublicationMultiData _sensor_status_pub; math::LowPassFilter2pVector3f _filter{1000, 100}; + math::NotchFilter _notch_filter{}; hrt_abstime _control_last_publish{0}; @@ -140,6 +143,8 @@ private: DEFINE_PARAMETERS( (ParamFloat) _param_imu_gyro_cutoff, + (ParamFloat) _param_imu_gyro_nf_freq, + (ParamFloat) _param_imu_gyro_nf_bw, (ParamInt) _param_imu_gyro_rate_max ) diff --git a/src/lib/mathlib/CMakeLists.txt b/src/lib/mathlib/CMakeLists.txt index 24b39cb71b..d054eedba1 100644 --- a/src/lib/mathlib/CMakeLists.txt +++ b/src/lib/mathlib/CMakeLists.txt @@ -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) diff --git a/src/lib/mathlib/math/filter/NotchFilter.hpp b/src/lib/mathlib/math/filter/NotchFilter.hpp new file mode 100644 index 0000000000..770fa27a3a --- /dev/null +++ b/src/lib/mathlib/math/filter/NotchFilter.hpp @@ -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 + */ + +#pragma once + +#include +#include +#include +#include + +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 +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 +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; + _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 +T NotchFilter::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 diff --git a/src/lib/mathlib/math/filter/NotchFilterArray.hpp b/src/lib/mathlib/math/filter/NotchFilterArray.hpp new file mode 100644 index 0000000000..15cb62351b --- /dev/null +++ b/src/lib/mathlib/math/filter/NotchFilterArray.hpp @@ -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 + */ + +#pragma once + +#include "NotchFilter.hpp" + +namespace math +{ +template +class NotchFilterArray : public NotchFilter +{ + using NotchFilter::_delay_element_1; + using NotchFilter::_delay_element_2; + using NotchFilter::_a1; + using NotchFilter::_a2; + using NotchFilter::_b0; + using NotchFilter::_b1; + using NotchFilter::_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 diff --git a/src/lib/mathlib/math/filter/NotchFilterTest.cpp b/src/lib/mathlib/math/filter/NotchFilterTest.cpp new file mode 100644 index 0000000000..6eee56c7ce --- /dev/null +++ b/src/lib/mathlib/math/filter/NotchFilterTest.cpp @@ -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 +#include + +#include "NotchFilter.hpp" + +using namespace math; +using matrix::Vector3f; + +class NotchFilterTest : public ::testing::Test +{ +public: + NotchFilter _notch_float; + NotchFilter _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); + } +} diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index dbfbcd03da..cb28dc2503 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -213,6 +213,36 @@ PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0.0f); */ PARAM_DEFINE_INT32(SENS_EN_THERMAL, -1); +/** +* Driver level notch frequency for gyro +* +* The center frequency for the 2nd order notch filter on the gyro driver. +* This filter can be enabled to avoid feedback amplification of structural resonances at a specific frequency. +* This only affects the signal sent to the controllers, not the estimators. 0 disables the filter. +* See "IMU_GYRO_NF_BW" to set the bandwidth of the filter. +* +* @min 0 +* @max 1000 +* @unit Hz +* @reboot_required true +* @group Sensors +*/ +PARAM_DEFINE_FLOAT(IMU_GYRO_NF_FREQ, 0.0f); + +/** +* Driver level notch bandwidth for gyro +* +* The frequency width of the stop band for the 2nd order notch filter on the gyro driver. +* See "IMU_GYRO_NF_FREQ" to activate the filter and to set the notch frequency. +* +* @min 0 +* @max 100 +* @unit Hz +* @reboot_required true +* @group Sensors +*/ +PARAM_DEFINE_FLOAT(IMU_GYRO_NF_BW, 20.0f); + /** * Driver level cutoff frequency for gyro *