Line data Source code
1 : #pragma once
2 :
3 : #include <AH/Containers/Array.hpp>
4 : #include <AH/STL/type_traits>
5 : #include <Filters/TransferFunction.hpp>
6 :
7 : /// @addtogroup FilterImplementations
8 : /// @{
9 :
10 : /**
11 : * @brief Infinite Impulse Response filter implementation that does not
12 : * normalize the coefficients upon initialization, the division by
13 : * @f$ a_0 @f$ is carried out on each filter iteration.
14 : *
15 : * This class is slower than @ref NormalizingIIRFilter, but it works better
16 : * for integer types, because it has no rounding error on the coefficients.
17 : *
18 : * Implements the following difference equation using the Direct-Form 1
19 : * implementation:
20 : *
21 : * @f[
22 : * y[n] = \frac{1}{a_0} \left(\sum_{i=0}^{N_b-1} b_i \cdot x[n-i]
23 : * - \sum_{i=1}^{N_a-1} a_i \cdot y[n-i] \right)
24 : * @f]
25 : */
26 : template <uint8_t NB, uint8_t NA, class T>
27 : class NonNormalizingIIRFilter {
28 : public:
29 : /**
30 : * @brief Construct a new Non-Normalizing IIR Filter object.
31 : *
32 : * The coefficients @f$ b @f$ and @f$ a @f$ can be derived from the transfer
33 : * function:
34 : *
35 : * @f[
36 : * H(z) = \frac{b_0 + b_1 z^{-1} + \ldots + b_{N_b} z ^{-N_b}}
37 : * {a_0 + a_1 z^{-1} + \ldots + a_{N_b} z ^{-N_a}}
38 : * @f]
39 : *
40 : * @param b_coefficients
41 : * The coefficients of the transfer function numerator.
42 : * @param a_coefficients
43 : * The coefficients of the transfer function denominator.
44 : */
45 4 : NonNormalizingIIRFilter(const AH::Array<T, NB> &b_coefficients,
46 : const AH::Array<T, NA> &a_coefficients)
47 4 : : a0(a_coefficients[0]) {
48 32 : for (uint8_t i = 0; i < 2 * NB - 1; ++i)
49 28 : this->b_coefficients[i] = b_coefficients[(2 * NB - 1 - i) % NB];
50 20 : for (uint8_t i = 0; i < 2 * MA - 1; ++i)
51 16 : this->a_coefficients[i] = a_coefficients[(2 * MA - 2 - i) % MA + 1];
52 4 : }
53 :
54 : NonNormalizingIIRFilter(const TransferFunction<NB, NA, T> &tf)
55 : : NonNormalizingIIRFilter{tf.b, tf.a} {}
56 :
57 : /**
58 : * @brief Update the internal state with the new input @f$ x[n] @f$ and
59 : * return the new output @f$ y[n] @f$.
60 : *
61 : * @param input
62 : * The new input @f$ x[n] @f$.
63 : * @return The new output @f$ y[n] @f$.
64 : */
65 80 : T operator()(T input) {
66 : // Save the new input to the ring buffer.
67 80 : x[index_b] = input;
68 :
69 : // Calculate the offset to the shifted coefficients.
70 80 : T *b_coeff_shift = b_coefficients.end() - NB - index_b;
71 80 : T *a_coeff_shift = a_coefficients.end() - MA - index_a;
72 :
73 : // Multiply and accumulate the inputs and their respective coefficients.
74 80 : T acc = {};
75 400 : for (uint8_t i = 0; i < NB; i++)
76 320 : acc += x[i] * b_coeff_shift[i];
77 :
78 : // Multiply and accumulate the inputs and their respective coefficients.
79 280 : for (uint8_t i = 0; i < MA; i++)
80 200 : acc -= y[i] * a_coeff_shift[i];
81 :
82 : // Save the current output
83 80 : acc /= a0;
84 80 : y[index_a] = acc;
85 :
86 : // Increment and wrap around the index of the ring buffer.
87 80 : index_b++;
88 80 : if (index_b == NB)
89 20 : index_b = 0;
90 :
91 80 : index_a++;
92 80 : if (index_a == MA)
93 35 : index_a = 0;
94 :
95 80 : return acc;
96 : }
97 :
98 : private:
99 : constexpr static uint8_t MA = NA - 1;
100 : uint8_t index_b = 0, index_a = 0;
101 : AH::Array<T, NB> x = {}; ///< Previous inputs
102 : AH::Array<T, MA> y = {}; ///< Previous outputs
103 : AH::Array<T, 2 * NB - 1> b_coefficients; ///< Numerator coefficients
104 : AH::Array<T, 2 * MA - 1> a_coefficients; ///< Denominator coefficients
105 : T a0;
106 : };
107 :
108 : /**
109 : * @brief Infinite Impulse Response filter implementation that normalizes the
110 : * coefficients upon initialization.
111 : *
112 : * This class is faster than @ref NonNormalizingIIRFilter, because each filter
113 : * iteration involves only addition and multiplication, no divisions.
114 : * It works great for floating point numbers, but might be less ideal
115 : * for integer types, because it can create large rounding errors on the
116 : * coefficients.
117 : *
118 : * Implements the following difference equation using the Direct-Form 1
119 : * implementation:
120 : *
121 : * @f[
122 : * y[n] = \frac{1}{a_0} \left(\sum_{i=0}^{N_b-1} b_i \cdot x[n-i]
123 : * - \sum_{i=1}^{N_a-1} a_i \cdot y[n-i] \right)
124 : * @f]
125 : */
126 : template <uint8_t NB, uint8_t NA, class T>
127 : class NormalizingIIRFilter {
128 : public:
129 : /**
130 : * @brief Construct a new Normalizing IIR Filter object.
131 : *
132 : * The coefficients @f$ b @f$ and @f$ a @f$ can be derived from the transfer
133 : * function:
134 : *
135 : * @f[
136 : * H(z) = \frac{b_0 + b_1 z^{-1} + \ldots + b_{N_b} z ^{-N_b}}
137 : * {a_0 + a_1 z^{-1} + \ldots + a_{N_b} z ^{-N_a}}
138 : * @f]
139 : *
140 : * @param b_coefficients
141 : * The coefficients of the transfer function numerator.
142 : * @param a_coefficients
143 : * The coefficients of the transfer function denominator.
144 : */
145 5 : NormalizingIIRFilter(const AH::Array<T, NB> &b_coefficients,
146 5 : const AH::Array<T, NA> &a_coefficients) {
147 5 : T a0 = a_coefficients[0];
148 50 : for (uint8_t i = 0; i < 2 * NB - 1; ++i)
149 90 : this->b_coefficients[i] =
150 90 : b_coefficients[(2 * NB - 1 - i) % NB] / a0;
151 36 : for (uint8_t i = 0; i < 2 * MA - 1; ++i)
152 62 : this->a_coefficients[i] =
153 62 : a_coefficients[(2 * MA - 2 - i) % MA + 1] / a0;
154 5 : }
155 :
156 1 : NormalizingIIRFilter(const TransferFunction<NB, NA, T> &tf)
157 1 : : NormalizingIIRFilter{tf.b, tf.a} {}
158 :
159 : /**
160 : * @brief Update the internal state with the new input @f$ x[n] @f$ and
161 : * return the new output @f$ y[n] @f$.
162 : *
163 : * @param input
164 : * The new input @f$ x[n] @f$.
165 : * @return The new output @f$ y[n] @f$.
166 : */
167 100 : T operator()(T input) {
168 : // Save the new input to the ring buffer.
169 100 : x[index_b] = input;
170 :
171 : // Calculate the offset to the shifted coefficients.
172 100 : T *b_coeff_shift = b_coefficients.end() - NB - index_b;
173 100 : T *a_coeff_shift = a_coefficients.end() - MA - index_a;
174 :
175 : // Multiply and accumulate the inputs and their respective coefficients.
176 100 : T acc = {};
177 600 : for (uint8_t i = 0; i < NB; i++)
178 500 : acc += x[i] * b_coeff_shift[i];
179 :
180 : // Multiply and accumulate the inputs and their respective coefficients.
181 460 : for (uint8_t i = 0; i < MA; i++)
182 360 : acc -= y[i] * a_coeff_shift[i];
183 :
184 : // Save the current output
185 100 : y[index_a] = acc;
186 :
187 : // Increment and wrap around the index of the ring buffer.
188 100 : index_b++;
189 100 : if (index_b == NB)
190 22 : index_b = 0;
191 :
192 100 : index_a++;
193 100 : if (index_a == MA)
194 37 : index_a = 0;
195 :
196 100 : return acc;
197 : }
198 :
199 : private:
200 : constexpr static uint8_t MA = NA - 1;
201 : uint8_t index_b = 0, index_a = 0;
202 : AH::Array<T, NB> x = {};
203 : AH::Array<T, MA> y = {};
204 : AH::Array<T, 2 * NB - 1> b_coefficients;
205 : AH::Array<T, 2 * MA - 1> a_coefficients;
206 : };
207 :
208 : /// @}
209 :
210 : /// Select the @ref NormalizingIIRFilter implementation if @p T is a floating
211 : /// point type, @ref NonNormalizingIIRFilter otherwise.
212 : template <uint8_t NB, uint8_t NA, class T>
213 : using IIRImplementation =
214 : typename std::conditional<std::is_floating_point<T>::value,
215 : NormalizingIIRFilter<NB, NA, T>,
216 : NonNormalizingIIRFilter<NB, NA, T>>::type;
217 :
218 : /// @addtogroup Filters
219 : /// @{
220 :
221 : /**
222 : * @brief Generic Infinite Impulse Response filter class.
223 : *
224 : * Uses the @ref NormalizingIIRFilter implementation for floating point
225 : * types, and @ref NonNormalizingIIRFilter for all other types.
226 : *
227 : * Implements the following difference equation using the Direct-Form 1
228 : * implementation:
229 : *
230 : * @f[
231 : * y[n] = \frac{1}{a_0} \left(\sum_{i=0}^{N_b-1} b_i \cdot x[n-i]
232 : * - \sum_{i=1}^{N_a-1} a_i \cdot y[n-i] \right)
233 : * @f]
234 : */
235 : template <uint8_t NB, uint8_t NA = NB, class T = float>
236 : class IIRFilter : public IIRImplementation<NB, NA, T> {
237 : public:
238 : /**
239 : * @brief Construct a new IIR Filter object.
240 : *
241 : * The coefficients @f$ b @f$ and @f$ a @f$ can be derived from the transfer
242 : * function:
243 : *
244 : * @f[
245 : * H(z) = \frac{b_0 + b_1 z^{-1} + \ldots + b_{N_b} z ^{-N_b}}
246 : * {a_0 + a_1 z^{-1} + \ldots + a_{N_b} z ^{-N_a}}
247 : * @f]
248 : *
249 : * @param b_coefficients
250 : * The coefficients of the transfer function numerator.
251 : * @param a_coefficients
252 : * The coefficients of the transfer function denominator.
253 : */
254 8 : IIRFilter(const AH::Array<T, NB> &b_coefficients,
255 : const AH::Array<T, NA> &a_coefficients)
256 8 : : IIRImplementation<NB, NA, T>{b_coefficients, a_coefficients} {}
257 :
258 1 : IIRFilter(const TransferFunction<NB, NA, T> &tf)
259 1 : : IIRImplementation<NB, NA, T>{tf} {}
260 :
261 : /**
262 : * @brief Update the internal state with the new input @f$ x[n] @f$ and
263 : * return the new output @f$ y[n] @f$.
264 : *
265 : * @param input
266 : * The new input @f$ x[n] @f$.
267 : * @return The new output @f$ y[n] @f$.
268 : */
269 180 : T operator()(T input) {
270 180 : return IIRImplementation<NB, NA, T>::operator()(input);
271 : }
272 : };
273 :
274 : /// Create an IIRFilter from the given transfer function.
275 : template <size_t NB, size_t NA, class T = float>
276 1 : IIRFilter<NB, NA, T> makeIIRFilter(const TransferFunction<NB, NA, T> &tf) {
277 1 : return tf;
278 : }
279 :
280 : /// Create an IIRFilter from the given transfer function coefficients.
281 : template <size_t NB, size_t NA, class T = float>
282 : IIRFilter<NB, NA, T> makeIIRFilter(const AH::Array<T, NB> &b_coefficients,
283 : const AH::Array<T, NA> &a_coefficients) {
284 : return {b_coefficients, a_coefficients};
285 : }
286 :
287 : /// @}
|
