matrixfurry.com
The open source OpenXR runtime
1// Copyright 2019, 2022, Collabora, Ltd.
2// SPDX-License-Identifier: BSL-1.0
3/*!
4 * @file
5 * @brief Low-pass IIR filter
6 * @author Rylie Pavlik <rylie.pavlik@collabora.com>
7 * @ingroup aux_math
8 */
9
10#pragma once
11
12#ifndef __cplusplus
13#error "This header is C++-only."
14#endif
15
16#include "util/u_time.h"
17
18#include "math/m_mathinclude.h"
19#include <cmath>
20#include <type_traits>
21
22
23namespace xrt::auxiliary::math {
24
25namespace detail {
26 /*!
27 * The shared implementation (between vector and scalar versions) of an
28 * IIR low-pass filter.
29 */
30 template <typename Value, typename Scalar> struct LowPassIIR
31 {
32 // For fixed point, you'd need more bits of data storage. See
33 // https://www.embeddedrelated.com/showarticle/779.php
34 static_assert(std::is_floating_point<Scalar>::value,
35 "Filter is designed only for floating-point values. If "
36 "you want fixed-point, you must reimplement it.");
37 // EIGEN_MAKE_ALIGNED_OPERATOR_NEW
38
39 /*!
40 * Constructor
41 *
42 * @param cutoff_hz A cutoff frequency in Hertz: signal changes
43 * much lower in frequency will be passed through the filter,
44 * while signal changes much higher in frequency will be
45 * blocked.
46 *
47 * @param val The value to initialize the filter with. Does not
48 * affect the filter itself: only seen if you get state
49 * before initializing the filter with the first sample.
50 */
51 explicit LowPassIIR(Scalar cutoff_hz, Value const &val) noexcept
52 : state(val), time_constant(static_cast<Scalar>(1.f / (2.f * M_PI * cutoff_hz)))
53 {}
54
55 /*!
56 * Reset the filter to newly-created state.
57 */
58 void
59 reset(Value const &val) noexcept
60 {
61 state = val;
62 initialized = false;
63 filter_timestamp_ns = 0;
64 }
65
66 /*!
67 * Filter a sample, with an optional weight.
68 *
69 * @param sample The value to filter
70 * @param timestamp_ns The time that this sample was measured.
71 * @param weight An optional value between 0 and 1. The smaller
72 * this value, the less the current sample influences the filter
73 * state. For the first call, this is always assumed to be 1.
74 */
75 void
76 addSample(Value const &sample, timepoint_ns timestamp_ns, Scalar weight = 1)
77 {
78 if (!initialized) {
79 initialized = true;
80 state = sample;
81 filter_timestamp_ns = timestamp_ns;
82 return;
83 }
84 // get dt in seconds
85 Scalar dt = static_cast<Scalar>(time_ns_to_s(timestamp_ns - filter_timestamp_ns));
86 //! @todo limit max dt?
87 Scalar weighted = dt * weight;
88 Scalar alpha = weighted / (time_constant + weighted);
89
90 // The update step below is equivalent to
91 // state = state * (1 - alpha) + alpha * sample;
92 // -- it blends the current sample and the filter state
93 // using alpha as the blending parameter.
94 state += alpha * (sample - state);
95 filter_timestamp_ns = timestamp_ns;
96 }
97
98 Value state;
99 Scalar time_constant;
100 bool initialized{false};
101 timepoint_ns filter_timestamp_ns{0};
102 };
103} // namespace detail
104
105/*!
106 * A very simple low-pass filter, using a "one-pole infinite impulse response"
107 * design (one-pole IIR).
108 *
109 * Configurable in scalar type.
110 */
111template <typename Scalar> class LowPassIIRFilter
112{
113public:
114 /*!
115 * Constructor
116 *
117 * @param cutoff_hz A cutoff frequency in Hertz: signal changes much
118 * lower in frequency will be passed through the filter, while signal
119 * changes much higher in frequency will be blocked.
120 */
121 explicit LowPassIIRFilter(Scalar cutoff_hz) noexcept : impl_(cutoff_hz, 0) {}
122
123
124 /*!
125 * Reset the filter to newly-created state.
126 */
127 void
128 reset() noexcept
129 {
130 impl_.reset(0);
131 }
132
133 /*!
134 * Filter a sample, with an optional weight.
135 *
136 * @param sample The value to filter
137 * @param timestamp_ns The time that this sample was measured.
138 * @param weight An optional value between 0 and 1. The smaller this
139 * value, the less the current sample influences the filter state. For
140 * the first call, this is always assumed to be 1.
141 */
142 void
143 addSample(Scalar sample, timepoint_ns timestamp_ns, Scalar weight = 1)
144 {
145 impl_.addSample(sample, timestamp_ns, weight);
146 }
147
148 /*!
149 * Get the filtered value.
150 */
151 Scalar
152 getState() const noexcept
153 {
154 return impl_.state;
155 }
156
157 /*!
158 * Get the time of last update.
159 */
160 timepoint_ns
161 getTimestampNs() const noexcept
162 {
163 return impl_.filter_timestamp_ns;
164 }
165
166 /*!
167 * Get whether we have initialized state.
168 */
169 bool
170 isInitialized() const noexcept
171 {
172 return impl_.initialized;
173 }
174
175private:
176 detail::LowPassIIR<Scalar, Scalar> impl_;
177};
178
179} // namespace xrt::auxiliary::math