feat: added the Butterworth filter class. (autowarefoundation#2113)

* added the Butterworth filter class.

Signed-off-by: ali <boyali@gmail.com>

* added the Butterworth filter class: typo in frequency fixed.

Signed-off-by: ali <boyali@gmail.com>

* added the Butterworth filter class: ReadMe and documentations are updated.

Signed-off-by: ali <boyali@gmail.com>

* removed unused comments.

Signed-off-by: ali <boyali@gmail.com>

* removed unused comments.

Signed-off-by: ali <boyali@gmail.com>

* added new line.

Signed-off-by: ali <boyali@gmail.com>

* a type in the butterworth_filter_test.cpp is fixed.

Signed-off-by: ali <boyali@gmail.com>

* a type in the butterworth_filter_test.cpp is fixed.

Signed-off-by: ali <boyali@gmail.com>

* a typo fixed in the ButterworthFilter.md.

Signed-off-by: ali <boyali@gmail.com>

* // print("Phase angle x = ", x); comment is removed.

Signed-off-by: ali <boyali@gmail.com>

* Test fixture is cleaned.

Signed-off-by: ali <boyali@gmail.com>

* for loop implementations changed.

Signed-off-by: ali <boyali@gmail.com>

* linted.

Signed-off-by: ali <boyali@gmail.com>

* early return.

Signed-off-by: ali <boyali@gmail.com>

* commented cmakline removed.

Signed-off-by: ali <boyali@gmail.com>

* converting cout to ros log.

Signed-off-by: ali <boyali@gmail.com>

* converting cout to ros log for root printing.

converting cout to ros log for root printing.

converting cout to ros log for root printing.

Signed-off-by: ali <boyali@gmail.com>

* const <--> auto, int, double

Signed-off-by: ali <boyali@gmail.com>

* added signed-off commit.

Signed-off-by: ali <boyali@gmail.com>

* Typo correction. Continuous

Signed-off-by: ali <boyali@gmail.com>

* Early return in computeDiscreteTimeTF.

Signed-off-by: ali <boyali@gmail.com>

* Reducing code smell primitive obsession.

Signed-off-by: ali <boyali@gmail.com>

* Reducing code smell primitive obsession.

Signed-off-by: ali <boyali@gmail.com>

* Reducing code smell primitive obsession.

Signed-off-by: ali <boyali@gmail.com>

* typo correction.

Signed-off-by: ali <boyali@gmail.com>

* Typo and copyright correction.

Signed-off-by: ali <boyali@gmail.com>

* Applied pre-commit.

Signed-off-by: ali <boyali@gmail.com>

* Applied pre-commit.

Signed-off-by: ali <boyali@gmail.com>

* Spell-check fix.

Signed-off-by: ali <boyali@gmail.com>

* Spell-check fix.

Signed-off-by: ali <boyali@gmail.com>

Signed-off-by: ali <boyali@gmail.com>
Signed-off-by: Kotaro Yoshimoto <pythagora.yoshimoto@gmail.com>

common/signal_processing/CMakeLists.txt

@@ -4,19 +4,21 @@ project(signal_processing)
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
-ament_auto_add_library(lowpass_filter_1d SHARED
+ament_auto_add_library(lowpass_filters SHARED
   src/lowpass_filter_1d.cpp
   src/lowpass_filter.cpp
-)
+  src/butterworth.cpp)
 
 if(BUILD_TESTING)
   ament_add_ros_isolated_gtest(test_signal_processing
     test/src/lowpass_filter_1d_test.cpp
     test/src/lowpass_filter_test.cpp
-  )
+    test/src/butterworth_filter_test.cpp)
+
+  target_include_directories(test_signal_processing PUBLIC test/include)
   target_link_libraries(test_signal_processing
-    lowpass_filter_1d
-  )
+    lowpass_filters)
+
 endif()
 
 ament_auto_package()

common/signal_processing/README.md

@@ -1,4 +1,10 @@
-# signal_processing
+# Signal Processing Methods
+
+In this package, we present signal processing related methods for the Autoware applications. The following
+functionalities are available in the current version.
+
+- an 1-D Low-pass filter,
+- [Butterworth low-pass filter tools.](documentation/ButterworthFilter.md)
 
 low-pass filter currently supports only the 1-D low pass filtering.
 

common/signal_processing/documentation/ButterworthFilter.md

@@ -0,0 +1,124 @@
+### Butterworth Low-pass Filter Design Tool Class
+
+This Butterworth low-pass filter design tool can be used to design a Butterworth filter in continuous and discrete-time
+from the given specifications of the filter performance. The Butterworth filter is a class implementation. A default
+constructor creates the object without any argument.
+
+The filter can be prepared in three ways. If the filter specifications are known, such as the pass-band, and stop-band
+frequencies (Wp and Ws) together with the pass-band and stop-band ripple magnitudes (Ap and As), one can call the
+filter's buttord method with these arguments to obtain the recommended filter order (N) and cut-off frequency
+(Wc_rad_sec [rad/s]).
+
+![img.png](img.png)
+Figure 1. Butterworth Low-pass filter specification from [1].
+
+An example call is demonstrated below;
+
+    ButterworthFilter bf();
+
+    Wp = 2.0; // pass-band frequency [rad/sec]
+    Ws = 3.0; // stop-band frequency [rad/sec]
+    Ap = 6.0; // pass-band ripple mag or loss [dB]
+    As = 20.0; // stop band ripple attenuation [dB]
+
+    // Computing filter coefficients from the specs
+    bf.Buttord(Wp, Ws, Ap, As);
+
+    // Get the computed order and Cut-Off frequency
+    sOrderCutOff NWc = bf.getOrderCutOff();]
+
+    cout << " The computed order is ;" << NWc.N << endl;
+    cout << " The computed cut-off frequency is ;" << NWc.Wc_rad_sec << endl;
+
+The filter order and cut-off frequency can be obtained in a struct using bf.getOrderCutoff() method. These specs can be
+printed on the screen by calling PrintFilterSpecs() method. If the user would like to define the order and cut-off
+frequency manually, the setter methods for these variables can be called to set the filter order (N) and the desired
+cut-off frequency (Wc_rad_sec [rad/sec]) for the filter.
+
+#### Obtaining Filter Transfer Functions
+
+The discrete transfer function of the filter requires the roots and gain of the continuous-time transfer function.
+Therefore, it is a must to call the first computeContinuousTimeTF() to create the continuous-time transfer function
+of the filter using;
+
+    bf.computeContinuousTimeTF();
+
+The computed continuous-time transfer function roots can be printed on the screen using the methods;
+
+    bf.PrintFilter_ContinuousTimeRoots();
+    bf.PrintContinuousTimeTF();
+
+The resulting screen output for a 5th order filter is demonstrated below.
+
+     Roots of Continuous Time Filter Transfer Function Denominator are :
+    -0.585518 + j 1.80204
+    -1.53291 + j 1.11372
+    -1.89478 + j 2.32043e-16
+    -1.53291 + j -1.11372
+    -0.585518 + j -1.80204
+
+
+    The Continuous-Time Transfer Function of the Filter is ;
+
+                                       24.422
+    -------------------------------------------------------------------------------
+    1.000 *s[5] + 6.132 *s[4] + 18.798 *s[3] + 35.619 *s[2] + 41.711 *s[1] + 24.422
+
+#### Discrete Time Transfer Function (Difference Equations)
+
+The digital filter equivalent of the continuous-time definitions is produced by using the bi-linear transformation.
+When creating the discrete-time function of the ButterworthFilter object, its Numerator (Bn) and Denominator (An
+) coefficients are stored in a vector of filter order size N.
+
+The discrete transfer function method is called using ;
+
+    bf.computeDiscreteTimeTF();
+    bf.PrintDiscreteTimeTF();
+
+The results are printed on the screen like;
+The Discrete-Time Transfer Function of the Filter is ;
+
+    0.191 *z[-5] + 0.956 *z[-4] + 1.913 *z[-3] + 1.913 *z[-2] + 0.956 *z[-1] + 0.191
+    --------------------------------------------------------------------------------
+    1.000 *z[-5] + 1.885 *z[-4] + 1.888 *z[-3] + 1.014 *z[-2] + 0.298 *z[-1] + 0.037
+
+and the associated difference coefficients An and Bn by withing a struct ;
+
+    sDifferenceAnBn AnBn = bf.getAnBn();
+
+The difference coefficients appear in the filtering equation in the form of.
+
+    An * Y_filtered = Bn * Y_unfiltered
+
+To filter a signal given in a vector form ;
+
+#### Calling Filter by a specified cut-off and sampling frequencies [in Hz]
+
+The Butterworth filter can also be created by defining the desired order (N), a cut-off frequency (fc in [Hz]), and a
+sampling frequency (fs in [Hz]). In this method, the cut-off frequency is pre-warped with respect to the sampling
+frequency [1, 2] to match the continuous and digital filter frequencies.
+
+The filter is prepared by the following calling options;
+
+     // 3rd METHOD defining a sampling frequency together with the cut-off fc, fs
+     bf.setOrder(2);
+     bf.setCutOffFrequency(10, 100);
+
+At this step, we define a boolean variable whether to use the pre-warping option or not.
+
+    // Compute Continuous Time TF
+    bool use_sampling_frequency = true;
+    bf.computeContinuousTimeTF(use_sampling_frequency);
+    bf.PrintFilter_ContinuousTimeRoots();
+    bf.PrintContinuousTimeTF();
+
+    // Compute Discrete Time TF
+    bf.computeDiscreteTimeTF(use_sampling_frequency);
+    bf.PrintDiscreteTimeTF();
+
+**References:**
+
+1. Manolakis, Dimitris G., and Vinay K. Ingle. Applied digital signal processing: theory and practice. Cambridge
+   University Press, 2011.
+
+2. <https://en.wikibooks.org/wiki/Digital_Signal_Processing/Bilinear_Transform>

common/signal_processing/include/signal_processing/butterworth.hpp

@@ -0,0 +1,137 @@
+// Copyright 2022 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef SIGNAL_PROCESSING__BUTTERWORTH_HPP_
+#define SIGNAL_PROCESSING__BUTTERWORTH_HPP_
+
+#include <cmath>
+#include <complex>
+#include <iomanip>
+#include <iostream>
+#include <vector>
+
+template <typename T>
+const T & append_separator(const T & arg)
+{
+  std::cout << " ";
+  return arg;
+}
+
+template <typename... Args>
+void print(Args &&... args)
+{
+  (std::cout << ... << append_separator(args)) << "\n";
+}
+
+struct sOrderCutOff
+{
+  int N{1};
+  double Wc_rad_sec{};  // sampling frequency rad/sec
+  double fs{1.};        // cut-off frequency Hz
+};
+
+struct sDifferenceAnBn
+{
+  std::vector<double> An;
+  std::vector<double> Bn;
+};
+
+class ButterworthFilter
+{
+public:
+  // Prints the filter order and cutoff frequency
+  void printFilterSpecs() const;
+  void printFilterContinuousTimeRoots() const;
+
+  void printContinuousTimeTF() const;
+  void printDiscreteTimeTF() const;
+
+  void Buttord(double const & Wp, double const & Ws, double const & Ap, double const & As);
+
+  // Setters and Getters
+  void setCutOffFrequency(double const & Wc);  // Wc is the cut-off frequency in [rad/sec]
+
+  // fc is cut-off frequency in [Hz] and fs is the sampling frequency in [Hz]
+  void setCutOffFrequency(double const & fc, double const & fs);
+  void setOrder(int const & N);
+
+  // Get the order, cut-off frequency and other filter properties
+  [[nodiscard]] sOrderCutOff getOrderCutOff() const;
+  [[nodiscard]] sDifferenceAnBn getAnBn() const;
+
+  [[nodiscard]] std::vector<double> getAn() const;
+  [[nodiscard]] std::vector<double> getBn() const;
+
+  // computes continuous time transfer function
+  void computeContinuousTimeTF(bool const & use_sampling_frequency = false);
+
+  // computes continuous time transfer function
+  void computeDiscreteTimeTF(bool const & use_sampling_frequency = false);
+
+private:
+  // member variables
+  sOrderCutOff filter_specs_{};
+  const double Td_{2.};  // constant of bi-linear transformation
+
+  // Continuous time transfer function roots
+  struct
+  {
+    std::vector<double> phase_angles_{};
+    std::vector<std::complex<double>> continuous_time_roots_{};
+
+    // Continuous time transfer function numerator denominators
+    std::vector<std::complex<double>> continuous_time_denominator_{{0.0, 0.0}};
+    double continuous_time_numerator_{0.0};
+  } ct_tf_{};
+
+  struct
+  {
+    // Gain of the discrete time function
+    std::complex<double> discrete_time_gain_{1.0, 0.0};
+
+    // Discrete time zeros and roots
+    std::vector<std::complex<double>> discrete_time_roots_{{0.0, 0.0}};
+    std::vector<std::complex<double>> discrete_time_zeros_{{-1.0, 0.0}};
+
+    // Discrete time transfer function numerator denominators
+    std::vector<std::complex<double>> discrete_time_denominator_{{0.0, 0.0}};
+    std::vector<std::complex<double>> discrete_time_numerator_{{0.0, 0.0}};
+  } dt_tf_{};
+
+  // Numerator and Denominator Coefficients Bn and An of Discrete Time Filter
+  sDifferenceAnBn AnBn_{};
+
+  // METHODS
+  // polynomial function returns the coefficients given the roots of a polynomial
+  static std::vector<std::complex<double>> poly(std::vector<std::complex<double>> const & roots);
+
+  /*
+   * Implementation starts by computing the pole locations of the filter in the
+   * polar coordinate system . The algorithm first locates the poles  computing
+   * the phase angle and then poles as a complex number From the poles, the
+   * coefficients of denominator polynomial is calculated.
+   *
+   * Therefore, without phase, the roots cannot be calculated. The following
+   * three methods should be called successively.
+   *
+   * */
+
+  // computes the filter root locations in the polar coordinate system
+  void computePhaseAngles();
+
+  // Computes continuous time roots from the phase angles
+  void computeContinuousTimeRoots(bool const & use_sampling_frequency = false);
+};
+
+#endif  // SIGNAL_PROCESSING__BUTTERWORTH_HPP_

common/signal_processing/package.xml

@@ -5,14 +5,16 @@
   <version>0.1.0</version>
   <description>The signal processing package</description>
   <maintainer email="takayuki.murooka@tier4.jp">Takayuki Murooka</maintainer>
+  <maintainer email="ali.boyali@tier4.jp">Ali Boyali</maintainer>
 
   <license>Apache License 2.0</license>
+  <author email="takayuki.murooka@tier4.jp">Takayuki Murooka</author>
+  <author email="ali.boyali@tier4.jp">Ali BOYALI</author>
 
   <build_depend>autoware_cmake</build_depend>
-
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
-
   <depend>geometry_msgs</depend>
+  <depend>rclcpp</depend>
 
   <test_depend>ament_cmake_ros</test_depend>
   <test_depend>ament_lint_auto</test_depend>