From 65a4ca9d65818b42197b91cabd14a0c3fa8d6ff2 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 8 Apr 2020 14:08:23 +0200 Subject: [PATCH] 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 | 56 +++++++++++++----- EKF/ekf.cpp | 4 +- EKF/ekf.h | 6 +- EKF/estimator_interface.h | 2 - test/test_AlphaFilter.cpp | 118 ++++++++++++++++++++++++++++++-------- 5 files changed, 142 insertions(+), 44 deletions(-) diff --git a/EKF/AlphaFilter.hpp b/EKF/AlphaFilter.hpp index 05d701d7b7..ebf838a42c 100644 --- a/EKF/AlphaFilter.hpp +++ b/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 + * @author Matthias Grob */ #pragma once template -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; } - // Typical 0.9/0.1 lowpass filter - void update(const T &input) { update(input, 0.1f); } + /** + * Set filter state to an initial value + * + * @param sample new initial value + */ + void reset(const T &sample) { _filter_state = sample; } - const T &getState() const { return _x; } + /** + * Add a new raw value to the filter + * + * @return retrieve the filtered result + */ + void update(const T &sample) { _filter_state = updateCalculation(sample); } -private: - T _x{}; ///< current state of the filter + 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{}; }; diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 9b0cb90443..b6e820b48a 100644 --- a/EKF/ekf.cpp +++ b/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; } diff --git a/EKF/ekf.h b/EKF/ekf.h index 8b1a67d04a..c601d9f6e9 100644 --- a/EKF/ekf.h +++ b/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 _accel_lpf; ///< filtered accelerometer measurement used to align tilt (m/s/s) + AlphaFilter _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 _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) diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index d379781485..92c36eac3d 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -59,8 +59,6 @@ class EstimatorInterface { public: - typedef AlphaFilter AlphaFilterVector3f; - EstimatorInterface():_imu_down_sampler(FILTER_UPDATE_PERIOD_S){}; virtual ~EstimatorInterface() = default; diff --git a/test/test_AlphaFilter.cpp b/test/test_AlphaFilter.cpp index aac7a977cc..8bedff1bf1 100644 --- a/test/test_AlphaFilter.cpp +++ b/test/test_AlphaFilter.cpp @@ -31,6 +31,12 @@ * ****************************************************************************/ +/** + * @file test_AlphaFilter.cpp + * + * @brief Unit tests for the alpha filter class + */ + #include #include #include @@ -39,26 +45,23 @@ using matrix::Vector3f; -class AlphaFilterTest : public ::testing::Test { - public: - AlphaFilter filter_float{}; - AlphaFilter filter_v3{}; -}; - -TEST_F(AlphaFilterTest, initializeToZero) +TEST(AlphaFilterTest, initializeToZero) { + AlphaFilter filter_float{}; ASSERT_EQ(filter_float.getState(), 0.f); } -TEST_F(AlphaFilterTest, resetToValue) +TEST(AlphaFilterTest, resetToValue) { + AlphaFilter 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 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 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 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 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 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 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 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 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 _alpha_filter; + _alpha_filter.update(0.f); + EXPECT_FLOAT_EQ(_alpha_filter.getState(), 0.f); +} + +TEST(AlphaFilterTest, AlphaOneTest) +{ + AlphaFilter _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 _alpha_filter; + _alpha_filter.setParameters(.1f, 0.f); + + for (int i = 0; i < 100; i++) { + const float new_smaple = static_cast(i); + _alpha_filter.update(new_smaple); + EXPECT_FLOAT_EQ(_alpha_filter.getState(), new_smaple); + } +} + +TEST(AlphaFilterTest, ConvergenceTest) +{ + AlphaFilter _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); +}