1#ifndef CONSTFILT_ELLIPTIC_HPP
2#define CONSTFILT_ELLIPTIC_HPP
4#include "analog_filter.hpp"
5#include "vendor/consteig/consteig.hpp"
6#include "vendor/gcem_wrapper.hpp"
32template <
typename T, consteig::Size N,
typename Method = TustinPW,
33 typename FilterType = LowPass>
35 :
public AnalogFilter<T, N, typename bind_method<T, Method>::type>
37 static_assert(N >= 1u,
"Elliptic order must be at least 1");
39 using BoundMethod =
typename bind_method<T, Method>::type;
42 constexpr Elliptic(T cutoff_hz, T ripple_db, T attenuation_db,
44 : AnalogFilter<T, N, BoundMethod>(
45 compute_continuous_tf(cutoff_hz, ripple_db, attenuation_db),
46 compute_factored_tf(cutoff_hz, ripple_db, attenuation_db,
48 sample_rate_hz, make_tustin_tag(cutoff_hz, BoundMethod{}))
53 using Complex = consteig::Complex<T>;
56 static constexpr consteig::Size M{N / 2u};
60 static constexpr double LN10_OVER_10{0.23025850929940457};
63 static constexpr int AGM_ITERATIONS{64};
67 static constexpr int SERIES_TERMS{30};
69 static constexpr int NOME_COEFF_Q0_5{2};
70 static constexpr int NOME_COEFF_Q0_9{15};
71 static constexpr int NOME_COEFF_Q0_13{150};
73 static constexpr TransferFunction<T, N + 1u, N + 1u> compute_continuous_tf(
74 T cutoff_hz, T ripple_db, T attenuation_db)
76 const T wc =
static_cast<T
>(2) *
static_cast<T
>(GCEM_PI) * cutoff_hz;
77 TransferFunction<T, N + 1u, N + 1u> tf{};
78 elliptic_tf(wc, ripple_db, attenuation_db, tf.b, tf.a, FilterType{});
87 static constexpr T elliptic_K(T k)
89 T a =
static_cast<T
>(1);
90 T b = gcem::sqrt(
static_cast<T
>(1) - k * k);
91 for (
int i = 0; i < AGM_ITERATIONS; ++i)
93 const T a2 = (a + b) /
static_cast<T
>(2);
94 const T b2 = gcem::sqrt(a * b);
98 return static_cast<T
>(GCEM_PI) / (
static_cast<T
>(2) * a);
102 static constexpr T from_db10(T x)
104 return gcem::exp(x *
static_cast<T
>(LN10_OVER_10));
110 static constexpr T compute_nome(T k)
112 const T kp = gcem::sqrt(
static_cast<T
>(1) - k * k);
113 const T sqrt_kp = gcem::sqrt(kp);
114 const T q0 =
static_cast<T
>(0.5) * (
static_cast<T
>(1) - sqrt_kp) /
115 (
static_cast<T
>(1) + sqrt_kp);
116 const T q0_2 = q0 * q0;
117 const T q0_4 = q0_2 * q0_2;
118 const T q0_5 = q0_4 * q0;
119 const T q0_9 = q0_5 * q0_4;
120 const T q0_13 = q0_9 * q0_4;
121 return q0 +
static_cast<T
>(NOME_COEFF_Q0_5) * q0_5 +
122 static_cast<T
>(NOME_COEFF_Q0_9) * q0_9 +
123 static_cast<T
>(NOME_COEFF_Q0_13) * q0_13;
136 static constexpr T modulus_from_nome(T q)
138 const T q14 = gcem::sqrt(gcem::sqrt(q));
144 T theta2 =
static_cast<T
>(0);
145 T qpow =
static_cast<T
>(1);
146 T q_2n =
static_cast<T
>(1);
147 for (
int n = 0; n <= SERIES_TERMS; ++n)
156 theta2 *=
static_cast<T
>(2) * q14;
159 T theta3 =
static_cast<T
>(1);
162 for (
int n = 1; n <= SERIES_TERMS; ++n)
169 theta3 +=
static_cast<T
>(2) * qpow3;
172 const T ratio = theta2 / theta3;
173 return ratio * ratio;
182 static constexpr T compute_sig0(T ripple_db, T q)
184 const T gain = from_db10(ripple_db /
static_cast<T
>(2));
196 gcem::log((gain +
static_cast<T
>(1)) / (gain -
static_cast<T
>(1))) /
197 (
static_cast<T
>(2) *
static_cast<T
>(N));
202 T sig01 =
static_cast<T
>(0);
203 T qpow1 =
static_cast<T
>(1);
204 T q_2m =
static_cast<T
>(1);
205 for (
int m = 0; m <= SERIES_TERMS; ++m)
213 (m % 2 == 0) ?
static_cast<T
>(1) :
static_cast<T
>(-1);
214 const T x =
static_cast<T
>(2 * m + 1) * l;
215 sig01 += sign * qpow1 * gcem::sinh(x);
219 T sig02 =
static_cast<T
>(0);
222 for (
int m = 1; m <= SERIES_TERMS; ++m)
230 (m % 2 == 0) ?
static_cast<T
>(1) :
static_cast<T
>(-1);
231 const T x =
static_cast<T
>(2 * m) * l;
232 sig02 += sign * qpow2 * gcem::cosh(x);
235 const T q14 = gcem::sqrt(gcem::sqrt(q));
237 const T sig0 =
static_cast<T
>(2) * q14 * sig01 /
238 (
static_cast<T
>(1) +
static_cast<T
>(2) * sig02);
240 return (sig0 <
static_cast<T
>(0)) ? -sig0 : sig0;
249 static constexpr T compute_wi(consteig::Size ii, T q)
251 const T mu = (N % 2u == 1u) ?
static_cast<T
>(ii)
252 :
static_cast<T
>(ii) -
static_cast<T
>(0.5);
253 const T q14 = gcem::sqrt(gcem::sqrt(q));
255 const T pi_mu_n =
static_cast<T
>(GCEM_PI) * mu /
static_cast<T
>(N);
258 T soma1 =
static_cast<T
>(0);
259 T qpow1 =
static_cast<T
>(1);
260 T q_2m =
static_cast<T
>(1);
261 for (
int m = 0; m <= SERIES_TERMS; ++m)
269 (m % 2 == 0) ?
static_cast<T
>(1) :
static_cast<T
>(-1);
270 const T arg =
static_cast<T
>(2 * m + 1) * pi_mu_n;
271 soma1 += sign * qpow1 * gcem::sin(arg);
273 soma1 *=
static_cast<T
>(2) * q14;
276 T soma2 =
static_cast<T
>(0);
279 for (
int m = 1; m <= SERIES_TERMS; ++m)
287 (m % 2 == 0) ?
static_cast<T
>(1) :
static_cast<T
>(-1);
288 const T arg =
static_cast<T
>(2 * m) * pi_mu_n;
289 soma2 += sign * qpow2 * gcem::cos(arg);
291 soma2 *=
static_cast<T
>(2);
293 return soma1 / (
static_cast<T
>(1) + soma2);
300 static constexpr void poly_mul_root(Complex (&poly)[N + 1u],
301 consteig::Size deg, Complex root)
303 for (consteig::Size j = deg; j > 0u; --j)
305 poly[j] = poly[j - 1u] - root * poly[j];
308 Complex{
static_cast<T
>(0),
static_cast<T
>(0)} - root * poly[0];
315 static constexpr void compute_prototype_poles_zeros(
316 T q, T sig0, T k, Complex (&poles)[N], consteig::Size &pole_cnt,
317 Complex (&zeros)[N], consteig::Size &zero_cnt)
319 const T ws =
static_cast<T
>(1) / k;
320 const T sqrt_ws = gcem::sqrt(ws);
321 const T w = gcem::sqrt((
static_cast<T
>(1) + k * sig0 * sig0) *
322 (
static_cast<T
>(1) + sig0 * sig0 / k));
327 for (consteig::Size ii = 1u; ii <= M; ++ii)
329 const T wi = compute_wi(ii, q);
330 const T Vi = gcem::sqrt((
static_cast<T
>(1) - k * wi * wi) *
331 (
static_cast<T
>(1) - wi * wi / k));
333 const T omega_z = sqrt_ws / wi;
334 zeros[zero_cnt++] = Complex{
static_cast<T
>(0), omega_z};
335 zeros[zero_cnt++] = Complex{
static_cast<T
>(0), -omega_z};
337 const T denom =
static_cast<T
>(1) + sig0 * sig0 * wi * wi;
338 const T p_re = sqrt_ws * (-sig0 * Vi) / denom;
339 const T p_im = sqrt_ws * (wi * w) / denom;
340 poles[pole_cnt++] = Complex{p_re, p_im};
341 poles[pole_cnt++] = Complex{p_re, -p_im};
346 poles[pole_cnt++] = Complex{-sig0 * sqrt_ws,
static_cast<T
>(0)};
364 static constexpr void elliptic_tf(T wc, T ripple_db, T attenuation_db,
365 T (&b)[N + 1u], T (&a)[N + 1u], LowPass)
369 const T ep = gcem::sqrt(from_db10(ripple_db) -
static_cast<T
>(1));
370 const T es = gcem::sqrt(from_db10(attenuation_db) -
static_cast<T
>(1));
371 const T k1 = ep / es;
378 const T q1 = compute_nome(k1);
379 const T q = gcem::exp(gcem::log(q1) /
static_cast<T
>(N));
380 const T k = modulus_from_nome(q);
388 const T sig0 = compute_sig0(ripple_db, q);
401 const T ws =
static_cast<T
>(1) / k;
402 const T sqrt_ws = gcem::sqrt(ws);
403 const T w = gcem::sqrt((
static_cast<T
>(1) + k * sig0 * sig0) *
404 (
static_cast<T
>(1) + sig0 * sig0 / k));
406 Complex poly_a[N + 1u]{};
407 Complex poly_b[N + 1u]{};
408 poly_a[0] = Complex{
static_cast<T
>(1),
static_cast<T
>(0)};
409 poly_b[0] = Complex{
static_cast<T
>(1),
static_cast<T
>(0)};
411 consteig::Size deg_a = 0u;
412 consteig::Size deg_b = 0u;
414 for (consteig::Size ii = 1u; ii <= M; ++ii)
416 const T wi = compute_wi(ii, q);
417 const T Vi = gcem::sqrt((
static_cast<T
>(1) - k * wi * wi) *
418 (
static_cast<T
>(1) - wi * wi / k));
420 const T omega_z = sqrt_ws / wi;
422 poly_mul_root(poly_b, deg_b, Complex{
static_cast<T
>(0), omega_z});
424 poly_mul_root(poly_b, deg_b, Complex{
static_cast<T
>(0), -omega_z});
426 const T denom =
static_cast<T
>(1) + sig0 * sig0 * wi * wi;
427 const T p_re = sqrt_ws * (-sig0 * Vi) / denom;
428 const T p_im = sqrt_ws * (wi * w) / denom;
431 poly_mul_root(poly_a, deg_a, Complex{p_re, p_im});
433 poly_mul_root(poly_a, deg_a, Complex{p_re, -p_im});
439 poly_mul_root(poly_a, deg_a,
440 Complex{-sig0 * sqrt_ws,
static_cast<T
>(0)});
444 for (consteig::Size i = 0u; i <= N; ++i)
446 a[i] = poly_a[N - i].real;
447 b[i] = poly_b[N - i].real;
454 static_cast<T
>(1) / gcem::sqrt(
static_cast<T
>(1) + ep * ep);
455 const T H0 = (N % 2u == 1u) ?
static_cast<T
>(1) : Gp;
456 const T gain = H0 * a[N] / b[N];
457 for (consteig::Size i = 0u; i <= N; ++i)
465 for (consteig::Size i = 0u; i <= N; ++i)
467 const T sc = gcem::pow(wc,
static_cast<int>(i));
478 static constexpr void elliptic_tf(T wc, T ripple_db, T attenuation_db,
479 T (&b)[N + 1u], T (&a)[N + 1u], HighPass)
483 elliptic_tf(
static_cast<T
>(1), ripple_db, attenuation_db, b_lp, a_lp,
486 for (consteig::Size j = 0u; j <= N; ++j)
488 const T sc = gcem::pow(wc,
static_cast<int>(j));
489 a[j] = a_lp[N - j] * sc;
490 b[j] = b_lp[N - j] * sc;
495 static constexpr FactoredTF<T, N> compute_factored_tf(T cutoff_hz,
500 const T wc =
static_cast<T
>(2) *
static_cast<T
>(GCEM_PI) * cutoff_hz;
501 const T ep = gcem::sqrt(from_db10(ripple_db) -
static_cast<T
>(1));
502 const T es = gcem::sqrt(from_db10(attenuation_db) -
static_cast<T
>(1));
503 const T k1 = ep / es;
504 const T q1 = compute_nome(k1);
505 const T q = gcem::exp(gcem::log(q1) /
static_cast<T
>(N));
506 const T k = modulus_from_nome(q);
507 const T sig0 = compute_sig0(ripple_db, q);
509 Complex poles_proto[N]{};
510 Complex zeros_proto[N]{};
511 consteig::Size pole_cnt = 0u;
512 consteig::Size zero_cnt = 0u;
513 compute_prototype_poles_zeros(q, sig0, k, poles_proto, pole_cnt,
514 zeros_proto, zero_cnt);
516 FactoredTF<T, N> factored_tf{};
517 factored_tf.nz = zero_cnt;
518 for (consteig::Size i = 0u; i < pole_cnt; ++i)
520 factored_tf.poles[i] =
521 Complex{wc * poles_proto[i].real, wc * poles_proto[i].imag};
523 for (consteig::Size i = 0u; i < zero_cnt; ++i)
525 factored_tf.zeros[i] =
526 Complex{wc * zeros_proto[i].real, wc * zeros_proto[i].imag};
533 elliptic_tf(wc, ripple_db, attenuation_db, b_tmp, a_tmp, LowPass{});
534 consteig::Size d_b = 0u;
535 while (d_b <= N && b_tmp[d_b] ==
static_cast<T
>(0))
540 (d_b > N) ?
static_cast<T
>(0) : b_tmp[d_b] / a_tmp[0];
549 static constexpr FactoredTF<T, N> compute_factored_tf(T cutoff_hz,
554 const T wc =
static_cast<T
>(2) *
static_cast<T
>(GCEM_PI) * cutoff_hz;
557 const T norm_cutoff =
558 static_cast<T
>(1) / (
static_cast<T
>(2) *
static_cast<T
>(GCEM_PI));
559 const FactoredTF<T, N> lp = compute_factored_tf(
560 norm_cutoff, ripple_db, attenuation_db, LowPass{});
562 FactoredTF<T, N> factored_tf{};
565 for (consteig::Size i = 0u; i < N; ++i)
567 const Complex &p = lp.poles[i];
568 const T denom_sq = p.real * p.real + p.imag * p.imag;
569 factored_tf.poles[i] =
570 Complex{wc * p.real / denom_sq, -wc * p.imag / denom_sq};
575 consteig::Size hp_nz = 0u;
576 for (consteig::Size i = 0u; i < lp.nz; ++i)
578 const Complex &z = lp.zeros[i];
579 const T denom_sq = z.real * z.real + z.imag * z.imag;
580 factored_tf.zeros[hp_nz++] =
581 Complex{wc * z.real / denom_sq, -wc * z.imag / denom_sq};
586 factored_tf.zeros[hp_nz++] =
587 Complex{
static_cast<T
>(0),
static_cast<T
>(0)};
589 factored_tf.nz = hp_nz;
594 elliptic_tf(wc, ripple_db, attenuation_db, b_tmp, a_tmp, HighPass{});
595 consteig::Size d_b = 0u;
596 while (d_b <= N && b_tmp[d_b] ==
static_cast<T
>(0))
601 (d_b > N) ?
static_cast<T
>(0) : b_tmp[d_b] / a_tmp[0];