Line data Source code
1 : /* ✔ */
2 :
3 : #pragma once
4 :
5 : #include <stdint.h>
6 : #include <AH/STL/limits>
7 : #include <AH/STL/type_traits>
8 :
9 : #include <AH/Settings/NamespaceSettings.hpp>
10 :
11 : BEGIN_AH_NAMESPACE
12 :
13 : /**
14 : * @brief Exponential moving average filter.
15 : *
16 : * Fast integer EMA implementation where the weight factor is a power of two.
17 : *
18 : * Difference equation: @f$ y[n] = \alpha·x[n]+(1-\alpha)·y[n-1] @f$
19 : * where @f$ \alpha = \left(\frac{1}{2}\right)^{K} @f$, @f$ x @f$ is the
20 : * input sequence, and @f$ y @f$ is the output sequence.
21 : *
22 : * [An in-depth explanation of the EMA filter](https://tttapa.github.io/Pages/Mathematics/Systems-and-Control-Theory/Digital-filters/Exponential%20Moving%20Average/)
23 : *
24 : * @tparam K
25 : * The amount of bits to shift by. This determines the location
26 : * of the pole in the EMA transfer function, and therefore the
27 : * cut-off frequency.
28 : * The higher this number, the more filtering takes place.
29 : * The pole location is @f$ 1 - 2^{-K} @f$.
30 : * @tparam input_t
31 : * The integer type to use for the input and output of the filter.
32 : * Can be signed or unsigned.
33 : * @tparam state_t
34 : * The unsigned integer type to use for the internal state of the
35 : * filter. A fixed-point representation with @f$ K @f$ fractional
36 : * bits is used, so this type should be at least @f$ M + K @f$ bits
37 : * wide, where @f$ M @f$ is the maximum number of bits of the input.
38 : *
39 : * Some examples of different combinations of template parameters:
40 : *
41 : * 1. Filtering the result of `analogRead`: analogRead returns an integer
42 : * between 0 and 1023, which can be represented using 10 bits, so
43 : * @f$ M = 10 @f$. If `input_t` and `output_t` are both `uint16_t`,
44 : * the maximum shift factor `K` is @f$ 16 - M = 6 @f$. If `state_t`
45 : * is increased to `uint32_t`, the maximum shift factor `K` is
46 : * @f$ 32 - M = 22 @f$.
47 : * 2. Filtering a signed integer between -32768 and 32767: this can be
48 : * represented using a 16-bit signed integer, so `input_t` is `int16_t`,
49 : * and @f$ M = 16 @f$. (2¹⁵ = 32768)
50 : * Let's say the shift factor `K` is 1, then the minimum width of
51 : * `state_t` should be @f$ M + K = 17 @f$ bits, so `uint32_t` would be
52 : * a sensible choice.
53 : *
54 : * @ingroup AH_Filters
55 : */
56 : template <uint8_t K,
57 : class input_t = uint_fast16_t,
58 : class state_t = typename std::make_unsigned<input_t>::type>
59 : class EMA {
60 : public:
61 : /// Constructor: initialize filter to zero or optional given value.
62 25 : EMA(input_t initial = input_t{0}) { reset(initial); }
63 :
64 : /**
65 : * @brief Reset the filter to the given value.
66 : *
67 : * @param value
68 : * The value to reset the filter state to.
69 : */
70 34 : void reset(input_t value = input_t(0)) {
71 34 : state_t value_s = static_cast<state_t>(value);
72 34 : state = zero + (value_s << K) - value_s;
73 34 : }
74 :
75 : /**
76 : * @brief Filter the input: Given @f$ x[n] @f$, calculate @f$ y[n] @f$.
77 : *
78 : * @param input
79 : * The new raw input value.
80 : * @return The new filtered output value.
81 : */
82 2063 : input_t filter(input_t input) {
83 2063 : state += static_cast<state_t>(input);
84 2063 : state_t output = (state + half) >> K;
85 2063 : output -= zero >> K;
86 2063 : state -= output;
87 2063 : return static_cast<input_t>(output);
88 : }
89 :
90 : /// @copydoc EMA::filter(input_t)
91 2021 : input_t operator()(input_t input) {
92 2021 : return filter(input);
93 : }
94 :
95 : constexpr static state_t
96 : max_state = std::numeric_limits<state_t>::max(),
97 : half_state = max_state / 2 + 1,
98 : zero = std::is_unsigned<input_t>::value ? state_t{0} : half_state,
99 : half = K > 0 ? state_t{1} << (K - 1) : state_t{0};
100 :
101 : static_assert(std::is_unsigned<state_t>::value,
102 : "state type should be unsigned");
103 :
104 : static_assert(max_state >= std::numeric_limits<input_t>::max(),
105 : "state type cannot be narrower than input type");
106 :
107 : /// Verify the input range to make sure it's compatible with the shift
108 : /// factor and the width of the state type.
109 : ///
110 : /// Examples:
111 : /// ~~~cpp
112 : /// EMA<5, int_fast16_t, uint_fast16_t> filter;
113 : /// static_assert(filter.supports_range(-1024, 1023),
114 : /// "use a wider state or input type, or a smaller shift factor");
115 : /// ~~~
116 : /// ~~~cpp
117 : /// EMA<5, uint_fast16_t, uint_fast16_t> filter;
118 : /// static_assert(filter.supports_range(0u, 2047u),
119 : /// "use a wider state or input type, or a smaller shift factor");
120 : /// ~~~
121 : template <class T>
122 : constexpr static bool supports_range(T min, T max) {
123 : using sstate_t = typename std::make_signed<state_t>::type;
124 : return min <= max &&
125 : min >= std::numeric_limits<input_t>::min() &&
126 : max <= std::numeric_limits<input_t>::max() &&
127 : (std::is_unsigned<input_t>::value
128 : ? state_t(max) <= (max_state >> K)
129 : : min >= -static_cast<sstate_t>(max_state >> (K + 1)) - 1 &&
130 : max <= static_cast<sstate_t>(max_state >> (K + 1)));
131 : }
132 :
133 : private:
134 : state_t state;
135 : };
136 :
137 : // -------------------------------------------------------------------------- //
138 :
139 : /**
140 : * @brief A class for single-pole infinite impulse response filters
141 : * or exponential moving average filters.
142 : *
143 : * This version uses floating point maths.
144 : *
145 : * Difference equation: @f$ y[n] = \alpha·x[n]+(1-\alpha)·y[n-1] @f$
146 : * @f$ x @f$ is the input sequence, and @f$ y @f$ is the output sequence.
147 : *
148 : * [An in-depth explanation of the EMA filter]
149 : * (https://tttapa.github.io/Pages/Mathematics/Systems-and-Control-Theory/Digital-filters/Exponential%20Moving%20Average/)
150 : *
151 : * @ingroup AH_Filters
152 : */
153 : class EMA_f {
154 : public:
155 : /**
156 : * @brief Create an exponential moving average filter with a pole at the
157 : * given location.
158 : *
159 : * @param pole
160 : * The pole of the filter (@f$1-\alpha@f$).
161 : * Should be a value in the range
162 : * @f$ \left[0,1\right) @f$.
163 : * Zero means no filtering, and closer to one means more filtering.
164 : */
165 1 : EMA_f(float pole) : alpha(1 - pole) {}
166 :
167 : /**
168 : * @brief Filter the input: Given @f$ x[n] @f$, calculate @f$ y[n] @f$.
169 : *
170 : * @param value
171 : * The new raw input value.
172 : * @return The new filtered output value.
173 : */
174 12 : float filter(float value) {
175 12 : filtered += (value - filtered) * alpha;
176 12 : return filtered;
177 : }
178 :
179 : /// @copydoc filter(float)
180 12 : float operator()(float value) { return filter(value); }
181 :
182 : private:
183 : float alpha;
184 : float filtered = 0;
185 : };
186 :
187 : END_AH_NAMESPACE
|
