Line data Source code
1 : #pragma once
2 :
3 : #include <AH/Containers/Array.hpp>
4 : #include <AH/STL/cmath>
5 : #include <Filters/TransferFunction.hpp>
6 :
7 : /// @addtogroup Filters
8 : /// @{
9 :
10 : /// BiQuadratic transfer function coefficients.
11 : template <class T = float>
12 : using BiQuadCoefficients = TransferFunction<3, 3, T>;
13 :
14 : /// @}
15 :
16 : /// @addtogroup FilterImplementations
17 : /// @{
18 :
19 : // Direct Form 1 :::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
20 :
21 : /**
22 : * @brief BiQuad filter Direct Form 1 implementation that normalizes the
23 : * coefficients upon initialization.
24 : *
25 : * This class is faster than @ref NonNormalizingBiQuadFilterDF1, because each
26 : * filter iteration involves only addition and multiplication, no divisions.
27 : * It works great for floating point numbers, but might be less ideal
28 : * for integer types, because it can create large rounding errors on the
29 : * coefficients.
30 : *
31 : * Implements the following difference equation:
32 : *
33 : * @f[
34 : * y[n] = \frac{b_0 \cdot x[n] + b_1 \cdot x[n-1] + b_2 \cdot x[n-2]
35 : * - a_1 \cdot y[n-1] - a_2 \cdot y[n-2]}{a_0}
36 : * @f]
37 : */
38 : template <class T>
39 : class NormalizingBiQuadFilterDF1 {
40 : public:
41 : NormalizingBiQuadFilterDF1() = default;
42 :
43 47 : NormalizingBiQuadFilterDF1(const AH::Array<T, 3> &b,
44 : const AH::Array<T, 3> &a)
45 47 : : b(b / a[0]), a(-a.template slice<1, 2>() / a[0]) {}
46 :
47 46 : NormalizingBiQuadFilterDF1(const BiQuadCoefficients<T> &coefficients)
48 46 : : NormalizingBiQuadFilterDF1{coefficients.b, coefficients.a} {}
49 :
50 : NormalizingBiQuadFilterDF1(const AH::Array<T, 3> &b,
51 : const AH::Array<T, 3> &a, T gain)
52 : : b(b * gain / a[0]), a(-a.template slice<1, 2>() / a[0]) {}
53 :
54 : NormalizingBiQuadFilterDF1(const BiQuadCoefficients<T> &coefficients,
55 : T gain)
56 : : NormalizingBiQuadFilterDF1{coefficients.b, coefficients.a, gain} {}
57 :
58 : template <bool Enable = true>
59 : static std::enable_if_t<std::is_floating_point<T>::value && Enable, T>
60 3188 : update(T input, AH::Array<T, 2> &x, AH::Array<T, 2> &y,
61 : const AH::Array<T, 3> &b, const AH::Array<T, 2> &a) {
62 3188 : T acc = input * b[0];
63 3188 : acc = std::fma(x[0], b[1], acc);
64 3188 : acc = std::fma(x[1], b[2], acc);
65 3188 : acc = std::fma(y[0], a[0], acc);
66 3188 : acc = std::fma(y[1], a[1], acc);
67 3188 : x[1] = x[0];
68 3188 : x[0] = input;
69 3188 : y[1] = y[0];
70 3188 : y[0] = acc;
71 3188 : return y[0];
72 : }
73 :
74 : template <bool Enable = true>
75 : static std::enable_if_t<!std::is_floating_point<T>::value && Enable, T>
76 : update(T input, AH::Array<T, 2> &x, AH::Array<T, 2> &y,
77 : const AH::Array<T, 3> &b, const AH::Array<T, 2> &a) {
78 : T acc = input * b[0];
79 : acc = x[0] * b[1] + acc;
80 : acc = x[1] * b[2] + acc;
81 : acc = y[0] * a[0] + acc;
82 : acc = y[1] * a[1] + acc;
83 : x[1] = x[0];
84 : x[0] = input;
85 : y[1] = y[0];
86 : y[0] = acc;
87 : return y[0];
88 : }
89 :
90 : /**
91 : * @brief Update the internal state with the new input @f$ x[n] @f$ and
92 : * return the new output @f$ y[n] @f$.
93 : *
94 : * @param input
95 : * The new input @f$ x[n] @f$.
96 : * @return The new output @f$ y[n] @f$.
97 : */
98 3188 : T operator()(T input) { return update(input, x, y, b, a); }
99 :
100 : private:
101 : AH::Array<T, 2> x = {{}}; ///< Previous inputs
102 : AH::Array<T, 2> y = {{}}; ///< Previous outputs
103 : AH::Array<T, 3> b = {{}}; ///< Numerator coefficients
104 : AH::Array<T, 2> a = {{}}; ///< Denominator coefficients
105 : };
106 :
107 : /**
108 : * @brief BiQuad filter Direct Form 1 implementation that does not
109 : * normalize the coefficients upon initialization, the division by
110 : * @f$ a_0 @f$ is carried out on each filter iteration.
111 : *
112 : * This class is slower than @ref NormalizingBiQuadFilterDF1, but it works
113 : * better for integer types, because it has no rounding error on the
114 : * coefficients.
115 : *
116 : * Implements the following difference equation:
117 : *
118 : * @f[
119 : * y[n] = \frac{b_0 \cdot x[n] + b_1 \cdot x[n-1] + b_2 \cdot x[n-2]
120 : * - a_1 \cdot y[n-1] - a_2 \cdot y[n-2]}{a_0}
121 : * @f]
122 : */
123 : template <class T>
124 : class NonNormalizingBiQuadFilterDF1 {
125 : public:
126 30 : NonNormalizingBiQuadFilterDF1() = default;
127 :
128 33 : NonNormalizingBiQuadFilterDF1(const AH::Array<T, 3> &b,
129 : const AH::Array<T, 3> &a)
130 33 : : b(b), a(-a.template slice<1, 2>()), a0(a[0]) {}
131 :
132 32 : NonNormalizingBiQuadFilterDF1(const BiQuadCoefficients<T> &coefficients)
133 32 : : NonNormalizingBiQuadFilterDF1{coefficients.b, coefficients.a} {}
134 :
135 : NonNormalizingBiQuadFilterDF1(const AH::Array<T, 3> &b,
136 : const AH::Array<T, 3> &a, T gain)
137 : : b(b * gain), a(-a.template slice<1, 2>()), a0(a[0]) {}
138 :
139 : NonNormalizingBiQuadFilterDF1(const BiQuadCoefficients<T> &coefficients,
140 : T gain)
141 : : NonNormalizingBiQuadFilterDF1{coefficients.b, coefficients.a, gain} {}
142 :
143 : template <bool Enable = true>
144 : static std::enable_if_t<std::is_floating_point<T>::value && Enable, T>
145 : update(T input, AH::Array<T, 2> &x, AH::Array<T, 2> &y,
146 : const AH::Array<T, 3> &b, const AH::Array<T, 2> &a, T a0) {
147 : T acc = input * b[0];
148 : acc = std::fma(x[0], b[1], acc);
149 : acc = std::fma(x[1], b[2], acc);
150 : acc = std::fma(y[0], a[0], acc);
151 : acc = std::fma(y[1], a[1], acc);
152 : x[1] = x[0];
153 : x[0] = input;
154 : y[1] = y[0];
155 : y[0] = acc / a0;
156 : return y[0];
157 : }
158 :
159 : template <bool Enable = true>
160 : static std::enable_if_t<!std::is_floating_point<T>::value && Enable, T>
161 2908 : update(T input, AH::Array<T, 2> &x, AH::Array<T, 2> &y,
162 : const AH::Array<T, 3> &b, const AH::Array<T, 2> &a, T a0) {
163 2908 : T acc = input * b[0];
164 2908 : acc = x[0] * b[1] + acc;
165 2908 : acc = x[1] * b[2] + acc;
166 2908 : acc = y[0] * a[0] + acc;
167 2908 : acc = y[1] * a[1] + acc;
168 2908 : x[1] = x[0];
169 2908 : x[0] = input;
170 2908 : y[1] = y[0];
171 2908 : y[0] = acc / a0;
172 2908 : return y[0];
173 : }
174 :
175 : /**
176 : * @brief Update the internal state with the new input @f$ x[n] @f$ and
177 : * return the new output @f$ y[n] @f$.
178 : *
179 : * @param input
180 : * The new input @f$ x[n] @f$.
181 : * @return The new output @f$ y[n] @f$.
182 : */
183 2908 : T operator()(T input) { return update(input, x, y, b, a, a0); }
184 :
185 : private:
186 : AH::Array<T, 2> x = {{}}; ///< Previous inputs
187 : AH::Array<T, 2> y = {{}}; ///< Previous outputs
188 : AH::Array<T, 3> b = {{}}; ///< Numerator coefficients
189 : AH::Array<T, 2> a = {{}}; ///< Denominator coefficients
190 : T a0 = T(1.); ///< First denominator coefficient
191 : };
192 :
193 : /// @}
194 :
195 : /// Select the @ref NormalizingIIRFilter implementation if @p T is a floating
196 : /// point type, @ref NonNormalizingIIRFilter otherwise.
197 : template <class T>
198 : using BiQuadDF1Implementation =
199 : typename std::conditional<std::is_floating_point<T>::value,
200 : NormalizingBiQuadFilterDF1<T>,
201 : NonNormalizingBiQuadFilterDF1<T>>::type;
202 :
203 : /// @addtogroup Filters
204 : /// @{
205 :
206 : /**
207 : * @brief Generic BiQuad (Bi-Quadratic) filter class, Direct Form 1
208 : * implementation.
209 : *
210 : * Uses the @ref NormalizingBiQuadFilterDF1 implementation for floating point
211 : * types, and @ref NonNormalizingBiQuadFilterDF1 for all other types.
212 : *
213 : * Implements the following difference equation:
214 : *
215 : * @f[
216 : * y[n] = \frac{b_0 \cdot x[n] + b_1 \cdot x[n-1] + b_2 \cdot x[n-2]
217 : * - a_1 \cdot y[n-1] - a_2 \cdot y[n-2]}{a_0}
218 : * @f]
219 : */
220 : template <class T = float>
221 : class BiQuadFilterDF1 : public BiQuadDF1Implementation<T> {
222 : public:
223 30 : BiQuadFilterDF1() = default;
224 :
225 : /**
226 : * @brief Construct a new BiQuad (Bi-Quadratic) Filter object.
227 : *
228 : * The coefficients @f$ b @f$ and @f$ a @f$ can be derived from the transfer
229 : * function:
230 : *
231 : * @f[
232 : * H(z) = \frac{b_0 + b_1 z^{-1} + b_2 z ^{-2}}
233 : * {a_0 + a_1 z^{-1} + a_2 z ^{-2}}
234 : * @f]
235 : *
236 : * @param b_coefficients
237 : * The coefficients of the transfer function numerator.
238 : * @param a_coefficients
239 : * The coefficients of the transfer function denominator.
240 : */
241 2 : BiQuadFilterDF1(const AH::Array<T, 3> &b_coefficients,
242 : const AH::Array<T, 3> &a_coefficients)
243 2 : : BiQuadDF1Implementation<T>{b_coefficients, a_coefficients} {}
244 :
245 78 : BiQuadFilterDF1(const BiQuadCoefficients<T> &coefficients)
246 78 : : BiQuadDF1Implementation<T>{coefficients} {}
247 :
248 : /**
249 : * @brief Construct a new BiQuad (Bi-Quadratic) Filter object.
250 : *
251 : * The coefficients @f$ b @f$ and @f$ a @f$ can be derived from the transfer
252 : * function:
253 : *
254 : * @f[
255 : * H(z) = K \frac{b_0 + b_1 z^{-1} + b_2 z ^{-2}}
256 : * {a_0 + a_1 z^{-1} + a_2 z ^{-2}}
257 : * @f]
258 : *
259 : * @param b_coefficients
260 : * The coefficients of the transfer function numerator.
261 : * @param a_coefficients
262 : * The coefficients of the transfer function denominator.
263 : * @param gain
264 : * Gain factor @f$ K @f$.
265 : */
266 : BiQuadFilterDF1(const AH::Array<T, 3> &b_coefficients,
267 : const AH::Array<T, 3> &a_coefficients, T gain)
268 : : BiQuadDF1Implementation<T>{b_coefficients, a_coefficients, gain} {}
269 :
270 : BiQuadFilterDF1(const BiQuadCoefficients<T> &coefficients, T gain)
271 : : BiQuadDF1Implementation<T>{coefficients, gain} {}
272 :
273 : /**
274 : * @brief Update the internal state with the new input @f$ x[n] @f$ and
275 : * return the new output @f$ y[n] @f$.
276 : *
277 : * @param input
278 : * The new input @f$ x[n] @f$.
279 : * @return The new output @f$ y[n] @f$.
280 : */
281 6096 : T operator()(T input) {
282 6096 : return BiQuadDF1Implementation<T>::operator()(input);
283 : }
284 : };
285 :
286 : /// @}
287 :
288 : // Direct Form 2 :::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
289 :
290 : /// @addtogroup FilterImplementations
291 : /// @{
292 :
293 : /**
294 : * @brief BiQuad filter Direct Form 2 implementation that normalizes the
295 : * coefficients upon initialization.
296 : *
297 : * This class is faster than @ref NonNormalizingBiQuadFilterDF2, because each
298 : * filter iteration involves only addition and multiplication, no divisions.
299 : * It works great for floating point numbers, but might be less ideal
300 : * for integer types, because it can create large rounding errors on the
301 : * coefficients.
302 : *
303 : * Implements the following difference equation:
304 : *
305 : * @f[
306 : * y[n] = \frac{b_0 \cdot x[n] + b_1 \cdot x[n-1] + b_2 \cdot x[n-2]
307 : * - a_1 \cdot y[n-1] - a_2 \cdot y[n-2]}{a_0}
308 : * @f]
309 : */
310 : template <class T>
311 : class NormalizingBiQuadFilterDF2 {
312 : public:
313 : NormalizingBiQuadFilterDF2() = default;
314 :
315 1 : NormalizingBiQuadFilterDF2(const AH::Array<T, 3> &b,
316 : const AH::Array<T, 3> &a)
317 1 : : b(b / a[0]), a(-a.template slice<1, 2>() / a[0]) {}
318 :
319 : NormalizingBiQuadFilterDF2(const BiQuadCoefficients<T> &coefficients)
320 : : NormalizingBiQuadFilterDF2{coefficients.b, coefficients.a} {}
321 :
322 : NormalizingBiQuadFilterDF2(const AH::Array<T, 3> &b,
323 : const AH::Array<T, 3> &a, T gain)
324 : : b(b * gain / a[0]), a(-a.template slice<1, 2>() / a[0]) {}
325 :
326 : NormalizingBiQuadFilterDF2(const BiQuadCoefficients<T> &coefficients,
327 : T gain)
328 : : NormalizingBiQuadFilterDF2{coefficients.b, coefficients.a, gain} {}
329 :
330 : template <bool Enable = true>
331 : static std::enable_if_t<std::is_floating_point<T>::value && Enable, T>
332 20 : update(T input, AH::Array<T, 2> &w, const AH::Array<T, 3> &b,
333 : const AH::Array<T, 2> &a) {
334 20 : input = std::fma(a[0], w[0], input);
335 20 : input = std::fma(a[1], w[1], input);
336 20 : T result = b[0] * input;
337 20 : result = std::fma(b[1], w[0], result);
338 20 : result = std::fma(b[2], w[1], result);
339 20 : w[1] = w[0];
340 20 : w[0] = input;
341 20 : return result;
342 : }
343 :
344 : template <bool Enable = true>
345 : static std::enable_if_t<!std::is_floating_point<T>::value && Enable, T>
346 : update(T input, AH::Array<T, 2> &w, const AH::Array<T, 3> &b,
347 : const AH::Array<T, 2> &a) {
348 : input += a[0] * w[0];
349 : input += a[1] * w[1];
350 : T result = b[0] * input;
351 : result += b[1] * w[0];
352 : result += b[2] * w[1];
353 : w[1] = w[0];
354 : w[0] = input;
355 : return result;
356 : }
357 :
358 : /**
359 : * @brief Update the internal state with the new input @f$ x[n] @f$ and
360 : * return the new output @f$ y[n] @f$.
361 : *
362 : * @param input
363 : * The new input @f$ x[n] @f$.
364 : * @return The new output @f$ y[n] @f$.
365 : */
366 20 : T operator()(T input) { return update(input, w, b, a); }
367 :
368 : private:
369 : AH::Array<T, 2> w = {{}}; ///< Internal state
370 : AH::Array<T, 3> b = {{}}; ///< Numerator coefficients
371 : AH::Array<T, 2> a = {{}}; ///< Denominator coefficients
372 : };
373 :
374 : /**
375 : * @brief BiQuad filter Direct Form 2 implementation that does not
376 : * normalize the coefficients upon initialization, the division by
377 : * @f$ a_0 @f$ is carried out on each filter iteration.
378 : *
379 : * This class is slower than @ref NormalizingBiQuadFilterDF2, but it works
380 : * better for integer types, because it has no rounding error on the
381 : * coefficients.
382 : *
383 : * Implements the following difference equation:
384 : *
385 : * @f[
386 : * y[n] = \frac{b_0 \cdot x[n] + b_1 \cdot x[n-1] + b_2 \cdot x[n-2]
387 : * - a_1 \cdot y[n-1] - a_2 \cdot y[n-2]}{a_0}
388 : * @f]
389 : */
390 : template <class T>
391 : class NonNormalizingBiQuadFilterDF2 {
392 : public:
393 : NonNormalizingBiQuadFilterDF2() = default;
394 :
395 1 : NonNormalizingBiQuadFilterDF2(const AH::Array<T, 3> &b,
396 : const AH::Array<T, 3> &a)
397 1 : : b(b), a(-a.template slice<1, 2>()), a0(a[0]) {}
398 :
399 : NonNormalizingBiQuadFilterDF2(const BiQuadCoefficients<T> &coefficients)
400 : : NonNormalizingBiQuadFilterDF2{coefficients.b, coefficients.a} {}
401 :
402 : NonNormalizingBiQuadFilterDF2(const AH::Array<T, 3> &b,
403 : const AH::Array<T, 3> &a, T gain)
404 : : b(b * gain), a(-a.template slice<1, 2>()), a0(a[0]) {}
405 :
406 : NonNormalizingBiQuadFilterDF2(const BiQuadCoefficients<T> &coefficients,
407 : T gain)
408 : : NonNormalizingBiQuadFilterDF2{coefficients.b, coefficients.a, gain} {}
409 :
410 : template <bool Enable = true>
411 : static std::enable_if_t<std::is_floating_point<T>::value && Enable, T>
412 : update(T input, AH::Array<T, 2> &w, const AH::Array<T, 3> &b,
413 : const AH::Array<T, 2> &a, T a0) {
414 : input = std::fma(a[0], w[0], input);
415 : input = std::fma(a[1], w[1], input);
416 : input /= a0;
417 : T result = b[0] * input;
418 : result = std::fma(b[1], w[0], result);
419 : result = std::fma(b[2], w[1], result);
420 : w[1] = w[0];
421 : w[0] = input;
422 : return result;
423 : }
424 :
425 : template <bool Enable = true>
426 : static std::enable_if_t<!std::is_floating_point<T>::value && Enable, T>
427 20 : update(T input, AH::Array<T, 2> &w, const AH::Array<T, 3> &b,
428 : const AH::Array<T, 2> &a, T a0) {
429 20 : input += a[0] * w[0];
430 20 : input += a[1] * w[1];
431 20 : input /= a0;
432 20 : T result = b[0] * input;
433 20 : result += b[1] * w[0];
434 20 : result += b[2] * w[1];
435 20 : w[1] = w[0];
436 20 : w[0] = input;
437 20 : return result;
438 : }
439 :
440 : /**
441 : * @brief Update the internal state with the new input @f$ x[n] @f$ and
442 : * return the new output @f$ y[n] @f$.
443 : *
444 : * @param input
445 : * The new input @f$ x[n] @f$.
446 : * @return The new output @f$ y[n] @f$.
447 : */
448 20 : T operator()(T input) { return update(input, w, b, a, a0); }
449 :
450 : private:
451 : AH::Array<T, 2> w = {{}}; ///< Internal state
452 : AH::Array<T, 3> b = {{}}; ///< Numerator coefficients
453 : AH::Array<T, 2> a = {{}}; ///< Denominator coefficients
454 : T a0 = T(1.); ///< First denominator coefficient
455 : };
456 :
457 : /// @}
458 :
459 : /// Select the @ref NormalizingIIRFilter implementation if @p T is a floating
460 : /// point type, @ref NonNormalizingIIRFilter otherwise.
461 : template <class T>
462 : using BiQuadDF2Implementation =
463 : typename std::conditional<std::is_floating_point<T>::value,
464 : NormalizingBiQuadFilterDF2<T>,
465 : NonNormalizingBiQuadFilterDF2<T>>::type;
466 :
467 : /// @addtogroup Filters
468 : /// @{
469 :
470 : /**
471 : * @brief Generic BiQuad (Bi-Quadratic) filter class, Direct Form 2
472 : * implementation.
473 : *
474 : * Uses the @ref NormalizingBiQuadFilterDF2 implementation for floating point
475 : * types, and @ref NonNormalizingBiQuadFilterDF2 for all other types.
476 : *
477 : * Implements the following difference equation:
478 : *
479 : * @f[
480 : * y[n] = \frac{b_0 \cdot x[n] + b_1 \cdot x[n-1] + b_2 \cdot x[n-2]
481 : * - a_1 \cdot y[n-1] - a_2 \cdot y[n-2]}{a_0}
482 : * @f]
483 : */
484 : template <class T = float>
485 : class BiQuadFilterDF2 : public BiQuadDF2Implementation<T> {
486 : public:
487 : BiQuadFilterDF2() = default;
488 :
489 : /**
490 : * @brief Construct a new BiQuad (Bi-Quadratic) Filter object.
491 : *
492 : * The coefficients @f$ b @f$ and @f$ a @f$ can be derived from the transfer
493 : * function:
494 : *
495 : * @f[
496 : * H(z) = \frac{b_0 + b_1 z^{-1} + b_2 z ^{-2}}
497 : * {a_0 + a_1 z^{-1} + a_2 z ^{-2}}
498 : * @f]
499 : *
500 : * @param b_coefficients
501 : * The coefficients of the transfer function numerator.
502 : * @param a_coefficients
503 : * The coefficients of the transfer function denominator.
504 : */
505 2 : BiQuadFilterDF2(const AH::Array<T, 3> &b_coefficients,
506 : const AH::Array<T, 3> &a_coefficients)
507 2 : : BiQuadDF2Implementation<T>{b_coefficients, a_coefficients} {}
508 :
509 : BiQuadFilterDF2(const BiQuadCoefficients<T> &coefficients)
510 : : BiQuadDF2Implementation<T>{coefficients} {}
511 :
512 : /**
513 : * @brief Construct a new BiQuad (Bi-Quadratic) Filter object.
514 : *
515 : * The coefficients @f$ b @f$ and @f$ a @f$ can be derived from the transfer
516 : * function:
517 : *
518 : * @f[
519 : * H(z) = K \frac{b_0 + b_1 z^{-1} + b_2 z ^{-2}}
520 : * {a_0 + a_1 z^{-1} + a_2 z ^{-2}}
521 : * @f]
522 : *
523 : * @param b_coefficients
524 : * The coefficients of the transfer function numerator.
525 : * @param a_coefficients
526 : * The coefficients of the transfer function denominator.
527 : * @param gain
528 : * Gain factor @f$ K @f$.
529 : */
530 : BiQuadFilterDF2(const AH::Array<T, 3> &b_coefficients,
531 : const AH::Array<T, 3> &a_coefficients, T gain)
532 : : BiQuadDF2Implementation<T>{b_coefficients, a_coefficients, gain} {}
533 :
534 : BiQuadFilterDF2(const BiQuadCoefficients<T> &coefficients, T gain)
535 : : BiQuadDF2Implementation<T>{coefficients, gain} {}
536 :
537 : /**
538 : * @brief Update the internal state with the new input @f$ x[n] @f$ and
539 : * return the new output @f$ y[n] @f$.
540 : *
541 : * @param input
542 : * The new input @f$ x[n] @f$.
543 : * @return The new output @f$ y[n] @f$.
544 : */
545 40 : T operator()(T input) {
546 40 : return BiQuadDF2Implementation<T>::operator()(input);
547 : }
548 : };
549 :
550 : /// @}
|
