Autoware.Auto
low_pass_filter.hpp
Go to the documentation of this file.
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.
18 #ifndef SIGNAL_FILTERS__LOW_PASS_FILTER_HPP_
19 #define SIGNAL_FILTERS__LOW_PASS_FILTER_HPP_
20 
23 
24 #include <chrono>
25 
26 namespace autoware
27 {
28 namespace common
29 {
30 namespace signal_filters
31 {
32 
35 template<typename T, typename ClockT = std::chrono::steady_clock>
37 {
38 public:
39  explicit LowPassFilter(T cutoff_frequency_hz)
40  : FilterBase<T, ClockT>{}
41  {
42  if (T{} >= cutoff_frequency_hz) {
43  throw std::domain_error{"Cutoff frequency is non-positve"};
44  }
45  constexpr T TAU{static_cast<T>(2.0 * 3.14159)};
46  m_rc_inv = TAU * cutoff_frequency_hz;
47  }
49  virtual ~LowPassFilter() = default;
50 
51 protected:
52  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  const auto alpha = T{1.0} - std::exp(-dt * m_rc_inv);
57  m_signal += alpha * (value - m_signal);
58  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_
autoware::common::types::TAU
constexpr float32_t TAU
tau = 2 pi
Definition: types.hpp:54
autoware::common::signal_filters::LowPassFilter::filter_impl
T filter_impl(T value, std::chrono::nanoseconds duration) override
Actual implementation, error checking already done, can assume duration is positive.
Definition: low_pass_filter.hpp:52
autoware::common::signal_filters::LowPassFilter::LowPassFilter
LowPassFilter(T cutoff_frequency_hz)
Definition: low_pass_filter.hpp:39
visibility_control.hpp
autoware
This file defines the lanelet2_map_provider_node class.
Definition: quick_sort.hpp:24
autoware::common::signal_filters::FilterType::LowPassFilter
@ LowPassFilter
signal_filter.hpp
Interface definition for signal processing filters.
autoware::common::signal_filters::LowPassFilter
Definition: low_pass_filter.hpp:36
SIGNAL_FILTERS_PUBLIC
#define SIGNAL_FILTERS_PUBLIC
Definition: common/signal_filters/include/signal_filters/visibility_control.hpp:44
autoware::common::signal_filters::FilterBase
Definition: signal_filter.hpp:52