From e4b11c49c36d76ab19f2acf51ca7b7746abeef93 Mon Sep 17 00:00:00 2001 From: Thomas Stastny Date: Tue, 5 Apr 2022 10:31:41 +0200 Subject: [PATCH] mathlib: add second order reference model filter with optional rate feedback (#19246) mathlib: add second order reference model filter with optional rate feedback (#19246) Reference models can be used as filters which exhibit a particular, chosen (reference) dynamic behavior. This PR implements a simple second order transfer function which can be used as such a reference model, additionally with rate feedback. The system is parameterized by explicitly set natural frequency and damping ratio. Another nice externality is that the output state and rate are kinematically consistent. Forward-euler and bilinear transform discretizations for the state space integration step are available. --- src/lib/mathlib/CMakeLists.txt | 2 + .../filter/second_order_reference_model.hpp | 362 ++++++++++++++++++ .../second_order_reference_model_test.cpp | 132 +++++++ 3 files changed, 496 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 d5e7d9e046..3afabfc860 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 0000000000..08fd1202f7 --- /dev/null +++ b/src/lib/mathlib/math/filter/second_order_reference_model.hpp @@ -0,0 +1,362 @@ +/**************************************************************************** + * + * 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 + +#include +#include + +namespace math +{ + +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); + } + + /** + * 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 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 + */ + bool setParameters(const float natural_freq, const float damping_ratio) + { + if (natural_freq < FLT_EPSILON || damping_ratio < FLT_EPSILON) { + + // Deadzone the resulting constants (will result in zero filter acceleration) + 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 + 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; + + 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; + } + + /** + * @return System state [units] + */ + const T &getState() const { return filter_state_; } + + /** + * @return System rate [units/s] + */ + const T &getRate() const { return filter_rate_; } + + /** + * @return System acceleration [units/s^2] + */ + const T &getAccel() const { return filter_accel_; } + + /** + * Update the system states + * + * Units of rate_sample must correspond to the derivative of state_sample units. + * + * @param[in] time_step Time since last sample [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 < 0.0f)) { + // time step is too large or is negative, reset the filter + reset(state_sample, rate_sample); + return; + } + + // take a step forward from the last state (and input), update the filter states + integrateStates(time_step, last_state_sample_, last_rate_sample_); + + // instantaneous acceleration from current input / state + filter_accel_ = calculateInstantaneousAcceleration(state_sample, rate_sample); + + // store the current samples + last_state_sample_ = state_sample; + last_rate_sample_ = rate_sample; + } + + /** + * Reset the system states + * + * @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()) + { + filter_state_ = state; + filter_rate_ = rate; + filter_accel_ = T(); + + last_state_sample_ = state; + last_rate_sample_ = rate; + } + +private: + + // 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_{FLT_EPSILON}; + + // (effective, no mass) Damping coefficient for second order system [s^-1] + 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}; + + /** + * Calculate the cutoff frequency in terms of undamped natural frequency and damping ratio + * + * @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 calculateInstantaneousAcceleration(const T &state_sample, const T &rate_sample) const + { + 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_; + } +}; + +} // 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 new file mode 100644 index 0000000000..fd1638f6ea --- /dev/null +++ b/src/lib/mathlib/math/test/second_order_reference_model_test.cpp @@ -0,0 +1,132 @@ +/**************************************************************************** + * + * 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 + * 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 math::SecondOrderReferenceModel; +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); +}