Implement the prewrapping in the butterworth filter (#841)

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

@@ -31,12 +31,9 @@ namespace ContinuousDynamicalSystem
  * 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.
+ * What follows is a brief description of the filter and how it is implemented.
  *
- * @section Compute the transfer function of the continuous system
+ * @section ButterworthLowPassFilter_continuous 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
@@ -51,7 +48,7 @@ namespace ContinuousDynamicalSystem
  * \f[
  * K = \prod_{k=0}^{N-1} s_k = \omega_c^N
  * \f]
- * @section Compute the transfer function of the discrete system
+ * @section ButterworthLowPassFilter_discrete 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[
@@ -71,7 +68,20 @@ namespace ContinuousDynamicalSystem
  * \f[
  * K^d = \text{real} \frac{K}{ \prod (\frac{2}{\delta T} - p_k) }
  * \f]
- * @section Compute the coefficients of the discrete system
+ * @subsection ButterworthLowPassFilter_prewrapping Pre-wrapping
+ * The ButterworthLowPass supports the pre-wrapping of the filter. The pre-wrapping is a technique
+ * used to mitigate the distortion that can occur during the bilinear transformation. It
+ * consists in shifting the poles of the continuous system in the s-plane.
+ * To easily implement the pre-wrapping, we slightly modify the bilinear transformation as
+ * \f[
+ * s = \frac{\omega_c}{\tan\left(\frac{\omega_c \delta t}{2}\right)} \frac{1 - z^{-1}}{1 + z^{-1}}
+ * \f]
+ * where \f$\omega_c\f$ is the cutoff frequency and \f$\delta t\f$ is the sampling time.
+ * In the class the pre-wrapping is enabled by default and can be disabled by setting the parameter
+ * `enable_prewrapping` to false.
+ * The interested reader can find more information about the pre-wrapping at
+ * [this link](https://en.wikipedia.org/wiki/Bilinear_transform#Frequency_warping).
+ * @section ButterworthLowPassFilter_coefficients 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
@@ -80,9 +90,9 @@ namespace ContinuousDynamicalSystem
  * \f]
  * Once the numerator and the denominator are computed we can easily antitransform the transfer function
  * to obtain the coefficients of the filter as
- * \[
+ * \f[
  * 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)
- * \]
+ * \f]
  * where \f$x[k]\f$ is the input of the filter and \f$y[k]\f$ is the output of the filter.
  */
 class ButterworthLowPassFilter
@@ -104,11 +114,12 @@ class ButterworthLowPassFilter
  * 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 |
+ * | 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 |
+ * |`enable_prewrapping`| `bool` | Enable the pre-wrapping of the filter. (Default value True). | No |
  * @return true in case of success/false otherwise.
  */
  bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler) override;
@@ -141,13 +152,6 @@ class ButterworthLowPassFilter
  */
  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.

src/ContinuousDynamicalSystem/src/ButterworthLowPassFilter.cpp

@@ -35,6 +35,7 @@ struct ButterworthLowPassFilter::Impl
  std::deque<Eigen::VectorXd> outputBuffer;
 
  Eigen::VectorXd output;
+ double bilinearTransformationFactor{0.0};
 
  bool isOutputValid{false};
 
@@ -62,7 +63,7 @@ struct ButterworthLowPassFilter::Impl
 
  void bilinearTransformation()
  {
- const double k = 2 * this->frequency;
+ const double k = bilinearTransformationFactor;
  // bilinear transformation
  for (const auto& pole : this->continuousPoles)
  {
@@ -106,7 +107,7 @@ struct ButterworthLowPassFilter::Impl
 
 ButterworthLowPassFilter::ButterworthLowPassFilter()
 {
- this->m_pimpl = std::make_unique<Impl>();
+ m_pimpl = std::make_unique<Impl>();
 }
 
 ButterworthLowPassFilter::~ButterworthLowPassFilter() = default;
@@ -128,7 +129,8 @@ bool ButterworthLowPassFilter::initialize(
  log()->error("{} Unable to get the parameter named 'sampling_time'.", logPrefix);
  return false;
  }
- m_pimpl->frequency = 1.0 / std::chrono::duration<double>(samplingTime).count();
+ const double dt = std::chrono::duration<double>(samplingTime).count();
+ m_pimpl->frequency = 1.0 / dt;
 
  double cutOffFrequency;
  if (!ptr->getParameter("cutoff_frequency", cutOffFrequency))
@@ -138,12 +140,27 @@ bool ButterworthLowPassFilter::initialize(
  }
  m_pimpl->cutOffPulsation = 2 * M_PI * cutOffFrequency;
 
- if (!ptr->getParameter("order", m_pimpl->order))
+ if (!ptr->getParameter("order", m_pimpl->order) || m_pimpl->order < 1)
  {
- log()->error("{} Unable to get the parameter named 'order'.", logPrefix);
+ log()->error("{} Unable to get the parameter named 'order'. The order must be greater than "
+ "or equal to 1.",
+ logPrefix);
  return false;
  }
 
+ bool enablePrewrapping{true};
+ if (!ptr->getParameter("enable_prewrapping", enablePrewrapping))
+ {
+ log()->info("{} Unable to get the parameter named 'enable_prewrapping'. Using the default "
+ "value {}.",
+ logPrefix,
+ enablePrewrapping);
+ }
+
+ m_pimpl->bilinearTransformationFactor
+ = enablePrewrapping ? m_pimpl->cutOffPulsation / tan((m_pimpl->cutOffPulsation * dt / 2.0))
+ : 2.0 * m_pimpl->frequency;
+
  // compute the poles and the gain of the continuous system
  m_pimpl->computePole();
 
@@ -181,7 +198,7 @@ bool ButterworthLowPassFilter::reset(Eigen::Ref<const Eigen::VectorXd> initialSt
  return true;
 }
 
-bool ButterworthLowPassFilter::setInput(Eigen::Ref<const Eigen::VectorXd> input)
+bool ButterworthLowPassFilter::setInput(const Eigen::VectorXd& input)
 {
  constexpr auto logPrefix = "[ButterworthLowPassFilter::setInput]";
  if (m_pimpl->inputBuffer.empty() || m_pimpl->outputBuffer.empty())
@@ -203,11 +220,6 @@ bool ButterworthLowPassFilter::setInput(Eigen::Ref<const Eigen::VectorXd> input)
  return true;
 }
 
-bool ButterworthLowPassFilter::setInput(const Eigen::VectorXd& input)
-{
- return this->setInput(Eigen::Ref<const Eigen::VectorXd>(input));
-}
-
 bool ButterworthLowPassFilter::advance()
 {
  constexpr auto logPrefix = "[ButterworthLowPassFilter::advance]";

src/ContinuousDynamicalSystem/tests/ButterworthLowPassFilter.cpp

@@ -29,21 +29,41 @@ TEST_CASE("ButterworthLowPass filter")
  paramHandler->setParameter("cutoff_frequency", cutOffFrequency);
  paramHandler->setParameter("sampling_time", dT);
 
- ButterworthLowPassFilter butterworthLowPass;
- REQUIRE(butterworthLowPass.initialize(paramHandler));
-
  Eigen::VectorXd input = Eigen::VectorXd::Zero(2);
-
- REQUIRE(butterworthLowPass.reset(input));
-
  input << 1.0, 1.0;
 
+ constexpr double tolerance = 1e-3;
  constexpr std::chrono::nanoseconds simulationTime = 2s;
- for (std::chrono::nanoseconds t = 0ns; t < simulationTime; t += dT)
+
+ SECTION("Prewrapping disabled")
+ {
+ ButterworthLowPassFilter butterworthLowPass;
+ paramHandler->setParameter("enable_prewrapping", false);
+ REQUIRE(butterworthLowPass.initialize(paramHandler));
+ REQUIRE(butterworthLowPass.reset(Eigen::Vector2d::Zero()));
+
+ for (std::chrono::nanoseconds t = 0ns; t < simulationTime; t += dT)
+ {
+ REQUIRE(butterworthLowPass.setInput(input));
+ REQUIRE(butterworthLowPass.advance());
+ }
+
+ REQUIRE(butterworthLowPass.getOutput().isApprox(input, tolerance));
+ }
+
+ SECTION("Prewrapping enabled")
  {
- REQUIRE(butterworthLowPass.setInput(input));
- REQUIRE(butterworthLowPass.advance());
+ ButterworthLowPassFilter butterworthLowPass;
+ paramHandler->setParameter("enable_prewrapping", true);
+ REQUIRE(butterworthLowPass.initialize(paramHandler));
+ REQUIRE(butterworthLowPass.reset(Eigen::Vector2d::Zero()));
+
+ for (std::chrono::nanoseconds t = 0ns; t < simulationTime; t += dT)
+ {
+ REQUIRE(butterworthLowPass.setInput(input));
+ REQUIRE(butterworthLowPass.advance());
+ }
 
- std::cout << butterworthLowPass.getOutput().transpose() << std::endl;
+ REQUIRE(butterworthLowPass.getOutput().isApprox(input, tolerance));
  }
 }