mathlib: Notchfilter updates

- merge NotchFilterArray into regular NotchFilter (apply vs applyArray)
 - only use direct form 1 to prevent reset confusion
 - safe default field initialization
 - update VehicleAngularVelocity usage

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

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2019 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2019-2021 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
@@ -70,33 +70,16 @@ public:
 
 	void setParameters(float sample_freq, float notch_freq, float bandwidth);
 
-	/**
-	 * Add a new raw value to the filter using the Direct form II
-	 *
-	 * @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;
-	}
-
 	/**
 	 * Add a new raw value to the filter using the Direct Form I
 	 *
 	 * @return retrieve the filtered result
 	 */
-	inline T applyDF1(const T &sample)
+	inline T apply(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;
+		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;
@@ -109,6 +92,14 @@ public:
 		return output;
 	}
 
+	// Filter array of samples in place using the direct form I
+	inline void applyArray(T samples[], int num_samples)
+	{
+		for (int n = 0; n < num_samples; n++) {
+			samples[n] = apply(samples[n]);
+		}
+	}
+
 	float getNotchFreq() const { return _notch_freq; }
 	float getBandwidth() const { return _bandwidth; }
 
@@ -150,53 +141,77 @@ public:
 		_b2 = b[2];
 	}
 
+	void reset(const T &sample)
+	{
+		const T input = isFinite(sample) ? sample : T{};
+
+		_delay_element_1 = _delay_element_2 = input;
+		_delay_element_output_1 = _delay_element_output_2 = input * (_b0 + _b1 + _b2) / (1 + _a1 + _a2);
+
+		if (!isFinite(_delay_element_1) || !isFinite(_delay_element_2)) {
+			_delay_element_output_1 = _delay_element_output_2 = {};
+		}
+	}
+
+	void disable()
+	{
+		// no filtering
+		_notch_freq = 0.f;
+		_bandwidth = 0.f;
+		_sample_freq = 0.f;
+
+		_delay_element_1 = {};
+		_delay_element_2 = {};
+		_delay_element_output_1 = {};
+		_delay_element_output_2 = {};
+
+		_b0 = 1.f;
+		_b1 = 0.f;
+		_b2 = 0.f;
 
-	T reset(const T &sample);
+		_a1 = 0.f;
+		_a2 = 0.f;
+	}
 
 protected:
-	float _notch_freq{};
-	float _bandwidth{};
-	float _sample_freq{};
+	T _delay_element_1{};
+	T _delay_element_2{};
+	T _delay_element_output_1{};
+	T _delay_element_output_2{};
 
 	// All the coefficients are normalized by a0, so a0 becomes 1 here
-	float _a1{};
-	float _a2{};
+	float _a1{0.f};
+	float _a2{0.f};
 
-	float _b0{};
-	float _b1{};
-	float _b2{};
+	float _b0{1.f};
+	float _b1{0.f};
+	float _b2{0.f};
 
-	T _delay_element_1;
-	T _delay_element_2;
-	T _delay_element_output_1;
-	T _delay_element_output_2;
+	float _notch_freq{};
+	float _bandwidth{};
+	float _sample_freq{};
 };
 
 /**
  * Initialises the filter by setting its parameters and coefficients.
- * If using the direct form I (applyDF1) method, allows to dynamically
+ * Using the direct form I 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;
-	_sample_freq = sample_freq;
-
-	if (notch_freq <= 0.f) {
-		// no filtering
-		_b0 = 1.0f;
-		_b1 = 0.0f;
-		_b2 = 0.0f;
-
-		_a1 = 0.0f;
-		_a2 = 0.0f;
+	if ((sample_freq <= 0.f) || (notch_freq <= 0.f) || (bandwidth <= 0.f) || (notch_freq >= sample_freq / 2)
+	    || !isFinite(sample_freq) || !isFinite(notch_freq) || !isFinite(bandwidth)) {
 
+		disable();
 		return;
 	}
 
+	_notch_freq = math::constrain(notch_freq, 5.f, sample_freq / 2); // TODO: min based on actual numerical limit
+	_bandwidth = math::constrain(bandwidth, 5.f, sample_freq / 2);
+	_sample_freq = sample_freq;
+
 	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);
@@ -207,23 +222,10 @@ void NotchFilter<T>::setParameters(float sample_freq, float notch_freq, float ba
 
 	_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);
+	if (!isFinite(_b0) || !isFinite(_b1) || !isFinite(_b2) || !isFinite(_a1) || !isFinite(_a2)) {
+		disable();
 	}
-
-	_delay_element_1 = dval;
-	_delay_element_2 = dval;
-	_delay_element_output_1 = {};
-	_delay_element_output_2 = {};
-
-	return apply(sample);
 }
 
 } // namespace math

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

@@ -1,117 +0,0 @@
-/****************************************************************************
- *
- *   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>
- * @author Samuel Garcin <samuel.garcin@wecorpindustries.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>::_delay_element_output_1;
-	using NotchFilter<T>::_delay_element_output_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;
-
-	// Filter array of samples in place using the Direct form II.
-	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;
-		}
-	}
-
-	/**
-	 * 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
-			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

@@ -117,7 +117,7 @@ TEST_F(NotchFilterTest, filteringLowSideDF1)
 	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);
+		out = _notch_float.apply(input);
 		t = i * dt;
 
 		// Let some time for the filter to settle
@@ -167,7 +167,7 @@ TEST_F(NotchFilterTest, filteringHighSideDF1)
 	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);
+		out = _notch_float.apply(input);
 		t = i * dt;
 
 		// Let some time for the filter to settle
@@ -213,7 +213,7 @@ TEST_F(NotchFilterTest, filterOnNotchDF1)
 
 	for (int i = 0; i < 1000; i++) {
 		float input = sinf(omega * t);
-		out = _notch_float.applyDF1(input);
+		out = _notch_float.apply(input);
 		t = i * dt;
 
 		// Let some time for the filter to settle
@@ -282,7 +282,7 @@ TEST_F(NotchFilterTest, filterVector3fDF1)
 		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);
+		out = _notch_vector3f.apply(input);
 		t = i * dt;
 
 		// Let some time for the filter to settle

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp

@@ -545,7 +545,7 @@ float VehicleAngularVelocity::FilterAngularVelocity(int axis, float data[], int
 		for (auto &dnf : _dynamic_notch_filter_esc_rpm) {
 			for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) {
 				if (dnf[harmonic][axis].getNotchFreq() > 0.f) {
-					dnf[harmonic][axis].applyDF1(data, N);
+					dnf[harmonic][axis].applyArray(data, N);
 
 				} else {
 					break;
@@ -562,7 +562,7 @@ float VehicleAngularVelocity::FilterAngularVelocity(int axis, float data[], int
 
 		for (auto &dnf : _dynamic_notch_filter_fft) {
 			if (dnf[axis].getNotchFreq() > 0.f) {
-				dnf[axis].applyDF1(data, N);
+				dnf[axis].applyArray(data, N);
 			}
 		}
 
@@ -573,7 +573,7 @@ float VehicleAngularVelocity::FilterAngularVelocity(int axis, float data[], int
 
 	// Apply general notch filter (IMU_GYRO_NF_FREQ)
 	if (_notch_filter_velocity[axis].getNotchFreq() > 0.f) {
-		_notch_filter_velocity[axis].apply(data, N);
+		_notch_filter_velocity[axis].applyArray(data, N);
 	}
 
 	// Apply general low-pass filter (IMU_GYRO_CUTOFF)

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp

@@ -38,7 +38,7 @@
 #include <lib/matrix/matrix/math.hpp>
 #include <lib/mathlib/math/filter/LowPassFilter2p.hpp>
 #include <lib/mathlib/math/filter/LowPassFilter2pArray.hpp>
-#include <lib/mathlib/math/filter/NotchFilterArray.hpp>
+#include <lib/mathlib/math/filter/NotchFilter.hpp>
 #include <px4_platform_common/log.h>
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/px4_config.h>
@@ -133,7 +133,7 @@ private:
 
 	// angular velocity filters
 	math::LowPassFilter2pArray _lp_filter_velocity[3] {{kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}};
-	math::NotchFilterArray<float> _notch_filter_velocity[3] {};
+	math::NotchFilter<float> _notch_filter_velocity[3] {};
 
 #if !defined(CONSTRAINED_FLASH)
 
@@ -148,8 +148,8 @@ private:
 	static constexpr int MAX_NUM_FFT_PEAKS = sizeof(sensor_gyro_fft_s::peak_frequencies_x) / sizeof(
 				sensor_gyro_fft_s::peak_frequencies_x[0]);
 
-	math::NotchFilterArray<float> _dynamic_notch_filter_esc_rpm[MAX_NUM_ESC_RPM][MAX_NUM_ESC_RPM_HARMONICS][3] {};
-	math::NotchFilterArray<float> _dynamic_notch_filter_fft[MAX_NUM_FFT_PEAKS][3] {};
+	math::NotchFilter<float> _dynamic_notch_filter_esc_rpm[MAX_NUM_ESC_RPM][MAX_NUM_ESC_RPM_HARMONICS][3] {};
+	math::NotchFilter<float> _dynamic_notch_filter_fft[MAX_NUM_FFT_PEAKS][3] {};
 
 	perf_counter_t _dynamic_notch_filter_esc_rpm_update_perf{nullptr};
 	perf_counter_t _dynamic_notch_filter_fft_update_perf{nullptr};