Implement ButterworthLowPassFilter

CHANGELOG.md

@@ -13,10 +13,11 @@ All notable changes to this project are documented in this file.
 - Add getJointTorques to python binding in SensorBridge (https://github.com/ami-iit/bipedal-locomotion-framework/pull/825)
 - Add `System::TimeProfiler` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/826)
 - Add the possibility to programmatically build a `QPTSID` problem from the content of a `ParametersHandler` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/828)
-- Add support for testing if a portion of code allocates memory via `MemoryAllocationMonitor`` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/768)
+- Add support for testing if a portion of code allocates memory via `MemoryAllocationMonitor` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/768)
 - Implement `Math::ZeroOrderSpline` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/834)
 - Add the possibility to get only position or position/velocity from the spline (https://github.com/ami-iit/bipedal-locomotion-framework/pull/834)
 - Add the possibility to set the number of threads used by onnxruntime in `MANN` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/836)
+- Implement `ButterworthLowPassFilter` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/838)
 
 ### Changed
 - 🤖 [ergoCubSN001] Add logging of the wrist and fix the name of the waist imu (https://github.com/ami-iit/bipedal-locomotion-framework/pull/810)

src/ContinuousDynamicalSystem/CMakeLists.txt

@@ -17,10 +17,12 @@ if(FRAMEWORK_COMPILE_ContinuousDynamicalSystem)
                            ${H_PREFIX}/Integrator.h  ${H_PREFIX}/FixedStepIntegrator.h ${H_PREFIX}/ForwardEuler.h ${H_PREFIX}/RK4.h
                            ${H_PREFIX}/CompliantContactWrench.h
                            ${H_PREFIX}/MultiStateWeightProvider.h
+                           ${H_PREFIX}/ButterworthLowPassFilter.h
     PRIVATE_HEADERS        ${H_PREFIX}/impl/traits.h
     SOURCES                src/LinearTimeInvariantSystem.cpp src/FloatingBaseSystemKinematics.cpp src/CompliantContactWrench.cpp
                            src/FloatingBaseDynamicsWithCompliantContacts.cpp src/FixedBaseDynamics.cpp src/CentroidalDynamics.cpp
                            src/FirstOrderSmoother.cpp src/MultiStateWeightProvider.cpp
+                           src/ButterworthLowPassFilter.cpp
     PUBLIC_LINK_LIBRARIES  BipedalLocomotion::ParametersHandler BipedalLocomotion::ContactModels BipedalLocomotion::System
                            iDynTree::idyntree-high-level iDynTree::idyntree-model
                            Eigen3::Eigen BipedalLocomotion::TextLogging BipedalLocomotion::Math BipedalLocomotion::Contacts

src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/ButterworthLowPassFilter.h

@@ -0,0 +1,165 @@
+/**
+ * @file ButterworthLowPassFilter.h
+ * @authors Giulio Romualdi
+ * @copyright 2024 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the BSD-3-Clause license.
+ */
+
+#ifndef BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_BUTTERWORTH_LOW_PASS_FILTER_H
+#define BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_BUTTERWORTH_LOW_PASS_FILTER_H
+
+#include <memory>
+
+#include <BipedalLocomotion/ContinuousDynamicalSystem/ForwardEuler.h>
+#include <BipedalLocomotion/ContinuousDynamicalSystem/LinearTimeInvariantSystem.h>
+#include <BipedalLocomotion/System/Advanceable.h>
+
+#include <Eigen/Dense>
+
+namespace BipedalLocomotion
+{
+namespace ContinuousDynamicalSystem
+{
+
+/**
+ * ButterworthLowPass implements a low pass filter of order N.
+ *
+ * The system is described by the following transfer function
+ * \f[
+ * H(s) = \frac{1}{\sqrt{1 + \left(\frac{s}{\omega_c}\right)^{2N}}}
+ * \f]
+ * where \f$\omega_c\f$ is the cutoff frequency and \f$N\f$ is the order of the filter and \f$s\f$
+ * is the Laplace variable.
+ *
+ * To compute the coefficient of the filter we split the problem in three steps:
+ *  -# Compute the transfer function of the continuous system.
+ *  -# Compute the transfer function of the discrete system.
+ *  -# Compute the coefficients of the discrete system.
+ *
+ * @section Compute the transfer function of the continuous system
+ * What follows is taken from from Passive and Active Network Analysis and Synthesis, Aram Budak,
+ * Houghton Mifflin, 1974 and from
+ * https://dsp.stackexchange.com/questions/79498/butterworth-filter-poles
+ * The poles of the Butterworth filter  are evenly spaced on a circle of radius \f$\omega_c\f$ in
+ * the s-plane. The poles are given by
+ * \f[
+ * p_k = \omega_c e^{j \frac{\pi}{2} \left(1 + \frac{2k - 1}{2N}\right)}
+ * \f]
+ * where \f$k = 0, 1, \ldots, N-1\f$ and \f$j\f$ is the imaginary unit.
+ *  By construction, the Butterworth filter does not have zeros.
+ * The gain of the filter is  given by
+ * \f[
+ * K = \prod_{k=0}^{N-1} s_k = \omega_c^N
+ * \f]
+ * @section Compute the transfer function of the discrete system
+ * As mentioned before, the transfer function of the discrete system is obtained by the bilinear
+ * transform
+ * \f[
+ * s = \frac{2}{\delta t} \frac{1 - z^{-1}}{1 + z^{-1}}
+ * \f]
+ * The poles of the discrete system are obtained by substituting the poles of the continuous system
+ * in the bilinear transformation as explained in
+ * https://it.mathworks.com/help/signal/ref/bilinear.html
+ * The poles of the discrete system are given by
+ * \f[
+ * p^d_k = \frac{1 + p_k \delta t/2}{1 - p_k \delta t/2}
+ * \f]
+ * where \f$p_k\f$ are the poles of the continuous system, \f$\delta t\f$ is the sampling time and
+ * \f$k = 0, 1, \ldots, N-1\f$.
+ * All the zeros of the continuous system are mapped to -1.
+ * Finally, the gain of the discrete system is given by
+ * \f[
+ * K^d = \text{real}  \frac{K}{ \prod (\frac{2}{\delta T} - p_k) }
+ * \f]
+ * @section Compute the coefficients of the discrete system
+ * Once we have the poles and the gain of the discrete system we can compute the coefficients of the
+ * filter by applying the [Vieta's formulas](https://en.wikipedia.org/wiki/Vieta%27s_formulas).
+ * The transfer function of the discrete system is given by
+ * \f[
+ * H(z) = \frac{a_n + a_{n-1} z^{-1} + \ldots + a_1 z^{-n+1} + a_0 z^{-n}}{1 + b_{n-1} z^{-1} + \ldots + b_1 z^{-n+1} + b_0 z^{-n}}
+ * \f]
+ * Once the numerator and the denominator are computed we can easily antitransform the transfer function
+ * to obtain the coefficients of the filter as
+ * \[
+ * y[k] = \frac{1}{b_0} \left( a_0 x[k] + a_1 x[k-1] + \ldots + a_n x[k-n] - b_1 y[k-1] - \ldots - b_n y[k-n] \right)
+ * \]
+ * where \f$x[k]\f$ is the input of the filter and \f$y[k]\f$ is the output of the filter.
+ */
+class ButterworthLowPassFilter
+    : public BipedalLocomotion::System::Advanceable<Eigen::VectorXd, Eigen::VectorXd>
+{
+public:
+    /**
+     * Constructor
+     */
+    ButterworthLowPassFilter();
+
+    /**
+     * Destructor
+     */
+    ~ButterworthLowPassFilter() override;
+
+    // clang-format off
+    /**
+     * Initialize the Dynamical system.
+     * @param handler pointer to the parameter handler.
+     * @note the following parameters are required
+     * |   Parameter Name   |   Type   |                   Description                    | Mandatory |
+     * |:------------------:|:--------:|:------------------------------------------------:|:---------:|
+     * |  `sampling_time`   | `double` | Sampling time used to discrete the linear system |    Yes    |
+     * |  `cutoff_frequency`| `double` | Cutoff frequency of the low pass filter.         |    Yes    |
+     * |      `order`       |  `int`   |      Order of the low pass filter.               |    Yes    |
+     * @return true in case of success/false otherwise.
+     */
+    bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler) override;
+    // clang-format on
+
+    /**
+     * Set the state of the smoother.
+     * @param initialState initial state of the smoother
+     * @return true in case of success, false otherwise.
+     */
+    bool reset(Eigen::Ref<const Eigen::VectorXd> initialState);
+
+    /**
+     * Compute the output of the filter.
+     * @return true in case of success, false otherwise.
+     * @note The function update also the internal state of the filter.
+     */
+    bool advance() override;
+
+    /**
+     * Get the output of the filter.
+     * @return a vector containing the output of the smoother
+     */
+    const Eigen::VectorXd& getOutput() const override;
+
+    /**
+     * Set the input of the filter
+     * @param input the vector representing the input of the filter
+     * @return True in case of success and false otherwise
+     */
+    bool setInput(const Eigen::VectorXd& input) override;
+
+    /**
+     *  Set the input of the filter
+     * @param input the vector representing the input of the filter
+     * @return True in case of success and false otherwise
+     */
+    bool setInput(Eigen::Ref<const Eigen::VectorXd> input);
+
+    /**
+     * Determines the validity of the object retrieved with getOutput()
+     * @return True if the object is valid, false otherwise.
+     */
+    bool isOutputValid() const override;
+
+private:
+    struct Impl; /**< Pimpl idiom */
+    std::unique_ptr<Impl> m_pimpl; /**< Pointer to the implementation */
+};
+
+} // namespace ContinuousDynamicalSystem
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_BUTTERWORTH_LOW_PASS_FILTER_H