From c586fe36b7517e88aa1981dbefa11e494ebc1019 Mon Sep 17 00:00:00 2001 From: Thomas Stastny Date: Thu, 17 Feb 2022 14:39:46 +0100 Subject: [PATCH 1/6] mathlib: add second order reference model filter with optional rate feedforward --- src/lib/mathlib/CMakeLists.txt | 2 + .../filter/second_order_reference_model.hpp | 153 ++++++++++++++++++ .../second_order_reference_model_test.cpp | 131 +++++++++++++++ 3 files changed, 286 insertions(+) create mode 100644 src/lib/mathlib/math/filter/second_order_reference_model.hpp create mode 100644 src/lib/mathlib/math/test/second_order_reference_model_test.cpp diff --git a/src/lib/mathlib/CMakeLists.txt b/src/lib/mathlib/CMakeLists.txt index d5e7d9e04600..3afabfc86081 100644 --- a/src/lib/mathlib/CMakeLists.txt +++ b/src/lib/mathlib/CMakeLists.txt @@ -36,11 +36,13 @@ px4_add_library(mathlib math/filter/LowPassFilter2p.hpp math/filter/MedianFilter.hpp math/filter/NotchFilter.hpp + math/filter/second_order_reference_model.hpp ) px4_add_unit_gtest(SRC math/test/LowPassFilter2pVector3fTest.cpp LINKLIBS mathlib) px4_add_unit_gtest(SRC math/test/AlphaFilterTest.cpp) px4_add_unit_gtest(SRC math/test/MedianFilterTest.cpp) px4_add_unit_gtest(SRC math/test/NotchFilterTest.cpp) +px4_add_unit_gtest(SRC math/test/second_order_reference_model_test.cpp) px4_add_unit_gtest(SRC math/FunctionsTest.cpp) px4_add_unit_gtest(SRC math/WelfordMeanTest.cpp) diff --git a/src/lib/mathlib/math/filter/second_order_reference_model.hpp b/src/lib/mathlib/math/filter/second_order_reference_model.hpp new file mode 100644 index 000000000000..77efade1e8c3 --- /dev/null +++ b/src/lib/mathlib/math/filter/second_order_reference_model.hpp @@ -0,0 +1,153 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 second_order_reference_model.hpp + * + * @brief Implementation of a second order system with optional rate feed-forward. + * + * @author Thomas Stastny + */ + +#pragma once + +template +class SecondOrderReferenceModel +{ +public: + SecondOrderReferenceModel() = default; + + /** + * Construct with system parameters + * + * @param[in] natural_freq The desired natural frequency of the system [rad/s] + * @param[in] damping_ratio The desired damping ratio of the system + */ + SecondOrderReferenceModel(const float natural_freq, const float damping_ratio) + { + setParameters(natural_freq, damping_ratio); + } + + /** + * Set the system parameters + * + * Calculates the damping coefficient and spring stiffness + * + * @param[in] natural_freq The desired natural frequency of the system [rad/s] + * @param[in] damping_ratio The desired damping ratio of the system + * @return Whether or not the param set was successful + */ + bool setParameters(const float natural_freq, const float damping_ratio) + { + if (natural_freq < 0.0f || damping_ratio < 0.0f) { + return false; + } + + // note these are "effective" coefficients, as mass coefficient is considered baked in + spring_constant_ = natural_freq * natural_freq; + damping_coefficient_ = 2.0f * damping_ratio * natural_freq; + + return true; + } + + /** + * @return System state + */ + const T &getState() const { return filter_state_; } + + /** + * @return System rate + */ + const T &getRate() const { return filter_rate_; } + + /** + * @return System acceleration + */ + const T &getAccel() const { return filter_accel_; } + + /** + * Update the system states + * + * Units of rate_sample must correspond to the derivative of state_sample units. + * There is currently no handling of discrete time behavior w.r.t. continuous time + * system parameters. Sampling time should be sufficiently fast. + * + * @param[in] time_step Time since last sample [s] + * @param[in] state_sample New state sample + * @param[in] rate_sample New rate sample, if provided, otherwise defaults to zero(s) + */ + void update(const float time_step, const T &state_sample, const T &rate_sample = T()) + { + T state_error = state_sample - filter_state_; + T rate_error = rate_sample - filter_rate_; + + filter_accel_ = state_error * spring_constant_ + rate_error * damping_coefficient_; + filter_rate_ = integrate(filter_rate_, filter_accel_, time_step); + filter_state_ = integrate(filter_state_, filter_rate_, time_step); + } + + /** + * Reset the system states + * + * @param[in] state Initial state + * @param[in] rate Initial rate, if provided, otherwise defaults to zero(s) + */ + void reset(const T &state, const T &rate = T()) + { + filter_state_ = state; + filter_rate_ = rate; + filter_accel_ = T(); + } + +protected: + + float spring_constant_{0.0f}; + float damping_coefficient_{0.0f}; + + T filter_state_{}; + T filter_rate_{}; + T filter_accel_{}; + + /** + * Take one integration step using Euler integration + * + * @param[in] last_state + * @param[in] rate + * @param[in] time_step Time since last sample [s] + * @return The next state + */ + T integrate(const T &last_state, const T &rate, const float time_step) const + { + return last_state + rate * time_step; + } +}; diff --git a/src/lib/mathlib/math/test/second_order_reference_model_test.cpp b/src/lib/mathlib/math/test/second_order_reference_model_test.cpp new file mode 100644 index 000000000000..e248131b3989 --- /dev/null +++ b/src/lib/mathlib/math/test/second_order_reference_model_test.cpp @@ -0,0 +1,131 @@ +/**************************************************************************** + * + * 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 second_order_reference_model_test.cpp + * + * @brief Unit tests for the second order reference model implementation + */ + +#include +#include +#include + +using matrix::Vector3f; + +TEST(SecondOrderReferenceModel, FloatDefaultConstructorInitializesStatesToZero) +{ + SecondOrderReferenceModel sys_float; + + // default constructor leaves states initialized to zero + EXPECT_FLOAT_EQ(sys_float.getState(), 0.0f); + EXPECT_FLOAT_EQ(sys_float.getRate(), 0.0f); + EXPECT_FLOAT_EQ(sys_float.getAccel(), 0.0f); +} + +TEST(SecondOrderReferenceModel, FloatReset) +{ + SecondOrderReferenceModel sys_float; + + // reset the system states + float init_state = 3.0f; + float init_rate = 1.1f; + sys_float.reset(init_state, init_rate); + EXPECT_FLOAT_EQ(sys_float.getState(), init_state); + EXPECT_FLOAT_EQ(sys_float.getRate(), init_rate); + EXPECT_FLOAT_EQ(sys_float.getAccel(), 0.0f); + + // reset the system without providing rate + init_state = 5.5f; + sys_float.reset(init_state); + EXPECT_FLOAT_EQ(sys_float.getState(), init_state); + EXPECT_FLOAT_EQ(sys_float.getRate(), 0.0f); + EXPECT_FLOAT_EQ(sys_float.getAccel(), 0.0f); +} + +TEST(SecondOrderReferenceModel, Vector3DefaultConstructorInitializesStatesToZero) +{ + SecondOrderReferenceModel sys_vector3f; + + // default constructor leaves states initialized to zero + Vector3f state = sys_vector3f.getState(); + EXPECT_FLOAT_EQ(state(0), 0.0f); + EXPECT_FLOAT_EQ(state(1), 0.0f); + EXPECT_FLOAT_EQ(state(2), 0.0f); + Vector3f rate = sys_vector3f.getRate(); + EXPECT_FLOAT_EQ(rate(0), 0.0f); + EXPECT_FLOAT_EQ(rate(1), 0.0f); + EXPECT_FLOAT_EQ(rate(2), 0.0f); + Vector3f accel = sys_vector3f.getAccel(); + EXPECT_FLOAT_EQ(accel(0), 0.0f); + EXPECT_FLOAT_EQ(accel(1), 0.0f); + EXPECT_FLOAT_EQ(accel(2), 0.0f); +} + +TEST(SecondOrderReferenceModel, Vector3Reset) +{ + SecondOrderReferenceModel sys_vector3f; + + // reset the system states + Vector3f init_state(1.0f, 2.0f, 3.0f); + Vector3f init_rate(0.1f, 0.2f, 0.3f); + sys_vector3f.reset(init_state, init_rate); + Vector3f state = sys_vector3f.getState(); + EXPECT_FLOAT_EQ(state(0), init_state(0)); + EXPECT_FLOAT_EQ(state(1), init_state(1)); + EXPECT_FLOAT_EQ(state(2), init_state(2)); + Vector3f rate = sys_vector3f.getRate(); + EXPECT_FLOAT_EQ(rate(0), init_rate(0)); + EXPECT_FLOAT_EQ(rate(1), init_rate(1)); + EXPECT_FLOAT_EQ(rate(2), init_rate(2)); + Vector3f accel = sys_vector3f.getAccel(); + EXPECT_FLOAT_EQ(accel(0), 0.0f); + EXPECT_FLOAT_EQ(accel(1), 0.0f); + EXPECT_FLOAT_EQ(accel(2), 0.0f); + + // reset the system without providing rate + init_state = Vector3f(4.0f, 5.0f, 6.0f); + sys_vector3f.reset(init_state); + state = sys_vector3f.getState(); + EXPECT_FLOAT_EQ(state(0), init_state(0)); + EXPECT_FLOAT_EQ(state(1), init_state(1)); + EXPECT_FLOAT_EQ(state(2), init_state(2)); + rate = sys_vector3f.getRate(); + EXPECT_FLOAT_EQ(rate(0), 0.0f); + EXPECT_FLOAT_EQ(rate(1), 0.0f); + EXPECT_FLOAT_EQ(rate(2), 0.0f); + accel = sys_vector3f.getAccel(); + EXPECT_FLOAT_EQ(accel(0), 0.0f); + EXPECT_FLOAT_EQ(accel(1), 0.0f); + EXPECT_FLOAT_EQ(accel(2), 0.0f); +} From fae13a77b2850b9e71b19866a455b6f5209eef06 Mon Sep 17 00:00:00 2001 From: Thomas Stastny Date: Tue, 15 Mar 2022 19:12:27 +0100 Subject: [PATCH 2/6] second order ref model: handle min/max time steps, update units / improve documentation, wrap in math namespace --- .../filter/second_order_reference_model.hpp | 70 +++++++++++++++---- .../second_order_reference_model_test.cpp | 1 + 2 files changed, 56 insertions(+), 15 deletions(-) diff --git a/src/lib/mathlib/math/filter/second_order_reference_model.hpp b/src/lib/mathlib/math/filter/second_order_reference_model.hpp index 77efade1e8c3..7f36978af636 100644 --- a/src/lib/mathlib/math/filter/second_order_reference_model.hpp +++ b/src/lib/mathlib/math/filter/second_order_reference_model.hpp @@ -41,6 +41,11 @@ #pragma once +#include + +namespace math +{ + template class SecondOrderReferenceModel { @@ -61,7 +66,8 @@ class SecondOrderReferenceModel /** * Set the system parameters * - * Calculates the damping coefficient and spring stiffness + * Calculates the damping coefficient, spring constant, and maximum allowed + * time step based on the natural frequency. * * @param[in] natural_freq The desired natural frequency of the system [rad/s] * @param[in] damping_ratio The desired damping ratio of the system @@ -69,7 +75,16 @@ class SecondOrderReferenceModel */ bool setParameters(const float natural_freq, const float damping_ratio) { - if (natural_freq < 0.0f || damping_ratio < 0.0f) { + if (natural_freq < FLT_EPSILON || damping_ratio < FLT_EPSILON) { + + // deadzone the resulting constants (will result in zero filter acceleration) + spring_constant_ = 0.0f; + damping_coefficient_ = 0.0f; + + // zero natural frequency means our time step is irrelevant + max_time_step_ = INFINITY; + + // fail return false; } @@ -77,21 +92,24 @@ class SecondOrderReferenceModel spring_constant_ = natural_freq * natural_freq; damping_coefficient_ = 2.0f * damping_ratio * natural_freq; + // Based on *conservative nyquist frequency via SAMPLE_RATE_MULTIPLIER + max_time_step_ = 2.0f * M_PI_F / (natural_freq * SAMPLE_RATE_MULTIPLIER); + return true; } /** - * @return System state + * @return System state [units] */ const T &getState() const { return filter_state_; } /** - * @return System rate + * @return System rate [units/s] */ const T &getRate() const { return filter_rate_; } /** - * @return System acceleration + * @return System acceleration [units/s^2] */ const T &getAccel() const { return filter_accel_; } @@ -103,11 +121,22 @@ class SecondOrderReferenceModel * system parameters. Sampling time should be sufficiently fast. * * @param[in] time_step Time since last sample [s] - * @param[in] state_sample New state sample - * @param[in] rate_sample New rate sample, if provided, otherwise defaults to zero(s) + * @param[in] state_sample New state sample [units] + * @param[in] rate_sample New rate sample, if provided, otherwise defaults to zero(s) [units/s] */ void update(const float time_step, const T &state_sample, const T &rate_sample = T()) { + if (time_step > max_time_step_) { + // time step is too large, reset the filter + reset(state_sample, rate_sample); + return; + } + + if (time_step < 0.0f) { + // erroneous input, dont update + return; + } + T state_error = state_sample - filter_state_; T rate_error = rate_sample - filter_rate_; @@ -119,8 +148,8 @@ class SecondOrderReferenceModel /** * Reset the system states * - * @param[in] state Initial state - * @param[in] rate Initial rate, if provided, otherwise defaults to zero(s) + * @param[in] state Initial state [units] + * @param[in] rate Initial rate, if provided, otherwise defaults to zero(s) [units/s] */ void reset(const T &state, const T &rate = T()) { @@ -131,23 +160,34 @@ class SecondOrderReferenceModel protected: + // A conservative multiplier on sample frequency to bound the maximum time step + static constexpr float SAMPLE_RATE_MULTIPLIER = 10.0f; + + // (effective, no mass) Spring constant for second order system [s^-2] float spring_constant_{0.0f}; + + // (effective, no mass) Damping coefficient for second order system [s^-1] float damping_coefficient_{0.0f}; - T filter_state_{}; - T filter_rate_{}; - T filter_accel_{}; + T filter_state_{}; // [units] + T filter_rate_{}; // [units/s] + T filter_accel_{}; // [units/s^2] + + // Maximum time step [s] + float max_time_step_{INFINITY}; /** * Take one integration step using Euler integration * - * @param[in] last_state - * @param[in] rate + * @param[in] last_state [units] + * @param[in] rate [units/s] * @param[in] time_step Time since last sample [s] - * @return The next state + * @return The next state [units] */ T integrate(const T &last_state, const T &rate, const float time_step) const { return last_state + rate * time_step; } }; + +} // namespace math diff --git a/src/lib/mathlib/math/test/second_order_reference_model_test.cpp b/src/lib/mathlib/math/test/second_order_reference_model_test.cpp index e248131b3989..96a505649cbd 100644 --- a/src/lib/mathlib/math/test/second_order_reference_model_test.cpp +++ b/src/lib/mathlib/math/test/second_order_reference_model_test.cpp @@ -41,6 +41,7 @@ #include #include +using math::SecondOrderReferenceModel; using matrix::Vector3f; TEST(SecondOrderReferenceModel, FloatDefaultConstructorInitializesStatesToZero) From 77b866ee3830ebbe16d74d6fd8b519c65b1a4d01 Mon Sep 17 00:00:00 2001 From: Thomas Stastny Date: Tue, 15 Mar 2022 22:53:00 +0100 Subject: [PATCH 3/6] second order ref model: correct the integration order of filter states --- .../mathlib/math/filter/second_order_reference_model.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/lib/mathlib/math/filter/second_order_reference_model.hpp b/src/lib/mathlib/math/filter/second_order_reference_model.hpp index 7f36978af636..74d89ea3c7be 100644 --- a/src/lib/mathlib/math/filter/second_order_reference_model.hpp +++ b/src/lib/mathlib/math/filter/second_order_reference_model.hpp @@ -137,12 +137,12 @@ class SecondOrderReferenceModel return; } + filter_state_ = integrate(filter_state_, filter_rate_, time_step); + filter_rate_ = integrate(filter_rate_, filter_accel_, time_step); + T state_error = state_sample - filter_state_; T rate_error = rate_sample - filter_rate_; - filter_accel_ = state_error * spring_constant_ + rate_error * damping_coefficient_; - filter_rate_ = integrate(filter_rate_, filter_accel_, time_step); - filter_state_ = integrate(filter_state_, filter_rate_, time_step); } /** From 187844d3e9dee5aabc4ceff1b138948356997dda Mon Sep 17 00:00:00 2001 From: Thomas Stastny Date: Tue, 15 Mar 2022 23:09:23 +0100 Subject: [PATCH 4/6] second order ref model: doc note on sample time abstraction --- .../math/filter/second_order_reference_model.hpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/lib/mathlib/math/filter/second_order_reference_model.hpp b/src/lib/mathlib/math/filter/second_order_reference_model.hpp index 74d89ea3c7be..3cc1a767da8a 100644 --- a/src/lib/mathlib/math/filter/second_order_reference_model.hpp +++ b/src/lib/mathlib/math/filter/second_order_reference_model.hpp @@ -35,6 +35,10 @@ * @file second_order_reference_model.hpp * * @brief Implementation of a second order system with optional rate feed-forward. + * Note that sample time abstraction is not done here. Users should run the filter + * at a sufficiently fast rate to achieve the continuous time performance described + * by the natural frequency and damping ratio parameters. Tuning of these parameters + * will only maintain a given performance for the sampled time for which it was tuned. * * @author Thomas Stastny */ @@ -77,18 +81,18 @@ class SecondOrderReferenceModel { if (natural_freq < FLT_EPSILON || damping_ratio < FLT_EPSILON) { - // deadzone the resulting constants (will result in zero filter acceleration) + // Deadzone the resulting constants (will result in zero filter acceleration) spring_constant_ = 0.0f; damping_coefficient_ = 0.0f; - // zero natural frequency means our time step is irrelevant + // Zero natural frequency means our time step is irrelevant max_time_step_ = INFINITY; - // fail + // Fail return false; } - // note these are "effective" coefficients, as mass coefficient is considered baked in + // Note these are "effective" coefficients, as mass coefficient is considered baked in spring_constant_ = natural_freq * natural_freq; damping_coefficient_ = 2.0f * damping_ratio * natural_freq; From 97b8edfe6f42997b31d73bb7d0445f740e61a0da Mon Sep 17 00:00:00 2001 From: Thomas Stastny Date: Mon, 4 Apr 2022 14:58:30 +0200 Subject: [PATCH 5/6] second order ref model: refactor filter with option for bilinear transformed discretization - use cutoff frequency for max time step calculation - implement forward-euler and bilinear transformed filters with matirx math - store previous samples to update from last step --- .../filter/second_order_reference_model.hpp | 235 +++++++++++++++--- 1 file changed, 200 insertions(+), 35 deletions(-) diff --git a/src/lib/mathlib/math/filter/second_order_reference_model.hpp b/src/lib/mathlib/math/filter/second_order_reference_model.hpp index 3cc1a767da8a..08fd1202f730 100644 --- a/src/lib/mathlib/math/filter/second_order_reference_model.hpp +++ b/src/lib/mathlib/math/filter/second_order_reference_model.hpp @@ -35,10 +35,6 @@ * @file second_order_reference_model.hpp * * @brief Implementation of a second order system with optional rate feed-forward. - * Note that sample time abstraction is not done here. Users should run the filter - * at a sufficiently fast rate to achieve the continuous time performance described - * by the natural frequency and damping ratio parameters. Tuning of these parameters - * will only maintain a given performance for the sampled time for which it was tuned. * * @author Thomas Stastny */ @@ -46,11 +42,12 @@ #pragma once #include +#include namespace math { -template +template class SecondOrderReferenceModel { public: @@ -67,13 +64,26 @@ class SecondOrderReferenceModel setParameters(natural_freq, damping_ratio); } + /** + * Enumeration for available time discretization methods + */ + enum DiscretizationMethod { + kForwardEuler, + kBilinear + }; + + /** + * Set the time discretization method used for state integration + */ + void setDiscretizationMethod(const DiscretizationMethod method) { discretization_method_ = method; } + /** * Set the system parameters * * Calculates the damping coefficient, spring constant, and maximum allowed * time step based on the natural frequency. * - * @param[in] natural_freq The desired natural frequency of the system [rad/s] + * @param[in] natural_freq The desired undamped natural frequency of the system [rad/s] * @param[in] damping_ratio The desired damping ratio of the system * @return Whether or not the param set was successful */ @@ -82,10 +92,11 @@ class SecondOrderReferenceModel if (natural_freq < FLT_EPSILON || damping_ratio < FLT_EPSILON) { // Deadzone the resulting constants (will result in zero filter acceleration) - spring_constant_ = 0.0f; - damping_coefficient_ = 0.0f; + spring_constant_ = FLT_EPSILON; + damping_coefficient_ = FLT_EPSILON; // Zero natural frequency means our time step is irrelevant + cutoff_freq_ = FLT_EPSILON; max_time_step_ = INFINITY; // Fail @@ -96,8 +107,10 @@ class SecondOrderReferenceModel spring_constant_ = natural_freq * natural_freq; damping_coefficient_ = 2.0f * damping_ratio * natural_freq; - // Based on *conservative nyquist frequency via SAMPLE_RATE_MULTIPLIER - max_time_step_ = 2.0f * M_PI_F / (natural_freq * SAMPLE_RATE_MULTIPLIER); + cutoff_freq_ = calculateCutoffFrequency(natural_freq, damping_ratio); + + // Based on *conservative nyquist frequency via kSampleRateMultiplier >= 2 + max_time_step_ = 2.0f * M_PI_F / (cutoff_freq_ * kSampleRateMultiplier); return true; } @@ -121,8 +134,6 @@ class SecondOrderReferenceModel * Update the system states * * Units of rate_sample must correspond to the derivative of state_sample units. - * There is currently no handling of discrete time behavior w.r.t. continuous time - * system parameters. Sampling time should be sufficiently fast. * * @param[in] time_step Time since last sample [s] * @param[in] state_sample New state sample [units] @@ -130,23 +141,21 @@ class SecondOrderReferenceModel */ void update(const float time_step, const T &state_sample, const T &rate_sample = T()) { - if (time_step > max_time_step_) { - // time step is too large, reset the filter + if ((time_step > max_time_step_) || (time_step < 0.0f)) { + // time step is too large or is negative, reset the filter reset(state_sample, rate_sample); return; } - if (time_step < 0.0f) { - // erroneous input, dont update - return; - } + // take a step forward from the last state (and input), update the filter states + integrateStates(time_step, last_state_sample_, last_rate_sample_); - filter_state_ = integrate(filter_state_, filter_rate_, time_step); - filter_rate_ = integrate(filter_rate_, filter_accel_, time_step); + // instantaneous acceleration from current input / state + filter_accel_ = calculateInstantaneousAcceleration(state_sample, rate_sample); - T state_error = state_sample - filter_state_; - T rate_error = rate_sample - filter_rate_; - filter_accel_ = state_error * spring_constant_ + rate_error * damping_coefficient_; + // store the current samples + last_state_sample_ = state_sample; + last_rate_sample_ = rate_sample; } /** @@ -160,37 +169,193 @@ class SecondOrderReferenceModel filter_state_ = state; filter_rate_ = rate; filter_accel_ = T(); + + last_state_sample_ = state; + last_rate_sample_ = rate; } -protected: +private: - // A conservative multiplier on sample frequency to bound the maximum time step - static constexpr float SAMPLE_RATE_MULTIPLIER = 10.0f; + // A conservative multiplier (>=2) on sample frequency to bound the maximum time step + static constexpr float kSampleRateMultiplier = 4.0f; // (effective, no mass) Spring constant for second order system [s^-2] - float spring_constant_{0.0f}; + float spring_constant_{FLT_EPSILON}; // (effective, no mass) Damping coefficient for second order system [s^-1] - float damping_coefficient_{0.0f}; + float damping_coefficient_{FLT_EPSILON}; + + // cutoff frequency [rad/s] + float cutoff_freq_{FLT_EPSILON}; T filter_state_{}; // [units] T filter_rate_{}; // [units/s] T filter_accel_{}; // [units/s^2] + // the last samples need to be stored because we don't know the time step over which we integrate to update to the + // next step a priori + T last_state_sample_{}; // [units] + T last_rate_sample_{}; // [units/s] + // Maximum time step [s] float max_time_step_{INFINITY}; + // The selected time discretization method used for state integration + DiscretizationMethod discretization_method_{kBilinear}; + /** - * Take one integration step using Euler integration + * Calculate the cutoff frequency in terms of undamped natural frequency and damping ratio * - * @param[in] last_state [units] - * @param[in] rate [units/s] - * @param[in] time_step Time since last sample [s] - * @return The next state [units] + * @param[in] natural_freq The desired undamped natural frequency of the system [rad/s] + * @param[in] damping_ratio The desired damping ratio of the system + * @return Cutoff frequency [rad/s] + */ + float calculateCutoffFrequency(const float natural_freq, const float damping_ratio) + { + const float damping_ratio_squared = damping_ratio * damping_ratio; + return natural_freq * sqrtf(1.0f - 2.0f * damping_ratio_squared + sqrtf(4.0f * damping_ratio_squared * + damping_ratio_squared - 4.0f * damping_ratio_squared + 2.0f)); + } + + /** + * Take one integration step using selected discretization method + * + * @param[in] time_step Integration time [s] + * @param[in] state_sample [units] + * @param[in] rate_sample [units/s] + */ + void integrateStates(const float time_step, const T &state_sample, const T &rate_sample) + { + switch (discretization_method_) { + case DiscretizationMethod::kForwardEuler: { + // forward-Euler discretization + integrateStatesForwardEuler(time_step, state_sample, rate_sample); + break; + } + + default: { + // default to bilinear transform + integrateStatesBilinear(time_step, state_sample, rate_sample); + } + } + } + + /** + * Take one integration step using Euler-forward integration + * + * @param[in] time_step Integration time [s] + * @param[in] state_sample [units] + * @param[in] rate_sample [units/s] + */ + void integrateStatesForwardEuler(const float time_step, const T &state_sample, const T &rate_sample) + { + // general notation for what follows: + // c: continuous + // d: discrete + // Kx: spring constant + // Kv: damping coefficient + // T: sample time + + // state matrix + // Ac = [ 0 1 ] + // [-Kx -Kv] + // Ad = I + Ac * T + matrix::SquareMatrix state_matrix; + state_matrix(0, 0) = 1.0f; + state_matrix(0, 1) = time_step; + state_matrix(1, 0) = -spring_constant_ * time_step; + state_matrix(1, 1) = -damping_coefficient_ * time_step + 1.0f; + + // input matrix + // Bc = [0 0 ] + // [Kx Kv] + // Bd = Bc * T + matrix::SquareMatrix input_matrix; + input_matrix(0, 0) = 0.0f; + input_matrix(0, 1) = 0.0f; + input_matrix(1, 0) = spring_constant_ * time_step; + input_matrix(1, 1) = damping_coefficient_ * time_step; + + // discrete state transition + transitionStates(state_matrix, input_matrix, state_sample, rate_sample); + } + + /** + * Take one integration step using discrete state space calculated from bilinear transform + * + * @param[in] time_step Integration time [s] + * @param[in] state_sample [units] + * @param[in] rate_sample [units/s] + */ + void integrateStatesBilinear(const float time_step, const T &state_sample, const T &rate_sample) + { + const float time_step_squared = time_step * time_step; + const float inv_denominator = 1.0f / (0.25f * spring_constant_ * time_step_squared + 0.5f * damping_coefficient_ * + time_step + 1.0f); + + // general notation for what follows: + // c: continuous + // d: discrete + // Kx: spring constant + // Kv: damping coefficient + // T: sample time + + // state matrix + // Ac = [ 0 1 ] + // [-Kx -Kv] + // Ad = (I + 1/2 * Ac * T) * (I - 1/2 * Ac * T)^-1 + matrix::SquareMatrix state_matrix; + state_matrix(0, 0) = -0.25f * spring_constant_ * time_step_squared + 0.5f * damping_coefficient_ * time_step + 1.0f; + state_matrix(0, 1) = time_step; + state_matrix(1, 0) = -spring_constant_ * time_step; + state_matrix(1, 1) = -0.25f * spring_constant_ * time_step_squared - 0.5f * damping_coefficient_ * time_step + 1.0f; + state_matrix *= inv_denominator; + + // input matrix + // Bc = [0 0 ] + // [Kx Kv] + // Bd = Ac^-1 * (Ad - I) * Bc + matrix::SquareMatrix input_matrix; + input_matrix(0, 0) = 0.5f * spring_constant_ * time_step_squared; + input_matrix(0, 1) = 0.5f * damping_coefficient_ * time_step_squared; + input_matrix(1, 0) = spring_constant_ * time_step; + input_matrix(1, 1) = damping_coefficient_ * time_step; + input_matrix *= inv_denominator; + + // discrete state transition + transitionStates(state_matrix, input_matrix, state_sample, rate_sample); + } + + /** + * Transition the states using discrete state space matrices + * + * @param[in] state_matrix Discrete state matrix (2x2) + * @param[in] input_matrix Discrete input matrix (2x2) + * @param[in] state_sample [units] + * @param[in] rate_sample [units/s] + */ + void transitionStates(const matrix::SquareMatrix &state_matrix, + const matrix::SquareMatrix &input_matrix, const T &state_sample, const T &rate_sample) + { + const T new_state = state_matrix(0, 0) * filter_state_ + state_matrix(0, 1) * filter_rate_ + input_matrix(0, + 0) * state_sample + input_matrix(0, 1) * rate_sample; + const T new_rate = state_matrix(1, 0) * filter_state_ + state_matrix(1, 1) * filter_rate_ + input_matrix(1, + 0) * state_sample + input_matrix(1, 1) * rate_sample; + filter_state_ = new_state; + filter_rate_ = new_rate; + } + + /** + * Calculate the instantaneous acceleration of the system + * + * @param[in] state_sample [units] + * @param[in] rate_sample [units/s] */ - T integrate(const T &last_state, const T &rate, const float time_step) const + T calculateInstantaneousAcceleration(const T &state_sample, const T &rate_sample) const { - return last_state + rate * time_step; + const T state_error = state_sample - filter_state_; + const T rate_error = rate_sample - filter_rate_; + return state_error * spring_constant_ + rate_error * damping_coefficient_; } }; From f1851048196bc77ddf1af3b67f954b2450118458 Mon Sep 17 00:00:00 2001 From: Thomas Stastny Date: Mon, 4 Apr 2022 17:10:27 +0200 Subject: [PATCH 6/6] Update src/lib/mathlib/math/test/second_order_reference_model_test.cpp Co-authored-by: Mathieu Bresciani --- src/lib/mathlib/math/test/second_order_reference_model_test.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/mathlib/math/test/second_order_reference_model_test.cpp b/src/lib/mathlib/math/test/second_order_reference_model_test.cpp index 96a505649cbd..fd1638f6ea59 100644 --- a/src/lib/mathlib/math/test/second_order_reference_model_test.cpp +++ b/src/lib/mathlib/math/test/second_order_reference_model_test.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2022 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