AlphaFilter: move from ecl -> mathlib

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

@@ -0,0 +1,113 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 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
+ * 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 2nd order Butterworth low-pass filter
+ * Run this test only using make tests TESTFILTER=LowPassFilter2pVector3f
+ */
+
+#include <gtest/gtest.h>
+#include <matrix/matrix/math.hpp>
+#include <px4_platform_common/defines.h>
+
+#include <lib/mathlib/math/filter/LowPassFilter2p.hpp>
+
+using matrix::Vector3f;
+
+class LowPassFilter2pVector3fTest : public ::testing::Test
+{
+public:
+	void runSimulatedFilter(const Vector3f &signal_freq_hz, const Vector3f &phase_delay_deg, const Vector3f &gain_db);
+
+	math::LowPassFilter2p<Vector3f> _lpf{800.f, 30.f};
+
+	const float _epsilon_near = 0.08f;
+};
+
+void LowPassFilter2pVector3fTest::runSimulatedFilter(const Vector3f &signal_freq_hz, const Vector3f &phase_delay_deg,
+		const Vector3f &gain_db)
+{
+	const Vector3f phase_delay = phase_delay_deg * M_PI_F / 180.f;
+	const Vector3f omega = 2.f * M_PI_F * signal_freq_hz;
+	Vector3f gain;
+
+	for (int i = 0; i < 3; i++) {
+		gain(i) = powf(10.f, gain_db(i) / 20.f);
+	}
+
+	const float dt = 1.f / _lpf.get_sample_freq();
+	const int n_steps = roundf(1.f / dt); // run for 1 second
+
+	float t = 0.f;
+
+	for (int i = 0; i < n_steps; i++) {
+		Vector3f input{0.f, sinf(omega(1) * t), -sinf(omega(2) * t)};
+		Vector3f output_expected{0.f,
+					 gain(1) *sinf(omega(1) * t - phase_delay(1)),
+					 -gain(2) *sinf(omega(2) * t - phase_delay(2))};
+		Vector3f out = _lpf.apply(input);
+		t = i * dt;
+
+		// Let some time for the filter to settle
+		if (t > 0.3f) {
+			EXPECT_EQ(out(0), 0.f);
+			EXPECT_NEAR(out(1), output_expected(1), _epsilon_near);
+			EXPECT_NEAR(out(2), output_expected(2), _epsilon_near);
+		}
+	}
+}
+
+TEST_F(LowPassFilter2pVector3fTest, setGet)
+{
+	const float sample_freq = 1000.f;
+	const float cutoff_freq = 80.f;
+
+	_lpf.set_cutoff_frequency(sample_freq, cutoff_freq);
+	EXPECT_EQ(_lpf.get_sample_freq(), sample_freq);
+	EXPECT_EQ(_lpf.get_cutoff_freq(), cutoff_freq);
+}
+
+TEST_F(LowPassFilter2pVector3fTest, simulate80HzCutoff)
+{
+	const float sample_freqs[4] = {400.f, 1000.f, 8000.f, 16000.f};
+	const float cutoff_freq = 80.f;
+
+	const Vector3f signal_freq_hz{0.f, 10.f, 100.f};
+	const Vector3f phase_delay_deg = Vector3f{0.f, 10.4f, 108.5f}; // Given by simulation
+	const Vector3f gain_db{0.f, 0.f, -5.66f}; // given by simulation
+
+	for (float sample_freq : sample_freqs) {
+		_lpf.set_cutoff_frequency(sample_freq, cutoff_freq);
+		runSimulatedFilter(signal_freq_hz, phase_delay_deg, gain_db);
+	}
+}

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

@@ -0,0 +1,93 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 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.
+ *
+ ****************************************************************************/
+
+/**
+ * Test code for the Median filter
+ * Run this test only using make tests TESTFILTER=MedianFilter
+ */
+
+#include <gtest/gtest.h>
+#include <matrix/matrix/math.hpp>
+#include <mathlib/mathlib.h>
+
+#include <lib/mathlib/math/filter/MedianFilter.hpp>
+
+using namespace math;
+using matrix::Vector3f;
+
+class MedianFilterTest : public ::testing::Test
+{
+public:
+
+
+};
+
+TEST_F(MedianFilterTest, test3f_simple)
+{
+	MedianFilter<float, 3> median_filter3;
+
+	for (int i = 0; i < 3; i++) {
+		median_filter3.insert(i);
+	}
+
+	EXPECT_EQ(median_filter3.median(), 1); // 0, 1, 2
+}
+
+TEST_F(MedianFilterTest, test3f_100)
+{
+	MedianFilter<float, 3> median_filter3;
+
+	for (int i = 0; i < 100; i++) {
+		EXPECT_EQ(median_filter3.apply(i), max(0, i - 1));
+	}
+}
+
+TEST_F(MedianFilterTest, test5u_simple)
+{
+	MedianFilter<uint16_t, 5> median_filter5;
+
+	for (int i = 0; i < 5; i++) {
+		median_filter5.insert(i);
+	}
+
+	EXPECT_EQ(median_filter5.median(), 2); // 0, 1, 2, 4, 5
+}
+
+TEST_F(MedianFilterTest, test5i_100)
+{
+	MedianFilter<uint16_t, 5> median_filter5;
+
+	for (int i = 0; i < 100; i++) {
+		EXPECT_EQ(median_filter5.apply(i), max(0, i - 2));
+	}
+}

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

@@ -0,0 +1,349 @@
+/****************************************************************************
+ *
+ *   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 <lib/mathlib/math/filter/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.reset(0.0f);
+	_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, filteringLowSideDF1)
+{
+	// Send a 25Hz sinusoidal signal into a 50Hz notch filter
+	_notch_float.reset(0.0f);
+	_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.reset(0.0f);
+	_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, filteringHighSideDF1)
+{
+	// Send a 98 sinusoidal signal into a 50Hz notch filter
+	_notch_float.reset(0.0f);
+	_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.reset(0.0f);
+	_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, filterOnNotchDF1)
+{
+	// Send a 50 sinusoidal signal into a 50Hz notch filter
+	_notch_float.reset(0.0f);
+	_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, updateFilter)
+{
+	_notch_float.reset(0.0f);
+	_notch_float.setParameters(_sample_freq, _notch_freq, _bandwidth);
+	float new_notch_freq = 100.f;
+	float new_bandwidth = 10.f;
+	_notch_float.setParameters(_sample_freq, new_notch_freq, new_bandwidth);
+
+	EXPECT_EQ(new_notch_freq, _notch_float.getNotchFreq());
+	EXPECT_EQ(new_bandwidth, _notch_float.getBandwidth());
+
+}
+
+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, filterVector3fDF1)
+{
+	// 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);
+	}
+}
+
+TEST_F(NotchFilterTest, setCoefficients)
+{
+
+	float a_new[2] = {1.f, 2.f};
+	float b_new[3] = {1.f, 2.f, 3.f};
+	float a[3];
+	float b[3];
+
+	_notch_float.setCoefficients(a_new, b_new);
+	_notch_float.getCoefficients(a, b);
+
+	for (int i = 0; i < 3; i++) {
+		if (i >= 1) {EXPECT_EQ(a[i], a_new[i - 1]);} //a0 is not part of set function
+
+		EXPECT_EQ(b[i], b_new[i]);
+	}
+}