AlphaFilter: merge with PX4 implementation

I made a separate implementation of the same filter for PX4. Now
that I know it's duplicate I merge the two into one and reuse it.

EKF/AlphaFilter.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2020 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
@@ -32,32 +32,60 @@
  ****************************************************************************/
 
 /**
- * First order "alpha" IIR digital filter
+ * @file AlphaFilter.hpp
+ *
+ * @brief First order "alpha" IIR digital filter also known as leaky integrator or forgetting average.
+ *
  * @author Mathieu Bresciani <brescianimathieu@gmail.com>
+ * @author Matthias Grob <maetugr@gmail.com>
  */
 
 #pragma once
 
 template <typename T>
-class AlphaFilter final {
+class AlphaFilter {
 public:
 	AlphaFilter() = default;
 	~AlphaFilter() = default;
 
-	void reset(const T &val) { _x = val; }
-
-	void update(const T &input, float tau, float dt) {
-		const float alpha = dt / tau;
-		update(input, alpha);
+	/**
+	 * Set filter parameters for time abstraction
+	 *
+	 * Both parameters have to be provided in the same units.
+	 *
+	 * @param sample_interval interval between two samples
+	 * @param time_constant filter time constant determining convergence
+	 */
+	void setParameters(float sample_interval, float time_constant) {
+		setAlpha(sample_interval / (time_constant + sample_interval));
 	}
 
-	void update(const T &input, float alpha) { _x = (1.f - alpha) * _x + alpha * input; }
+	/**
+	 * 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; }
+
+	/**
+	 * Set filter state to an initial value
+	 *
+	 * @param sample new initial value
+	 */
+	void reset(const T &sample) { _filter_state = sample; }
+
+	/**
+	 * Add a new raw value to the filter
+	 *
+	 * @return retrieve the filtered result
+	 */
+	void update(const T &sample) { _filter_state = updateCalculation(sample); }
 
-	// Typical 0.9/0.1 lowpass filter
-	void update(const T &input) { update(input, 0.1f); }
+	const T &getState() const { return _filter_state; }
 
-	const T &getState() const { return _x; }
+protected:
+	T updateCalculation(const T &sample) { return (1.f - _alpha) * _filter_state + _alpha * sample; }
 
-private:
-	T _x{};  ///< current state of the filter
+	float _alpha{0.f};
+	T _filter_state{};
 };

EKF/ekf.cpp

@@ -48,7 +48,9 @@ bool Ekf::init(uint64_t timestamp)
 {
 	bool ret = initialise_interface(timestamp);
 	reset();
-
+	_accel_lpf.setAlpha(.1f);
+	_gyro_lpf.setAlpha(.1f);
+	_mag_lpf.setAlpha(.1f);
 	return ret;
 }
 

EKF/ekf.h

@@ -458,11 +458,11 @@ private:
 	bool _is_first_imu_sample{true};
 	uint32_t _baro_counter{0};		///< number of baro samples read during initialisation
 	uint32_t _mag_counter{0};		///< number of magnetometer samples read during initialisation
-	AlphaFilterVector3f _accel_lpf;		///< filtered accelerometer measurement used to align tilt (m/s/s)
-	AlphaFilterVector3f _gyro_lpf;		///< filtered gyro measurement used for alignment excessive movement check (rad/sec)
+	AlphaFilter<Vector3f> _accel_lpf;	///< filtered accelerometer measurement used to align tilt (m/s/s)
+	AlphaFilter<Vector3f> _gyro_lpf;	///< filtered gyro measurement used for alignment excessive movement check (rad/sec)
 
 	// Variables used to perform in flight resets and switch between height sources
-	AlphaFilterVector3f _mag_lpf;		///< filtered magnetometer measurement for instant reset (Gauss)
+	AlphaFilter<Vector3f> _mag_lpf;		///< filtered magnetometer measurement for instant reset (Gauss)
 	float _hgt_sensor_offset{0.0f};		///< set as necessary if desired to maintain the same height after a height reset (m)
 	float _baro_hgt_offset{0.0f};		///< baro height reading at the local NED origin (m)
 

EKF/estimator_interface.h

@@ -59,8 +59,6 @@ class EstimatorInterface
 {
 
 public:
-	typedef AlphaFilter<Vector3f> AlphaFilterVector3f;
-
 	EstimatorInterface():_imu_down_sampler(FILTER_UPDATE_PERIOD_S){};
 	virtual ~EstimatorInterface() = default;
 

test/test_AlphaFilter.cpp

@@ -31,6 +31,12 @@
  *
  ****************************************************************************/
 
+/**
+ * @file test_AlphaFilter.cpp
+ *
+ * @brief Unit tests for the alpha filter class
+ */
+
 #include <gtest/gtest.h>
 #include <cmath>
 #include <matrix/math.hpp>
@@ -39,26 +45,23 @@
 
 using matrix::Vector3f;
 
-class AlphaFilterTest : public ::testing::Test {
- public:
-	AlphaFilter<float> filter_float{};
-	AlphaFilter<Vector3f> filter_v3{};
-};
-
-TEST_F(AlphaFilterTest, initializeToZero)
+TEST(AlphaFilterTest, initializeToZero)
 {
+	AlphaFilter<float> filter_float{};
 	ASSERT_EQ(filter_float.getState(), 0.f);
 }
 
-TEST_F(AlphaFilterTest, resetToValue)
+TEST(AlphaFilterTest, resetToValue)
 {
+	AlphaFilter<float> filter_float{};
 	const float reset_value = 42.42f;
 	filter_float.reset(reset_value);
 	ASSERT_EQ(filter_float.getState(), reset_value);
 }
 
-TEST_F(AlphaFilterTest, runZero)
+TEST(AlphaFilterTest, runZero)
 {
+	AlphaFilter<float> filter_float{};
 	const float input = 0.f;
 	for (int i = 0; i < 10; i++) {
 		filter_float.update(input);
@@ -66,10 +69,12 @@ TEST_F(AlphaFilterTest, runZero)
 	ASSERT_EQ(filter_float.getState(), input);
 }
 
-TEST_F(AlphaFilterTest, runPositive)
+TEST(AlphaFilterTest, runPositive)
 {
-	// GIVEN an input of 1 in a filter with a default time constant of 9 (alpha = 0.1)
+	// GIVEN an input of 1 in a filter with a default time constant of 9 (alpha = 0.9)
+	AlphaFilter<float> filter_float{};
 	const float input = 1.f;
+	filter_float.setAlpha(.1f);
 
 	// WHEN we run the filter 9 times
 	for (int i = 0; i < 9; i++) {
@@ -80,10 +85,12 @@ TEST_F(AlphaFilterTest, runPositive)
 	ASSERT_NEAR(filter_float.getState(), 0.63f, 0.02);
 }
 
-TEST_F(AlphaFilterTest, runNegative)
+TEST(AlphaFilterTest, runNegative)
 {
-	// GIVEN an input of 1 in a filter with a default time constant of 9 (alpha = 0.1)
+	// GIVEN an input of 1 in a filter with a default time constant of 9 (alpha = 0.9)
+	AlphaFilter<float> filter_float{};
 	const float input = -1.f;
+	filter_float.setAlpha(.1f);
 
 	// WHEN we run the filter 9 times
 	for (int i = 0; i < 9; i++) {
@@ -94,10 +101,12 @@ TEST_F(AlphaFilterTest, runNegative)
 	ASSERT_NEAR(filter_float.getState(), -0.63f, 0.02);
 }
 
-TEST_F(AlphaFilterTest, riseTime)
+TEST(AlphaFilterTest, riseTime)
 {
-	// GIVEN an input of 1 in a filter with a default time constant of 9 (alpha = 0.1)
+	// GIVEN an input of 1 in a filter with a default time constant of 9 (alpha = 0.9)
+	AlphaFilter<float> filter_float{};
 	const float input = 1.f;
+	filter_float.setAlpha(.1f);
 
 	// WHEN we run the filter 27 times (3 * time constant)
 	for (int i = 0; i < 3 * 9; i++) {
@@ -108,10 +117,12 @@ TEST_F(AlphaFilterTest, riseTime)
 	ASSERT_NEAR(filter_float.getState(), 0.95f, 0.02f);
 }
 
-TEST_F(AlphaFilterTest, convergence)
+TEST(AlphaFilterTest, convergence)
 {
-	// GIVEN an input of 1 in a filter with a default time constant of 9 (alpha = 0.1)
+	// GIVEN an input of 1 in a filter with a default time constant of 9 (alpha = 0.9)
+	AlphaFilter<float> filter_float{};
 	const float input = 1.f;
+	filter_float.setAlpha(.1f);
 
 	// WHEN we run the filter 45 times (5 * time constant)
 	for (int i = 0; i < 5 * 9; i++) {
@@ -122,10 +133,12 @@ TEST_F(AlphaFilterTest, convergence)
 	ASSERT_NEAR(filter_float.getState(), 1.f, 0.01f);
 }
 
-TEST_F(AlphaFilterTest, convergenceVector3f)
+TEST(AlphaFilterTest, convergenceVector3f)
 {
-	// GIVEN an Vector3f input in a filter with a default time constant of 9 (alpha = 0.1)
+	// GIVEN an Vector3f input in a filter with a default time constant of 9 (alpha = 0.9)
+	AlphaFilter<Vector3f> filter_v3{};
 	const Vector3f input = {3.f, 7.f, -11.f};
+	filter_v3.setAlpha(.1f);
 
 	// WHEN we run the filter 45 times (5 * time constant)
 	for (int i = 0; i < 5 * 9; i++) {
@@ -139,17 +152,18 @@ TEST_F(AlphaFilterTest, convergenceVector3f)
 	}
 }
 
-TEST_F(AlphaFilterTest, convergenceVector3fAlpha)
+TEST(AlphaFilterTest, convergenceVector3fAlpha)
 {
 	// GIVEN a Vector3f input in a filter with a defined time constant and the default sampling time
+	AlphaFilter<Vector3f> filter_v3{};
 	const Vector3f input = {3.f, 7.f, -11.f};
 	const float tau = 18.f;
 	const float dt = 1.f;
-	const float alpha = dt / tau;
+	filter_v3.setParameters(dt, tau);
 
 	// WHEN we run the filter 18 times (1 * time constant)
 	for (int i = 0; i < 18; i++) {
-		filter_v3.update(input, alpha); // dt is assumed equal to 1
+		filter_v3.update(input); // dt is assumed equal to 1
 	}
 
 	// THEN the state of the filter should have reached 65% (2% error allowed)
@@ -159,17 +173,19 @@ TEST_F(AlphaFilterTest, convergenceVector3fAlpha)
 	}
 }
 
-TEST_F(AlphaFilterTest, convergenceVector3fTauDt)
+TEST(AlphaFilterTest, convergenceVector3fTauDt)
 {
 	// GIVEN a Vector3f input in a filter with a defined time constant and sampling time
+	AlphaFilter<Vector3f> filter_v3{};
 	const Vector3f input = {51.f, 7.f, -11.f};
 	const float tau = 2.f;
 	const float dt = 0.1f;
+	filter_v3.setParameters(dt, tau);
 
 	// WHEN we run the filter (1 * time constant)
 	const float n = tau / dt;
 	for (int i = 0; i < n; i++) {
-		filter_v3.update(input, tau, dt);
+		filter_v3.update(input);
 	}
 
 	// THEN the state of the filter should have reached 65% (2% error allowed)
@@ -188,3 +204,57 @@ TEST_F(AlphaFilterTest, convergenceVector3fTauDt)
 		ASSERT_EQ(output(i), reset_vector(i));
 	}
 }
+
+TEST(AlphaFilterTest, AllZeroTest)
+{
+	AlphaFilter<float> _alpha_filter;
+	_alpha_filter.update(0.f);
+	EXPECT_FLOAT_EQ(_alpha_filter.getState(), 0.f);
+}
+
+TEST(AlphaFilterTest, AlphaOneTest)
+{
+	AlphaFilter<float> _alpha_filter;
+	_alpha_filter.setParameters(1e-5f, 1e5f);
+
+	for (int i = 0; i < 100; i++) {
+		_alpha_filter.update(1.f);
+		EXPECT_NEAR(_alpha_filter.getState(), 0.f, 1e-4f);
+	}
+}
+
+TEST(AlphaFilterTest, AlphaZeroTest)
+{
+	AlphaFilter<float> _alpha_filter;
+	_alpha_filter.setParameters(.1f, 0.f);
+
+	for (int i = 0; i < 100; i++) {
+		const float new_smaple = static_cast<float>(i);
+		_alpha_filter.update(new_smaple);
+		EXPECT_FLOAT_EQ(_alpha_filter.getState(), new_smaple);
+	}
+}
+
+TEST(AlphaFilterTest, ConvergenceTest)
+{
+	AlphaFilter<float> _alpha_filter;
+	_alpha_filter.setParameters(.1f, 1.f);
+
+	float last_value{0.f};
+
+	for (int i = 0; i < 100; i++) {
+		_alpha_filter.update(1.f);
+		EXPECT_GE(_alpha_filter.getState(), last_value);
+		last_value = _alpha_filter.getState();
+	}
+
+	EXPECT_NEAR(last_value, 1.f, 1e-4f);
+
+	for (int i = 0; i < 1000; i++) {
+		_alpha_filter.update(-100.f);
+		EXPECT_LE(_alpha_filter.getState(), last_value);
+		last_value = _alpha_filter.getState();
+	}
+
+	EXPECT_NEAR(last_value, -100.f, 1e-4f);
+}