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 on vectors
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 "math/m_lowpass_float.hpp"
17
18#include <Eigen/Core>
19
20
21namespace xrt::auxiliary::math {
22
23/*!
24 * A very simple low-pass filter, using a "one-pole infinite impulse response"
25 * design (one-pole IIR).
26 *
27 * Configurable in dimension and scalar type.
28 */
29template <size_t Dim, typename Scalar> class LowPassIIRVectorFilter
30{
31public:
32 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
33
34 using Vector = Eigen::Matrix<Scalar, Dim, 1>;
35
36 /*!
37 * Constructor
38 *
39 * @param cutoff_hz A cutoff frequency in Hertz: signal changes much
40 * lower in frequency will be passed through the filter, while signal
41 * changes much higher in frequency will be blocked.
42 */
43 explicit LowPassIIRVectorFilter(Scalar cutoff_hz) noexcept : impl_(cutoff_hz, Vector::Zero()) {}
44
45
46 /*!
47 * Reset the filter to newly-created state.
48 */
49 void
50 reset() noexcept
51 {
52 impl_.reset(Vector::Zero());
53 }
54
55 /*!
56 * Filter a sample, with an optional weight.
57 *
58 * @param sample The value to filter
59 * @param timestamp_ns The time that this sample was measured.
60 * @param weight An optional value between 0 and 1. The smaller this
61 * value, the less the current sample influences the filter state. For
62 * the first call, this is always assumed to be 1.
63 */
64 void
65 addSample(Vector const &sample, std::uint64_t timestamp_ns, Scalar weight = 1)
66 {
67 impl_.addSample(sample, timestamp_ns, weight);
68 }
69
70 /*!
71 * Get the filtered value.
72 */
73 Vector const &
74 getState() const noexcept
75 {
76 return impl_.state;
77 }
78
79 /*!
80 * Get the time of last update.
81 */
82 std::uint64_t
83 getTimestampNs() const noexcept
84 {
85 return impl_.filter_timestamp_ns;
86 }
87
88 /*!
89 * Get whether we have initialized state.
90 */
91 bool
92 isInitialized() const noexcept
93 {
94 return impl_.initialized;
95 }
96
97private:
98 detail::LowPassIIR<Vector, Scalar> impl_;
99};
100
101} // namespace xrt::auxiliary::math