mathlib: AlphaFilter add additional checks and updates methods

- setParameters() and setCutoffFreq() sanity check all input
 - on invalid configuration disable filter and leave in safe state
 - constrain cutoff frequency to Nyquist rather than disabling the filter
 - allow configuring only time constant directly
 - add new update() method with dt that also recomputes parameters if necessary
 - filter update don't allow invalid data (NAN, etc) to propagate

src/lib/mathlib/math/Functions.hpp

@@ -271,6 +271,11 @@ inline bool isFinite(const float &value)
 	return PX4_ISFINITE(value);
 }
 
+inline bool isFinite(const matrix::Vector2f &value)
+{
+	return PX4_ISFINITE(value(0)) && PX4_ISFINITE(value(1));
+}
+
 inline bool isFinite(const matrix::Vector3f &value)
 {
 	return PX4_ISFINITE(value(0)) && PX4_ISFINITE(value(1)) && PX4_ISFINITE(value(2));

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

@@ -56,6 +56,30 @@ public:
 
 	~AlphaFilter() = default;
 
+	// 0.01 to 10,000 Hz
+	static constexpr float MIN_FREQ_HZ = 0.01f;    // 0.01 Hz or 100s interval
+	static constexpr float MAX_FREQ_HZ = 10'000.f; // 10,000 Hz or 0.0001s interval
+
+	static constexpr float min_interval_s() { return 1.f / MAX_FREQ_HZ; }
+	static constexpr float max_interval_s() { return 1.f / MIN_FREQ_HZ; }
+
+	/**
+	 * Set filter parameter alpha directly without time abstraction
+	 *
+	 * @param alpha [0,1] filter weight for the previous state. High value - long time constant.
+	 */
+	bool setAlpha(const float alpha)
+	{
+		if (alpha < 0.f || alpha > 1.f || !PX4_ISFINITE(alpha)) {
+			// Invalid parameters, disable filter
+			disable();
+			return false;
+		}
+
+		_alpha = alpha;
+		return true;
+	}
+
 	/**
 	 * Set filter parameters for time abstraction
 	 *
@@ -64,42 +88,86 @@ public:
 	 * @param sample_interval interval between two samples
 	 * @param time_constant filter time constant determining convergence
 	 */
-	void setParameters(float sample_interval, float time_constant)
+	bool setParameters(const float sample_interval, const float time_constant)
 	{
-		const float denominator = time_constant + sample_interval;
-
-		if (denominator > FLT_EPSILON) {
-			setAlpha(sample_interval / denominator);
-		}
-	}
-
-	bool setCutoffFreq(float sample_freq, float cutoff_freq)
-	{
-		if ((sample_freq <= 0.f) || (cutoff_freq <= 0.f) || (cutoff_freq >= sample_freq / 2.f)
-		    || !isFinite(sample_freq) || !isFinite(cutoff_freq)) {
-
-			// Invalid parameters
+		if ((sample_interval <= 0.f) || !PX4_ISFINITE(sample_interval)
+		    || (time_constant <= 0.f) || !PX4_ISFINITE(time_constant)) {
+			// invalid parameters, disable filter
+			disable();
 			return false;
 		}
 
-		setParameters(1.f / sample_freq, 1.f / (2.f * M_PI_F * cutoff_freq));
-		_cutoff_freq = cutoff_freq;
-		return true;
+		_sample_interval = math::constrain(sample_interval, min_interval_s(), max_interval_s());
+		_time_constant = time_constant;
+
+		const float alpha = _sample_interval / (_sample_interval + _time_constant);
+
+		return setAlpha(alpha);
 	}
 
-	/**
-	 * Set filter parameter alpha directly without time abstraction
-	 *
-	 * @param alpha [0,1] filter weight for the previous state. High value - long time constant.
-	 */
-	void setAlpha(float alpha) { _alpha = alpha; }
+	bool setSampleAndCutoffFrequency(const float sample_freq, const float cutoff_freq)
+	{
+		if ((sample_freq <= 0.f) || !PX4_ISFINITE(sample_freq)
+		    || (cutoff_freq <= 0.f) || !PX4_ISFINITE(cutoff_freq)) {
+			// invalid parameters, disable filter
+			disable();
+			return false;
+		}
+
+		const float sample_freq_constrained = math::constrain(sample_freq, MIN_FREQ_HZ, MAX_FREQ_HZ);
+		const float cutoff_freq_constrained = math::constrain(cutoff_freq, MIN_FREQ_HZ, sample_freq_constrained / 2.f);
+
+		_sample_interval = 1.f / sample_freq_constrained;
+		_time_constant = 1.f / (2.f * M_PI_F * cutoff_freq_constrained);
+
+		const float alpha = _sample_interval / (_sample_interval + _time_constant);
+
+		return setAlpha(alpha);
+	}
+
+	bool setTimeConstant(const float time_constant)
+	{
+		if ((time_constant <= 0.f) || !PX4_ISFINITE(time_constant)) {
+			// Invalid parameters, disable filter
+			disable();
+			return false;
+		}
+
+		if ((fabsf(time_constant - _time_constant) / _time_constant) < 0.01f) {
+			// no change, do nothing
+			return true;
+		}
+
+		// time constant changed, update alpha
+		_time_constant = time_constant;
+		const float alpha = _sample_interval / (_sample_interval + _time_constant);
+
+		return setAlpha(alpha);
+	}
+
+	bool setCutoffFrequency(const float cutoff_freq)
+	{
+		const float time_constant = 1.f / (2.f * M_PI_F * cutoff_freq);
+		return setTimeConstant(time_constant);
+	}
 
 	/**
 	 * Set filter state to an initial value
 	 *
 	 * @param sample new initial value
 	 */
-	void reset(const T &sample) { _filter_state = sample; }
+	const T &reset(const T &sample = {})
+	{
+		_filter_state = sample;
+		return _filter_state;
+	}
+
+	void disable()
+	{
+		_alpha = 1.f;
+		_sample_interval = 1.f;
+		_time_constant = 0.f;
+	}
 
 	/**
 	 * Add a new raw value to the filter
@@ -112,13 +180,43 @@ public:
 		return _filter_state;
 	}
 
+	const T &update(const T &sample, const float dt)
+	{
+		if ((fabsf(dt - _sample_interval) / _sample_interval) > 0.01f) {
+			// update parameters if changed by more than 1%
+			if (!setParameters(dt, _time_constant)) {
+				// invalid new parameters, reset filter
+				return reset(sample);
+			}
+		}
+
+		_filter_state = updateCalculation(sample);
+		return _filter_state;
+	}
+
 	const T &getState() const { return _filter_state; }
-	float getCutoffFreq() const { return _cutoff_freq; }
 
-protected:
-	T updateCalculation(const T &sample) { return (1.f - _alpha) * _filter_state + _alpha * sample; }
+	float getCutoffFreq() const { return 1.f / (2.f * M_PI_F * _time_constant); }
+
+	const float &getSampleInterval() const { return _sample_interval; }
+	const float &getTimeConstant() const { return _time_constant; }
+
+private:
+	T updateCalculation(const T &sample)
+	{
+		// don't allow bad updates to propagate
+		const T ret = (1.f - _alpha) * _filter_state + _alpha * sample;
+
+		if (isFinite(ret)) {
+			return ret;
+		}
+
+		return {};
+	}
 
-	float _cutoff_freq{0.f};
-	float _alpha{0.f};
 	T _filter_state{};
+	float _alpha{1.f};
+
+	float _sample_interval{1.f};
+	float _time_constant{0.f};
 };

src/lib/mathlib/math/test/AlphaFilterTest.cpp

@@ -246,15 +246,19 @@ TEST(AlphaFilterTest, SetFrequencyTest)
 	AlphaFilter<float> _alpha_filter;
 	const float fs = .1f;
 
-	EXPECT_FALSE(_alpha_filter.setCutoffFreq(fs, 0.f));
-	EXPECT_FALSE(_alpha_filter.setCutoffFreq(fs, fs)); // Cutoff above Nyquist freq
-	EXPECT_FALSE(_alpha_filter.setCutoffFreq(0.f, fs / 4.f));
-	EXPECT_FALSE(_alpha_filter.setCutoffFreq(0.f, 0.f));
-	EXPECT_FALSE(_alpha_filter.setCutoffFreq(-fs, fs / 4.f));
-	EXPECT_FALSE(_alpha_filter.setCutoffFreq(-fs, -fs / 4.f));
-	EXPECT_FALSE(_alpha_filter.setCutoffFreq(fs, -fs / 4.f));
+	EXPECT_FALSE(_alpha_filter.setSampleAndCutoffFrequency(fs, 0.f));
 
-	EXPECT_TRUE(_alpha_filter.setCutoffFreq(fs, fs / 4.f));
+	// Cutoff above Nyquist freq
+	EXPECT_TRUE(_alpha_filter.setSampleAndCutoffFrequency(fs, fs));
+	EXPECT_FLOAT_EQ(_alpha_filter.getCutoffFreq(), fs / 2.f);
+
+	EXPECT_FALSE(_alpha_filter.setSampleAndCutoffFrequency(0.f, fs / 4.f));
+	EXPECT_FALSE(_alpha_filter.setSampleAndCutoffFrequency(0.f, 0.f));
+	EXPECT_FALSE(_alpha_filter.setSampleAndCutoffFrequency(-fs, fs / 4.f));
+	EXPECT_FALSE(_alpha_filter.setSampleAndCutoffFrequency(-fs, -fs / 4.f));
+	EXPECT_FALSE(_alpha_filter.setSampleAndCutoffFrequency(fs, -fs / 4.f));
+
+	EXPECT_TRUE(_alpha_filter.setSampleAndCutoffFrequency(fs, fs / 4.f));
 }
 
 TEST(AlphaFilterTest, ConvergenceTest)

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp

@@ -183,7 +183,7 @@ void VehicleAngularVelocity::ResetFilters(const hrt_abstime &time_now_us)
 
 			// angular acceleration low pass
 			if ((_param_imu_dgyro_cutoff.get() > 0.f)
-			    && (_lp_filter_acceleration[axis].setCutoffFreq(_filter_sample_rate_hz, _param_imu_dgyro_cutoff.get()))) {
+			    && (_lp_filter_acceleration[axis].setSampleAndCutoffFrequency(_filter_sample_rate_hz, _param_imu_dgyro_cutoff.get()))) {
 				_lp_filter_acceleration[axis].reset(angular_acceleration_uncalibrated(axis));
 
 			} else {