diff --git a/common/signal_processing/CMakeLists.txt b/common/signal_processing/CMakeLists.txt index cca36dcac0341..89f14d4339223 100644 --- a/common/signal_processing/CMakeLists.txt +++ b/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() diff --git a/common/signal_processing/README.md b/common/signal_processing/README.md index cb566061a87ff..5f7aaa978ed6d 100644 --- a/common/signal_processing/README.md +++ b/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. diff --git a/common/signal_processing/documentation/ButterworthFilter.md b/common/signal_processing/documentation/ButterworthFilter.md new file mode 100644 index 0000000000000..e29ce1f4cfb69 --- /dev/null +++ b/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. diff --git a/common/signal_processing/documentation/img.png b/common/signal_processing/documentation/img.png new file mode 100644 index 0000000000000..8de9d193d9504 Binary files /dev/null and b/common/signal_processing/documentation/img.png differ diff --git a/common/signal_processing/include/signal_processing/butterworth.hpp b/common/signal_processing/include/signal_processing/butterworth.hpp new file mode 100644 index 0000000000000..579a915cc9968 --- /dev/null +++ b/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 +#include +#include +#include +#include + +template +const T & append_separator(const T & arg) +{ + std::cout << " "; + return arg; +} + +template +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 An; + std::vector 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 getAn() const; + [[nodiscard]] std::vector 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 phase_angles_{}; + std::vector> continuous_time_roots_{}; + + // Continuous time transfer function numerator denominators + std::vector> continuous_time_denominator_{{0.0, 0.0}}; + double continuous_time_numerator_{0.0}; + } ct_tf_{}; + + struct + { + // Gain of the discrete time function + std::complex discrete_time_gain_{1.0, 0.0}; + + // Discrete time zeros and roots + std::vector> discrete_time_roots_{{0.0, 0.0}}; + std::vector> discrete_time_zeros_{{-1.0, 0.0}}; + + // Discrete time transfer function numerator denominators + std::vector> discrete_time_denominator_{{0.0, 0.0}}; + std::vector> 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> poly(std::vector> 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_ diff --git a/common/signal_processing/package.xml b/common/signal_processing/package.xml index 8825ff0e31577..9496e87fc8240 100644 --- a/common/signal_processing/package.xml +++ b/common/signal_processing/package.xml @@ -5,14 +5,16 @@ 0.1.0 The signal processing package Takayuki Murooka + Ali Boyali Apache License 2.0 + Takayuki Murooka + Ali BOYALI autoware_cmake - ament_cmake_auto - geometry_msgs + rclcpp ament_cmake_ros ament_lint_auto diff --git a/common/signal_processing/src/butterworth.cpp b/common/signal_processing/src/butterworth.cpp new file mode 100644 index 0000000000000..3b4263461e9f6 --- /dev/null +++ b/common/signal_processing/src/butterworth.cpp @@ -0,0 +1,324 @@ +// 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. + +#include "signal_processing/butterworth.hpp" + +#include + +#include +#include +#include + +/** + * @brief Computes the minimum of an analog Butterworth filter order and cut-off frequency give + * the pass and stop-band frequencies and ripple magnitude (tolerances). + * @param Wp [in] pass-band frequency [rad/sc], + * @param Ws [in] stop-band frequency [rad/sc], + * @param Ap [in] pass-band ripple [dB] + * @param As [in] stop-band ripple [dB] + * */ + +void ButterworthFilter::Buttord( + const double & Wp, const double & Ws, const double & Ap, const double & As) +{ + // N*ln(alpha) > ln(beta) + auto alpha = Ws / Wp; + auto beta = std::sqrt((std::pow(10, As / 10.0) - 1.0) / (std::pow(10, Ap / 10.0) - 1.0)); + auto order = static_cast(std::ceil(std::log(beta) / std::log(alpha))); + + setOrder(order); + + // right limit, left limit + /** + * The left and right limits of the magnitudes satisfy the specs at the + * frequencies Ws and Wp Scipy.buttord gives left limit as the cut-off frequency whereas Matlab + * gives right limit. We keep left_lim as a definition and commented out. + * */ + + double right_lim = Ws * (std::pow((std::pow(10.0, As / 10.0) - 1.0), -1.0 / (2. * order))); + // double left_lim = Wp * (std::pow((std::pow(10.0, Ap / 10.0) - 1.0), -1.0 / (2. * order))); + + setCutOffFrequency(right_lim); +} + +void ButterworthFilter::setOrder(const int & N) { filter_specs_.N = N; } + +void ButterworthFilter::setCutOffFrequency(const double & Wc) { filter_specs_.Wc_rad_sec = Wc; } + +/** + * @brief Sets the cut-off and sampling frequencies. + * @param fc [in] cut-off frequency in Hz. + * @param fs [in] sampling frequency in Hz. + * */ +void ButterworthFilter::setCutOffFrequency(const double & fc, const double & fs) +{ + /* + * fc is the cut-off frequency in [Hz] + * fs is the sampling frequency in [Hz] + * */ + if (fc >= fs / 2) { + print("Invalid argument : Cut-off frequency fc must be less than fs/2 \n"); + return; + } + + filter_specs_.Wc_rad_sec = fc * 2.0 * M_PI; + filter_specs_.fs = fs; +} + +sOrderCutOff ButterworthFilter::getOrderCutOff() const { return filter_specs_; } + +/** + * @brief Matlab equivalent : [b, a] = butter(n, Wn, 's') + * */ +void ButterworthFilter::computeContinuousTimeTF(const bool & use_sampling_frequency) +{ + // First compute the phase angles of the roots + computePhaseAngles(); + computeContinuousTimeRoots(use_sampling_frequency); + + auto cutoff_frequency_rad_sec = filter_specs_.Wc_rad_sec; + auto order = filter_specs_.N; + + ct_tf_.continuous_time_denominator_ = poly(ct_tf_.continuous_time_roots_); + ct_tf_.continuous_time_numerator_ = std::pow(cutoff_frequency_rad_sec, order); +} + +void ButterworthFilter::computePhaseAngles() +{ + const auto & order = filter_specs_.N; + ct_tf_.phase_angles_.resize(order, 0.); + + for (size_t i = 0; i < ct_tf_.phase_angles_.size(); ++i) { + auto & x = ct_tf_.phase_angles_.at(i); + x = M_PI_2 + (M_PI * (2.0 * static_cast((i + 1)) - 1.0) / (2.0 * order)); + } +} + +void ButterworthFilter::computeContinuousTimeRoots(const bool & use_sampling_frequency) +{ + const auto & order = filter_specs_.N; + const auto & sampling_frequency_hz = filter_specs_.fs; + const auto & cutoff_frequency_rad_sec = filter_specs_.Wc_rad_sec; + + ct_tf_.continuous_time_roots_.resize(order, {0.0, 0.0}); + + if (use_sampling_frequency) { + const double & Fc = (sampling_frequency_hz / M_PI) * + tan(cutoff_frequency_rad_sec / (sampling_frequency_hz * 2.0)); + + for (size_t i = 0; i < ct_tf_.continuous_time_roots_.size(); ++i) { + auto & x = ct_tf_.continuous_time_roots_[i]; + x = { + std::cos(ct_tf_.phase_angles_[i]) * Fc * 2.0 * M_PI, + std::sin(ct_tf_.phase_angles_[i]) * Fc * 2.0 * M_PI}; + } + return; + } + + for (size_t i = 0; i < ct_tf_.continuous_time_roots_.size(); ++i) { + auto & x = ct_tf_.continuous_time_roots_[i]; + x = { + cutoff_frequency_rad_sec * cos(ct_tf_.phase_angles_[i]), + cutoff_frequency_rad_sec * sin(ct_tf_.phase_angles_[i])}; + } +} +std::vector> ButterworthFilter::poly( + std::vector> const & roots) +{ + std::vector> coefficients(roots.size() + 1, {0, 0}); + + const int n{static_cast(roots.size())}; + + coefficients[0] = {1.0, 0.0}; + + for (int i = 0; i < n; i++) { + for (int j = i; j != -1; j--) { + coefficients[j + 1] = coefficients[j + 1] - (roots[i] * coefficients[j]); + } + } + + return coefficients; +} + +/** + * @brief Prints the order and cut-off angular frequency (rad/sec) of the filter + * */ + +void ButterworthFilter::printFilterContinuousTimeRoots() const +{ + std::stringstream stream; + stream << "\n The roots of the continuous-time filter Transfer Function's Denominator are : \n"; + + for (const auto & x : ct_tf_.continuous_time_roots_) { + stream << std::fixed << std::setprecision(2) << std::real(x) << " j"; + + auto txt = std::imag(x) < 0 ? " - j " : " + j "; + stream << std::fixed << std::setprecision(2) << txt << std::abs(std::imag(x)) << " \n"; + } + + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "%s", stream.str().c_str()); +} +void ButterworthFilter::printContinuousTimeTF() const +{ + const auto & n = filter_specs_.N; + + RCLCPP_INFO( + rclcpp::get_logger("rclcpp"), "\nThe Continuous Time Transfer Function of the Filter is ;\n"); + + std::stringstream stream; + stream << std::fixed << std::setprecision(2) << ct_tf_.continuous_time_numerator_ << " / \n"; + + for (int i = n; i > 0; i--) { + stream << std::fixed << std::setprecision(2) + << ct_tf_.continuous_time_denominator_[n - i].real() << " * s [" << i << "] + "; + } + + stream << std::fixed << std::setprecision(2) << ct_tf_.continuous_time_denominator_[n].real(); + + const auto & tf_text = stream.str(); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "[%s]", tf_text.c_str()); +} + +/** + * @brief This method assumes the continuous time transfer function of filter has already been + * computed and stored in the object and uses the bilinear transformation to obtain the discrete + * time transfer function. + * + * Matlab equivalent : + * Td = 2. + * [numd, dend] = bilinear(sys_filt.Numerator{1}, sys_filt.Denominator{1}, 1/Td) + * where sys_filt is the continuous time transfer function. + * */ +void ButterworthFilter::computeDiscreteTimeTF(const bool & use_sampling_frequency) +{ + const auto & order = filter_specs_.N; + const auto & sampling_frequency_hz = filter_specs_.fs; + + // Butter puts zeros at -1.0 for causality + dt_tf_.discrete_time_zeros_.resize(order, {-1.0, 0.0}); + dt_tf_.discrete_time_roots_.resize(order, {0.0, 0.0}); + + auto & An_ = AnBn_.An; + auto & Bn_ = AnBn_.Bn; + + An_.resize(order + 1, 0.0); + Bn_.resize(order + 1, 0.0); + + dt_tf_.discrete_time_gain_ = {ct_tf_.continuous_time_numerator_, 0.0}; + + // Bi-linear Transformation of the Roots + + if (use_sampling_frequency) { + for (size_t i = 0; i < dt_tf_.discrete_time_roots_.size(); ++i) { + auto & dr = dt_tf_.discrete_time_roots_[i]; + + dr = (1.0 + ct_tf_.continuous_time_roots_[i] / (sampling_frequency_hz * 2.0)) / + (1.0 - ct_tf_.continuous_time_roots_[i] / (sampling_frequency_hz * 2.0)); + } + + dt_tf_.discrete_time_denominator_ = poly(dt_tf_.discrete_time_roots_); + + // Obtain the coefficients of numerator and denominator + dt_tf_.discrete_time_numerator_ = poly(dt_tf_.discrete_time_zeros_); + + // Compute Discrete Time Gain + const auto & sum_num = std::accumulate( + dt_tf_.discrete_time_numerator_.cbegin(), dt_tf_.discrete_time_numerator_.cend(), + std::complex{}); + + const auto & sum_den = std::accumulate( + dt_tf_.discrete_time_denominator_.cbegin(), dt_tf_.discrete_time_denominator_.cend(), + std::complex{}); + + dt_tf_.discrete_time_gain_ = std::abs(sum_den / sum_num); + + for (size_t i = 0; i < dt_tf_.discrete_time_numerator_.size(); ++i) { + auto & dn = dt_tf_.discrete_time_numerator_[i]; + dn = dn * dt_tf_.discrete_time_gain_; + Bn_[i] = dn.real(); + } + + for (size_t i = 0; i < dt_tf_.discrete_time_denominator_.size(); ++i) { + const auto & dd = dt_tf_.discrete_time_denominator_[i]; + An_[i] = dd.real(); + } + + return; + } + + for (size_t i = 0; i < dt_tf_.discrete_time_roots_.size(); ++i) { + auto & dr = dt_tf_.discrete_time_roots_[i]; + dr = (1.0 + Td_ * ct_tf_.continuous_time_roots_[i] / 2.0) / + (1.0 - Td_ * ct_tf_.continuous_time_roots_[i] / 2.0); + + dt_tf_.discrete_time_gain_ = + dt_tf_.discrete_time_gain_ / (1.0 - ct_tf_.continuous_time_roots_[i]); + } + + // Obtain the coefficients of numerator and denominator + dt_tf_.discrete_time_denominator_ = poly(dt_tf_.discrete_time_roots_); + dt_tf_.discrete_time_numerator_ = poly(dt_tf_.discrete_time_zeros_); + + for (size_t i = 0; i < dt_tf_.discrete_time_numerator_.size(); ++i) { + auto & dn = dt_tf_.discrete_time_numerator_[i]; + dn = dn * dt_tf_.discrete_time_gain_; + Bn_[i] = dn.real(); + } + + for (size_t i = 0; i < dt_tf_.discrete_time_denominator_.size(); ++i) { + const auto & dd = dt_tf_.discrete_time_denominator_[i]; + An_[i] = dd.real(); + } +} +void ButterworthFilter::printDiscreteTimeTF() const +{ + const int & n = filter_specs_.N; + + std::stringstream stream; + stream << "\nThe Discrete Time Transfer Function of the Filter is ;\n"; + + for (int i = n; i > 0; i--) { + stream << std::fixed << std::setprecision(2) << dt_tf_.discrete_time_numerator_[n - i].real(); + stream << " z[-" << i << " ] + "; + } + + stream << std::fixed << std::setprecision(2) << dt_tf_.discrete_time_numerator_[n].real() + << " / \n"; + + for (int i = n; i > 0; i--) { + stream << std::fixed << std::setprecision(2) << dt_tf_.discrete_time_denominator_[n - i].real(); + stream << " z[-" << i << " ] + "; + } + stream << std::fixed << std::setprecision(2) << dt_tf_.discrete_time_denominator_[n].real() + << " \n\n"; + + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "[%s]", stream.str().c_str()); +} +std::vector ButterworthFilter::getAn() const { return AnBn_.An; } +std::vector ButterworthFilter::getBn() const { return AnBn_.Bn; } +sDifferenceAnBn ButterworthFilter::getAnBn() const { return AnBn_; } + +void ButterworthFilter::printFilterSpecs() const +{ + /** + * @brief Prints the order and cut-off angular frequency (rad/sec) of the filter + * + * */ + + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "The order of the filter : %d ", this->filter_specs_.N); + + RCLCPP_INFO( + rclcpp::get_logger("rclcpp"), "Cut-off Frequency : %2.2f rad/sec", + this->filter_specs_.Wc_rad_sec); +} diff --git a/common/signal_processing/test/include/butterworth_filter_test.hpp b/common/signal_processing/test/include/butterworth_filter_test.hpp new file mode 100644 index 0000000000000..6a43b0b95a7e2 --- /dev/null +++ b/common/signal_processing/test/include/butterworth_filter_test.hpp @@ -0,0 +1,30 @@ +// 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 BUTTERWORTH_FILTER_TEST_HPP_ +#define BUTTERWORTH_FILTER_TEST_HPP_ + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "signal_processing/butterworth.hpp" + +class ButterWorthTestFixture : public ::testing::Test +{ +protected: + ButterWorthTestFixture() = default; + + ~ButterWorthTestFixture() override = default; +}; + +#endif // BUTTERWORTH_FILTER_TEST_HPP_ diff --git a/common/signal_processing/test/src/butterworth_filter_test.cpp b/common/signal_processing/test/src/butterworth_filter_test.cpp new file mode 100644 index 0000000000000..d85b9a37f870f --- /dev/null +++ b/common/signal_processing/test/src/butterworth_filter_test.cpp @@ -0,0 +1,165 @@ +// 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. + +#include "butterworth_filter_test.hpp" + +TEST_F(ButterWorthTestFixture, butterworthOrderTest) +{ + double tol = 1e-4; + + // 1st Method + const double & Wp{2.}; // pass-band frequency [rad/sec] + const double & Ws{3.}; // stop-band frequency [rad/sec] + const double & Ap{6.}; // pass-band ripple mag or loss [dB] + const double & As{20.}; // stop band ripple attenuation [dB] + + ButterworthFilter bf; + bf.Buttord(Wp, Ws, Ap, As); + + const auto & NWc = bf.getOrderCutOff(); + // print("The computed order and frequency for the give specification : "); + // print("Minimum order N = ", NWc.N, ", and The cut-off frequency Wc = ", NWc.Wc, "rad/sec \n"); + bf.printFilterSpecs(); + + /** + * Approximate the continuous and discrete time transfer functions. + * */ + bf.computeContinuousTimeTF(); + + // Print continuous time roots. + bf.printFilterContinuousTimeRoots(); + bf.printContinuousTimeTF(); + + // Compute the discrete time transfer function. + bf.computeDiscreteTimeTF(); + bf.printDiscreteTimeTF(); + + ASSERT_EQ(5, NWc.N); + ASSERT_NEAR(1.89478, NWc.Wc_rad_sec, tol); + + // test transfer functions + bf.computeContinuousTimeTF(); + bf.computeDiscreteTimeTF(); + + const std::vector & An = bf.getAn(); + const std::vector & Bn = bf.getBn(); + + /** + * Bd = [0.1913 0.9564 1.9128 1.9128 0.9564 0.1913] + * Ad = [1.0000 1.8849 1.8881 1.0137 0.2976 0.0365] + */ + + ASSERT_NEAR(1.8849, An[1], tol); + ASSERT_NEAR(1.8881, An[2], tol); + ASSERT_NEAR(1.0137, An[3], tol); + ASSERT_NEAR(0.29762, An[4], tol); + ASSERT_NEAR(0.0365, An[5], tol); + + ASSERT_NEAR(0.9564, Bn[1], tol); + ASSERT_NEAR(1.9128, Bn[2], tol); + ASSERT_NEAR(1.9128, Bn[3], tol); + ASSERT_NEAR(0.9564, Bn[4], tol); + ASSERT_NEAR(0.1913, Bn[5], tol); +} + +TEST_F(ButterWorthTestFixture, butterDefinedSamplingOrder1) +{ + ButterworthFilter bf; + const double tol{1e-12}; + + // Test with a defined sampling frequency + const int order{1}; + const double cut_off_frq_hz{5.}; + const double sampling_frq_hz{40.}; + const bool use_sampling_frequency = true; + + // Prepare the filter + bf.setOrder(order); + bf.setCutOffFrequency(cut_off_frq_hz, sampling_frq_hz); + bf.computeContinuousTimeTF(use_sampling_frequency); + bf.computeDiscreteTimeTF(use_sampling_frequency); + + const auto & An = bf.getAn(); + const auto & Bn = bf.getBn(); + + std::vector const & An_ground_truth{1., -0.414213562373095}; + std::vector const & Bn_ground_truth{0.292893218813452, 0.292893218813452}; + + for (size_t k = 0; k < An.size(); ++k) { + ASSERT_NEAR(An[k], An_ground_truth[k], tol); + ASSERT_NEAR(Bn[k], Bn_ground_truth[k], tol); + } +} + +TEST_F(ButterWorthTestFixture, butterDefinedSamplingOrder2) +{ + ButterworthFilter bf; + const double tol{1e-12}; + + // Test with defined sampling frequency + const int order{2}; + const double cut_off_frq_hz{10.}; + const double sampling_frq_hz{100}; + const bool use_sampling_frequency = true; + + // Prepare the filter + bf.setOrder(order); + bf.setCutOffFrequency(cut_off_frq_hz, sampling_frq_hz); + bf.computeContinuousTimeTF(use_sampling_frequency); + bf.computeDiscreteTimeTF(use_sampling_frequency); + + const auto & An = bf.getAn(); + const auto & Bn = bf.getBn(); + + const std::vector & An_ground_truth{1., -1.142980502539901, 0.412801598096189}; + const std::vector & Bn_ground_truth{ + 0.067455273889072, 0.134910547778144, 0.067455273889072}; + + for (size_t k = 0; k < An.size(); ++k) { + ASSERT_NEAR(An[k], An_ground_truth[k], tol); + ASSERT_NEAR(Bn[k], Bn_ground_truth[k], tol); + } +} + +TEST_F(ButterWorthTestFixture, butterDefinedSamplingOrder3) +{ + ButterworthFilter bf; + const double tol{1e-12}; + + // Test with a defined sampling frequency + const int order{3}; + const double cut_off_frq_hz{10.}; + const double sampling_frq_hz{100}; + const bool use_sampling_frequency = true; + + // Prepare the filter + bf.setOrder(order); + bf.setCutOffFrequency(cut_off_frq_hz, sampling_frq_hz); + bf.computeContinuousTimeTF(use_sampling_frequency); + bf.computeDiscreteTimeTF(use_sampling_frequency); + + const auto & An = bf.getAn(); + const auto & Bn = bf.getBn(); + + const std::vector & An_ground_truth{ + 1., -1.760041880343169, 1.182893262037831, -0.278059917634546}; + + const std::vector & Bn_ground_truth{ + 0.018098933007514, 0.054296799022543, 0.054296799022543, 0.018098933007514}; + + for (size_t k = 0; k < An.size(); ++k) { + ASSERT_NEAR(An[k], An_ground_truth[k], tol); + ASSERT_NEAR(Bn[k], Bn_ground_truth[k], tol); + } +} diff --git a/common/signal_processing/usage_examples/main_butterworth.cpp b/common/signal_processing/usage_examples/main_butterworth.cpp new file mode 100644 index 0000000000000..2b36bd40b3578 --- /dev/null +++ b/common/signal_processing/usage_examples/main_butterworth.cpp @@ -0,0 +1,129 @@ +// 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. + +#include "signal_processing/butterworth.hpp" + +#include +#include + +/** The filter tool can be used in the following ways. + * + * 1- Defining specs Wp, Ws, Ap, As and obtaining order and cut-off frequency (rad/sec) + * ButterworthFilter bf; + * bf.Buttord(Wp, Ws, Ap, As); Wp and Ws in [rad/sec] + * bf.computeContinuousTimeTF(); + * bf.computeDiscreteTimeTF(); + * + * 2- Defining the order and cut-off frequency (rad/sec) directly and computing the filter TFs + * bf.setOrder(N); N is integer + * bf.setCutOffFrequency(Wc); Wc in [rad/sec] + * bf.computeContinuousTimeTF(); + * bf.computeDiscreteTimeTF(); + * + * 3- Defining the order N, cut-off and sampling frequencies (Hz) + * bf.setOrder(N); N is integer + * bf.setCutOffFrequency_Hz(fc, fs); cut-off fc and sampling fs are in [Hz] + * bf.computeContinuousTimeTF(); + * bf.computeDiscreteTimeTF(); + * */ + +int main() +{ + // 1st Method + double Wp{2.}; // pass-band frequency [rad/sec] + double Ws{3.}; // stop-band frequency [rad/sec] + double Ap{6.}; // pass-band ripple mag or loss [dB] + double As{20.}; // stop band ripple attenuation [dB] + + ButterworthFilter bf1; + bf1.Buttord(Wp, Ws, Ap, As); + + auto NWc = bf1.getOrderCutOff(); + print("The computed order and frequency for the give specification : "); + print( + "Minimum order N = ", NWc.N, ", and The cut-off frequency Wc = ", NWc.Wc_rad_sec, "rad/sec \n"); + bf1.printFilterSpecs(); + + /** + * Approximate the continuous and discrete time transfer functions. + * */ + bf1.computeContinuousTimeTF(); + + // Print continuous time roots. + bf1.printFilterContinuousTimeRoots(); + bf1.printContinuousTimeTF(); + + // Compute the discrete time transfer function. + bf1.computeDiscreteTimeTF(); + bf1.printDiscreteTimeTF(); + + // 2nd METHOD + // Setting filter order N and cut-off frequency explicitly + print("SECOND TYPE of FILTER INITIALIZATION "); + + ButterworthFilter bf2; + bf2.setOrder(2); + bf2.setCutOffFrequency(2.0); + bf2.printFilterSpecs(); + + // Get the computed order and Cut-off frequency + NWc = bf2.getOrderCutOff(); + + // Print continuous time roots. + bf2.computeContinuousTimeTF(); + bf2.printFilterContinuousTimeRoots(); + bf2.printContinuousTimeTF(); + + // Compute the discrete time transfer function. + bf2.computeDiscreteTimeTF(); + bf2.printDiscreteTimeTF(); + + // 3rd METHOD + // defining a sampling frequency together with the cut-off fc, fs + print("THIRD TYPE of FILTER INITIALIZATION "); + + ButterworthFilter bf3; + bf3.setOrder(3); + + bf3.setCutOffFrequency(10, 100); + bf3.printFilterSpecs(); + + bool use_sampling_frequency{true}; + + bf3.computeContinuousTimeTF(use_sampling_frequency); + bf3.printFilterContinuousTimeRoots(); + bf3.printContinuousTimeTF(); + + // Compute Discrete Time TF + bf3.computeDiscreteTimeTF(use_sampling_frequency); + bf3.printDiscreteTimeTF(); + + auto AnBn = bf3.getAnBn(); + auto An = bf3.getAn(); + auto Bn = bf3.getBn(); + + print("An : "); + + for (double it : An) { + std::cout << std::setprecision(4) << it << ", "; + } + + print("\nBn : \n"); + + for (double it : Bn) { + std::cout << std::setprecision(4) << it << ", "; + } + + return 0; +}