Branch data Line data Source code
1 : : // Copyright 2020 the Autoware Foundation 2 : : // 3 : : // Licensed under the Apache License, Version 2.0 (the "License"); 4 : : // you may not use this file except in compliance with the License. 5 : : // You may obtain a copy of the License at 6 : : // 7 : : // http://www.apache.org/licenses/LICENSE-2.0 8 : : // 9 : : // Unless required by applicable law or agreed to in writing, software 10 : : // distributed under the License is distributed on an "AS IS" BASIS, 11 : : // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 : : // See the License for the specific language governing permissions and 13 : : // limitations under the License. 14 : : // 15 : : // Co-developed by Tier IV, Inc. and Apex.AI, Inc. 16 : : /// \file 17 : : /// \brief Interface definition for signal processing filters 18 : : #ifndef SIGNAL_FILTERS__LOW_PASS_FILTER_HPP_ 19 : : #define SIGNAL_FILTERS__LOW_PASS_FILTER_HPP_ 20 : : 21 : : #include <signal_filters/signal_filter.hpp> 22 : : #include <signal_filters/visibility_control.hpp> 23 : : 24 : : #include <chrono> 25 : : 26 : : namespace autoware 27 : : { 28 : : namespace common 29 : : { 30 : : namespace signal_filters 31 : : { 32 : : 33 : : /// Basic low pass filter implemented as an exponential moving average 34 : : /// \tparam T A floating point type for the signal 35 : : template<typename T, typename ClockT = std::chrono::steady_clock> 36 : : class SIGNAL_FILTERS_PUBLIC LowPassFilter : public FilterBase<T, ClockT> 37 : : { 38 : : public: 39 [ + + ]: 24 : explicit LowPassFilter(T cutoff_frequency_hz) 40 : 24 : : FilterBase<T, ClockT>{} 41 : : { 42 [ + + ]: 24 : if (T{} >= cutoff_frequency_hz) { 43 [ + - ]: 6 : throw std::domain_error{"Cutoff frequency is non-positve"}; 44 : : } 45 : : constexpr T TAU{static_cast<T>(2.0 * 3.14159)}; 46 : 18 : m_rc_inv = TAU * cutoff_frequency_hz; 47 : 18 : } 48 : : /// Destructor 49 : 18 : virtual ~LowPassFilter() = default; 50 : : 51 : : protected: 52 : 1206 : T filter_impl(T value, std::chrono::nanoseconds duration) override 53 : : { 54 : : const auto dt = std::chrono::duration_cast<std::chrono::duration<T>>(duration).count(); 55 : : // From https://stackoverflow.com/a/1027808 56 : 1206 : const auto alpha = T{1.0} - std::exp(-dt * m_rc_inv); 57 : 1206 : m_signal += alpha * (value - m_signal); 58 : 1206 : return m_signal; 59 : : } 60 : : 61 : : private: 62 : : T m_rc_inv{}; 63 : : T m_signal{}; 64 : : }; 65 : : } // namespace signal_filters 66 : : } // namespace common 67 : : } // namespace autoware 68 : : 69 : : #endif // SIGNAL_FILTERS__LOW_PASS_FILTER_HPP_