AlphaFilter: move from ecl -> mathlib

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

@@ -0,0 +1,105 @@
+/****************************************************************************
+ *
+ *   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
+ * 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 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
+
+#include <float.h>
+
+template <typename T>
+class AlphaFilter
+{
+public:
+	AlphaFilter() = default;
+	explicit AlphaFilter(float alpha) : _alpha(alpha) {}
+
+	~AlphaFilter() = default;
+
+	/**
+	 * 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)
+	{
+		const float denominator = time_constant + sample_interval;
+
+		if (denominator > FLT_EPSILON) {
+			setAlpha(sample_interval / denominator);
+		}
+	}
+
+	/**
+	 * 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
+	 */
+	const T &update(const T &sample)
+	{
+		_filter_state = updateCalculation(sample);
+		return _filter_state;
+	}
+
+	const T &getState() const { return _filter_state; }
+
+protected:
+	T updateCalculation(const T &sample) { return (1.f - _alpha) * _filter_state + _alpha * sample; }
+
+	float _alpha{0.f};
+	T _filter_state{};
+};

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

@@ -0,0 +1,266 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 ECL 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 test_AlphaFilter.cpp
+ *
+ * @brief Unit tests for the alpha filter class
+ */
+
+#include <gtest/gtest.h>
+#include <cmath>
+#include <matrix/math.hpp>
+
+#include <lib/mathlib/math/filter/AlphaFilter.hpp>
+
+using matrix::Vector3f;
+
+TEST(AlphaFilterTest, initializeToZero)
+{
+	AlphaFilter<float> filter_float{};
+	ASSERT_EQ(filter_float.getState(), 0.f);
+}
+
+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(AlphaFilterTest, runZero)
+{
+	AlphaFilter<float> filter_float{};
+	const float input = 0.f;
+
+	for (int i = 0; i < 10; i++) {
+		filter_float.update(input);
+	}
+
+	ASSERT_EQ(filter_float.getState(), input);
+}
+
+TEST(AlphaFilterTest, runPositive)
+{
+	// 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++) {
+		filter_float.update(input);
+	}
+
+	// THEN the state of the filter should have reached 63%
+	ASSERT_NEAR(filter_float.getState(), 0.63f, 0.02);
+}
+
+TEST(AlphaFilterTest, runNegative)
+{
+	// 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++) {
+		filter_float.update(input);
+	}
+
+	// THEN the state of the filter should have reached 63%
+	ASSERT_NEAR(filter_float.getState(), -0.63f, 0.02);
+}
+
+TEST(AlphaFilterTest, riseTime)
+{
+	// 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++) {
+		filter_float.update(input);
+	}
+
+	// THEN the state of the filter should have reached 95%
+	ASSERT_NEAR(filter_float.getState(), 0.95f, 0.02f);
+}
+
+TEST(AlphaFilterTest, convergence)
+{
+	// 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++) {
+		filter_float.update(input);
+	}
+
+	// THEN the state of the filter should have converged to the input
+	ASSERT_NEAR(filter_float.getState(), 1.f, 0.01f);
+}
+
+TEST(AlphaFilterTest, convergenceVector3f)
+{
+	// 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++) {
+		filter_v3.update(input);
+	}
+
+	// THEN the state of the filter should have converged to the input (1% error allowed)
+	Vector3f output = filter_v3.getState();
+
+	for (int i = 0; i < 3; i++) {
+		ASSERT_NEAR(output(i), input(i), fabsf(0.01f * input(i)));
+	}
+}
+
+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;
+	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); // dt is assumed equal to 1
+	}
+
+	// THEN the state of the filter should have reached 65% (2% error allowed)
+	Vector3f output = filter_v3.getState();
+
+	for (int i = 0; i < 3; i++) {
+		ASSERT_NEAR(output(i), 0.63f * input(i), fabsf(0.02f * input(i)));
+	}
+}
+
+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);
+	}
+
+	// THEN the state of the filter should have reached 65% (2% error allowed)
+	Vector3f output = filter_v3.getState();
+
+	for (int i = 0; i < 3; i++) {
+		ASSERT_NEAR(output(i), 0.63f * input(i), fabsf(0.02f * input(i)));
+	}
+
+	// ALSO when the filter is reset to a specified value
+	const Vector3f reset_vector = {-1.f, 71.f, -42.f};
+	filter_v3.reset(reset_vector);
+	output = filter_v3.getState();
+
+	// THEN the filter should exactly contain those values
+	for (int i = 0; i < 3; i++) {
+		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);
+}

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

@@ -40,7 +40,7 @@
 #include <matrix/matrix/math.hpp>
 #include <px4_platform_common/defines.h>
 
-#include "LowPassFilter2p.hpp"
+#include <lib/mathlib/math/filter/LowPassFilter2p.hpp>
 
 using matrix::Vector3f;
 

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

@@ -40,7 +40,7 @@
 #include <matrix/matrix/math.hpp>
 #include <mathlib/mathlib.h>
 
-#include "MedianFilter.hpp"
+#include <lib/mathlib/math/filter/MedianFilter.hpp>
 
 using namespace math;
 using matrix::Vector3f;

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

@@ -39,7 +39,7 @@
 #include <gtest/gtest.h>
 #include <matrix/matrix/math.hpp>
 
-#include "NotchFilter.hpp"
+#include <lib/mathlib/math/filter/NotchFilter.hpp>
 
 using namespace math;
 using matrix::Vector3f;