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

@@ -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));

src/lib/drivers/gyroscope/PX4Gyroscope.hpp

@@ -40,6 +40,7 @@
 #include <lib/drivers/device/integrator.h>
 #include <lib/mathlib/math/filter/LowPassFilter2pArray.hpp>
 #include <lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp>
+#include <lib/mathlib/math/filter/NotchFilter.hpp>
 #include <px4_platform_common/module_params.h>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/sensor_gyro.h>
@@ -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_gyro_status_s>	_sensor_status_pub;
 
 	math::LowPassFilter2pVector3f _filter{1000, 100};
+	math::NotchFilter<matrix::Vector3f> _notch_filter{};
 
 	hrt_abstime	_control_last_publish{0};
 
@@ -140,6 +143,8 @@ private:
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::IMU_GYRO_CUTOFF>) _param_imu_gyro_cutoff,
+		(ParamFloat<px4::params::IMU_GYRO_NF_FREQ>) _param_imu_gyro_nf_freq,
+		(ParamFloat<px4::params::IMU_GYRO_NF_BW>) _param_imu_gyro_nf_bw,
 		(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_rate_max
 	)
 

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)

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 <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

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 <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

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

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
 *