/* GSL - Generic Sound Layer * Copyright (C) 2001 Stefan Westerfeld and Tim Janik * * This library is free software; you can redistribute it and/or * modify it under the terms of the GNU Lesser General Public * License as published by the Free Software Foundation; either * version 2 of the License, or (at your option) any later version. * * This library is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * Lesser General Public License for more details. * * You should have received a copy of the GNU Lesser General * Public License along with this library; if not, write to the * Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, * Boston, MA 02110-1301, USA. */ #include "gslfilter.h" #include "gslfft.h" #include "gslsignal.h" #include /* --- common utilities --- */ static inline double cotan (double x) { return - tan (x + GSL_PI * 0.5); } static double gsl_db_invert (double x) { /* db = 20*log(x)/log(10); */ return exp (x * log (10) / 20.0); } static void band_filter_common (unsigned int iorder, double p_freq, /* 0..pi */ double s_freq, /* 0..pi */ double epsilon, GslComplex *roots, GslComplex *poles, double *a, /* [0..iorder] */ double *b, gboolean band_pass, gboolean t1_norm) { unsigned int iorder2 = iorder >> 1; GslComplex *poly = g_newa (GslComplex, iorder + 1); GslComplex fpoly[2 + 1] = { { 0, }, { 0, }, { 1, 0 } }; double alpha, norm; guint i; epsilon = gsl_trans_zepsilon2ss (epsilon); alpha = cos ((s_freq + p_freq) * 0.5) / cos ((s_freq - p_freq) * 0.5); fpoly[0] = gsl_complex (1, 0); fpoly[1] = gsl_complex (1, 0); for (i = 0; i < iorder2; i++) { fpoly[0] = gsl_complex_mul (fpoly[0], gsl_complex_sub (gsl_complex (1, 0), gsl_complex_reciprocal (roots[i]))); fpoly[1] = gsl_complex_mul (fpoly[1], gsl_complex_sub (gsl_complex (1, 0), gsl_complex_reciprocal (poles[i]))); } norm = gsl_complex_div (fpoly[1], fpoly[0]).re; if ((iorder2 & 1) == 0) /* norm is fluctuation minimum */ norm *= sqrt (1.0 / (1.0 + epsilon * epsilon)); /* z nominator polynomial */ poly[0] = gsl_complex (norm, 0); for (i = 0; i < iorder2; i++) { GslComplex t, alphac = gsl_complex (alpha, 0); t = band_pass ? gsl_complex_inv (roots[i]) : roots[i]; fpoly[1] = gsl_complex_sub (gsl_complex_div (alphac, t), alphac); fpoly[0] = gsl_complex_inv (gsl_complex_reciprocal (t)); gsl_cpoly_mul (poly, i * 2, poly, 2, fpoly); } for (i = 0; i <= iorder; i++) a[i] = poly[i].re; /* z denominator polynomial */ poly[0] = gsl_complex (1, 0); for (i = 0; i < iorder2; i++) { GslComplex t, alphac = gsl_complex (alpha, 0); t = band_pass ? gsl_complex_inv (poles[i]) : poles[i]; fpoly[1] = gsl_complex_sub (gsl_complex_div (alphac, t), alphac); fpoly[0] = gsl_complex_inv (gsl_complex_reciprocal (t)); gsl_cpoly_mul (poly, i * 2, poly, 2, fpoly); } for (i = 0; i <= iorder; i++) b[i] = poly[i].re; gsl_poly_scale (iorder, a, 1.0 / b[0]); gsl_poly_scale (iorder, b, 1.0 / b[0]); } static void filter_rp_to_z (unsigned int iorder, GslComplex *roots, /* [0..iorder-1] */ GslComplex *poles, double *a, /* [0..iorder] */ double *b) { GslComplex *poly = g_newa (GslComplex, iorder + 1); guint i; /* z nominator polynomial */ poly[0] = gsl_complex (1, 0); for (i = 0; i < iorder; i++) gsl_cpoly_mul_reciprocal (i + 1, poly, roots[i]); for (i = 0; i <= iorder; i++) a[i] = poly[i].re; /* z denominator polynomial */ poly[0] = gsl_complex (1, 0); for (i = 0; i < iorder; i++) gsl_cpoly_mul_reciprocal (i + 1, poly, poles[i]); for (i = 0; i <= iorder; i++) b[i] = poly[i].re; } static void filter_lp_invert (unsigned int iorder, double *a, /* [0..iorder] */ double *b) { guint i; for (i = 1; i <= iorder; i +=2) { a[i] = -a[i]; b[i] = -b[i]; } } /* --- butterworth filter --- */ void gsl_filter_butter_rp (unsigned int iorder, double freq, /* 0..pi */ double epsilon, GslComplex *roots, /* [0..iorder-1] */ GslComplex *poles) { double pi = GSL_PI, order = iorder; double beta_mul = pi / (2.0 * order); /* double kappa = gsl_trans_freq2s (freq); */ double kappa; GslComplex root; unsigned int i; epsilon = gsl_trans_zepsilon2ss (epsilon); kappa = gsl_trans_freq2s (freq) * pow (epsilon, -1.0 / order); /* construct poles for butterworth filter */ for (i = 1; i <= iorder; i++) { double t = (i << 1) + iorder - 1; double beta = t * beta_mul; root.re = kappa * cos (beta); root.im = kappa * sin (beta); poles[i - 1] = gsl_trans_s2z (root); } /* z nominator polynomial */ for (i = 0; i < iorder; i++) roots[i] = gsl_complex (-1, 0); } /* --- tschebyscheff type 1 filter --- */ static double tschebyscheff_eval (unsigned int degree, double x) { double td = x, td_m_1 = 1; unsigned int d = 1; /* eval polynomial for a certain x */ if (degree == 0) return 1; while (d < degree) { double td1 = 2 * x * td - td_m_1; td_m_1 = td; td = td1; d++; } return td; } static double tschebyscheff_inverse (unsigned int degree, double x) { /* note, that thebyscheff_eval(degree,x)=cosh(degree*acosh(x)) */ return cosh (acosh (x) / degree); } void gsl_filter_tscheb1_rp (unsigned int iorder, double freq, /* 1..pi */ double epsilon, GslComplex *roots, /* [0..iorder-1] */ GslComplex *poles) { double pi = GSL_PI, order = iorder; double alpha; double beta_mul = pi / (2.0 * order); double kappa = gsl_trans_freq2s (freq); GslComplex root; unsigned int i; epsilon = gsl_trans_zepsilon2ss (epsilon); alpha = asinh (1.0 / epsilon) / order; /* construct poles polynomial from tschebyscheff polynomial */ for (i = 1; i <= iorder; i++) { double t = (i << 1) + iorder - 1; double beta = t * beta_mul; root.re = kappa * sinh (alpha) * cos (beta); root.im = kappa * cosh (alpha) * sin (beta); poles[i - 1] = gsl_trans_s2z (root); } /* z nominator polynomial */ for (i = 0; i < iorder; i++) roots[i] = gsl_complex (-1, 0); } /* --- tschebyscheff type 2 filter --- */ void gsl_filter_tscheb2_rp (unsigned int iorder, double c_freq, /* 1..pi */ double steepness, double epsilon, GslComplex *roots, /* [0..iorder-1] */ GslComplex *poles) { double pi = GSL_PI, order = iorder; double r_freq = c_freq * steepness; double kappa_c = gsl_trans_freq2s (c_freq); double kappa_r = gsl_trans_freq2s (r_freq); double tepsilon; double alpha; double beta_mul = pi / (2.0 * order); GslComplex root; unsigned int i; #if 0 /* triggers an internal compiler error with gcc-2.95.4 (and certain * combinations of optimization options) */ g_return_if_fail (c_freq * steepness < GSL_PI); #endif g_return_if_fail (steepness > 1.0); epsilon = gsl_trans_zepsilon2ss (epsilon); tepsilon = epsilon * tschebyscheff_eval (iorder, kappa_r / kappa_c); alpha = asinh (tepsilon) / order; /* construct poles polynomial from tschebyscheff polynomial */ for (i = 1; i <= iorder; i++) { double t = (i << 1) + iorder - 1; double beta = t * beta_mul; root.re = sinh (alpha) * cos (beta); root.im = cosh (alpha) * sin (beta); root = gsl_complex_div (gsl_complex (kappa_r, 0), root); root = gsl_trans_s2z (root); poles[i - 1] = root; } /* construct roots polynomial from tschebyscheff polynomial */ for (i = 1; i <= iorder; i++) { double t = (i << 1) - 1; GslComplex root = gsl_complex (0, cos (t * beta_mul)); if (fabs (root.im) > 1e-14) { root = gsl_complex_div (gsl_complex (kappa_r, 0), root); root = gsl_trans_s2z (root); } else root = gsl_complex (-1, 0); roots[i - 1] = root; } } /** * gsl_filter_tscheb2_steepness_db * @iorder: filter order * @c_freq: passband cutoff frequency (0..pi) * @epsilon: fall off at passband frequency (0..1) * @stopband_db: reduction in stopband in dB (>= 0) * Calculates the steepness parameter for Tschebyscheff type 2 lowpass filter, * based on the ripple residue in the stop band. */ double gsl_filter_tscheb2_steepness_db (unsigned int iorder, double c_freq, double epsilon, double stopband_db) { return gsl_filter_tscheb2_steepness (iorder, c_freq, epsilon, gsl_db_invert (-stopband_db)); } /** * gsl_filter_tscheb2_steepness * @iorder: filter order * @c_freq: passband cutoff frequency (0..pi) * @epsilon: fall off at passband frequency (0..1) * @residue: maximum of transfer function in stopband (0..1) * Calculates the steepness parameter for Tschebyscheff type 2 lowpass filter, * based on ripple residue in the stop band. */ double gsl_filter_tscheb2_steepness (unsigned int iorder, double c_freq, double epsilon, double residue) { double kappa_c, kappa_r, r_freq; epsilon = gsl_trans_zepsilon2ss (epsilon); kappa_c = gsl_trans_freq2s (c_freq); kappa_r = tschebyscheff_inverse (iorder, sqrt (1.0 / (residue * residue) - 1.0) / epsilon) * kappa_c; r_freq = gsl_trans_freq2z (kappa_r); return r_freq / c_freq; } /* --- lowpass filters --- */ /** * gsl_filter_butter_lp * @iorder: filter order * @freq: cutoff frequency (0..pi) * @epsilon: fall off at cutoff frequency (0..1) * @a: root polynomial coefficients a[0..iorder] * @b: pole polynomial coefficients b[0..iorder] * Butterworth lowpass filter. */ void gsl_filter_butter_lp (unsigned int iorder, double freq, /* 0..pi */ double epsilon, double *a, /* [0..iorder] */ double *b) { GslComplex *roots = g_newa (GslComplex, iorder); GslComplex *poles = g_newa (GslComplex, iorder); double norm; g_return_if_fail (freq > 0 && freq < GSL_PI); gsl_filter_butter_rp (iorder, freq, epsilon, roots, poles); filter_rp_to_z (iorder, roots, poles, a, b); /* scale maximum to 1.0 */ norm = gsl_poly_eval (iorder, b, 1) / gsl_poly_eval (iorder, a, 1); gsl_poly_scale (iorder, a, norm); } /** * gsl_filter_tscheb1_lp * @iorder: filter order * @freq: cutoff frequency (0..pi) * @epsilon: fall off at cutoff frequency (0..1) * @a: root polynomial coefficients a[0..iorder] * @b: pole polynomial coefficients b[0..iorder] * Tschebyscheff type 1 lowpass filter. */ void gsl_filter_tscheb1_lp (unsigned int iorder, double freq, /* 0..pi */ double epsilon, double *a, /* [0..iorder] */ double *b) { GslComplex *roots = g_newa (GslComplex, iorder); GslComplex *poles = g_newa (GslComplex, iorder); double norm; g_return_if_fail (freq > 0 && freq < GSL_PI); gsl_filter_tscheb1_rp (iorder, freq, epsilon, roots, poles); filter_rp_to_z (iorder, roots, poles, a, b); /* scale maximum to 1.0 */ norm = gsl_poly_eval (iorder, b, 1) / gsl_poly_eval (iorder, a, 1); if ((iorder & 1) == 0) /* norm is fluctuation minimum */ { epsilon = gsl_trans_zepsilon2ss (epsilon); norm *= sqrt (1.0 / (1.0 + epsilon * epsilon)); } gsl_poly_scale (iorder, a, norm); } /** * gsl_filter_tscheb2_lp * @iorder: filter order * @freq: passband cutoff frequency (0..pi) * @steepness: frequency steepness (c_freq * steepness < pi) * @epsilon: fall off at passband frequency (0..1) * @a: root polynomial coefficients a[0..iorder] * @b: pole polynomial coefficients b[0..iorder] * Tschebyscheff type 2 lowpass filter. * To gain a transition band between freq1 and freq2, pass arguements * @freq=freq1 and @steepness=freq2/freq1. To specify the transition * band width in fractions of octaves, pass @steepness=2^octave_fraction. */ void gsl_filter_tscheb2_lp (unsigned int iorder, double freq, /* 0..pi */ double steepness, double epsilon, double *a, /* [0..iorder] */ double *b) { GslComplex *roots = g_newa (GslComplex, iorder); GslComplex *poles = g_newa (GslComplex, iorder); double norm; g_return_if_fail (freq > 0 && freq < GSL_PI); g_return_if_fail (freq * steepness < GSL_PI); g_return_if_fail (steepness > 1.0); gsl_filter_tscheb2_rp (iorder, freq, steepness, epsilon, roots, poles); filter_rp_to_z (iorder, roots, poles, a, b); /* scale maximum to 1.0 */ norm = gsl_poly_eval (iorder, b, 1) / gsl_poly_eval (iorder, a, 1); /* H(z=0):=1, e^(j*omega) for omega=0 => e^0==1 */ gsl_poly_scale (iorder, a, norm); } /* --- highpass filters --- */ /** * gsl_filter_butter_hp * @iorder: filter order * @freq: passband frequency (0..pi) * @epsilon: fall off at passband frequency (0..1) * @a: root polynomial coefficients a[0..iorder] * @b: pole polynomial coefficients b[0..iorder] * Butterworth highpass filter. */ void gsl_filter_butter_hp (unsigned int iorder, double freq, /* 0..pi */ double epsilon, double *a, /* [0..iorder] */ double *b) { g_return_if_fail (freq > 0 && freq < GSL_PI); freq = GSL_PI - freq; gsl_filter_butter_lp (iorder, freq, epsilon, a, b); filter_lp_invert (iorder, a, b); } /** * gsl_filter_tscheb1_hp * @iorder: filter order * @freq: passband frequency (0..pi) * @epsilon: fall off at passband frequency (0..1) * @a: root polynomial coefficients a[0..iorder] * @b: pole polynomial coefficients b[0..iorder] * Tschebyscheff type 1 highpass filter. */ void gsl_filter_tscheb1_hp (unsigned int iorder, double freq, /* 0..pi */ double epsilon, double *a, /* [0..iorder] */ double *b) { g_return_if_fail (freq > 0 && freq < GSL_PI); freq = GSL_PI - freq; gsl_filter_tscheb1_lp (iorder, freq, epsilon, a, b); filter_lp_invert (iorder, a, b); } /** * gsl_filter_tscheb2_hp * @iorder: filter order * @freq: stopband frequency (0..pi) * @steepness: frequency steepness * @epsilon: fall off at passband frequency (0..1) * @a: root polynomial coefficients a[0..iorder] * @b: pole polynomial coefficients b[0..iorder] * Tschebyscheff type 2 highpass filter. */ void gsl_filter_tscheb2_hp (unsigned int iorder, double freq, double steepness, double epsilon, double *a, /* [0..iorder] */ double *b) { g_return_if_fail (freq > 0 && freq < GSL_PI); freq = GSL_PI - freq; gsl_filter_tscheb2_lp (iorder, freq, steepness, epsilon, a, b); filter_lp_invert (iorder, a, b); } /* --- bandpass filters --- */ /** * gsl_filter_butter_bp * @iorder: filter order (must be even) * @freq1: stopband end frequency (0..pi) * @freq2: passband end frequency (0..pi) * @epsilon: fall off at passband frequency (0..1) * @a: root polynomial coefficients a[0..iorder] * @b: pole polynomial coefficients b[0..iorder] * Butterworth bandpass filter. */ void gsl_filter_butter_bp (unsigned int iorder, double freq1, /* 0..pi */ double freq2, /* 0..pi */ double epsilon, double *a, /* [0..iorder] */ double *b) { unsigned int iorder2 = iorder >> 1; GslComplex *roots = g_newa (GslComplex, iorder2); GslComplex *poles = g_newa (GslComplex, iorder2); double theta; g_return_if_fail ((iorder & 0x01) == 0); g_return_if_fail (freq1 > 0); g_return_if_fail (freq1 < freq2); g_return_if_fail (freq2 < GSL_PI); theta = 2. * atan2 (1., cotan ((freq2 - freq1) * 0.5)); gsl_filter_butter_rp (iorder2, theta, epsilon, roots, poles); band_filter_common (iorder, freq1, freq2, epsilon, roots, poles, a, b, TRUE, FALSE); } /** * gsl_filter_tscheb1_bp * @iorder: filter order (must be even) * @freq1: stopband end frequency (0..pi) * @freq2: passband end frequency (0..pi) * @epsilon: fall off at passband frequency (0..1) * @a: root polynomial coefficients a[0..iorder] * @b: pole polynomial coefficients b[0..iorder] * Tschebyscheff type 1 bandpass filter. */ void gsl_filter_tscheb1_bp (unsigned int iorder, double freq1, /* 0..pi */ double freq2, /* 0..pi */ double epsilon, double *a, /* [0..iorder] */ double *b) { unsigned int iorder2 = iorder >> 1; GslComplex *roots = g_newa (GslComplex, iorder2); GslComplex *poles = g_newa (GslComplex, iorder2); double theta; g_return_if_fail ((iorder & 0x01) == 0); g_return_if_fail (freq1 > 0); g_return_if_fail (freq1 < freq2); g_return_if_fail (freq2 < GSL_PI); theta = 2. * atan2 (1., cotan ((freq2 - freq1) * 0.5)); gsl_filter_tscheb1_rp (iorder2, theta, epsilon, roots, poles); band_filter_common (iorder, freq1, freq2, epsilon, roots, poles, a, b, TRUE, TRUE); } /** * gsl_filter_tscheb2_bp * @iorder: filter order (must be even) * @freq1: stopband end frequency (0..pi) * @freq2: passband end frequency (0..pi) * @steepness: frequency steepness factor * @epsilon: fall off at passband frequency (0..1) * @a: root polynomial coefficients a[0..iorder] * @b: pole polynomial coefficients b[0..iorder] * Tschebyscheff type 2 bandpass filter. */ void gsl_filter_tscheb2_bp (unsigned int iorder, double freq1, /* 0..pi */ double freq2, /* 0..pi */ double steepness, double epsilon, double *a, /* [0..iorder] */ double *b) { unsigned int iorder2 = iorder >> 1; GslComplex *roots = g_newa (GslComplex, iorder2); GslComplex *poles = g_newa (GslComplex, iorder2); double theta; g_return_if_fail ((iorder & 0x01) == 0); g_return_if_fail (freq1 > 0); g_return_if_fail (freq1 < freq2); g_return_if_fail (freq2 < GSL_PI); theta = 2. * atan2 (1., cotan ((freq2 - freq1) * 0.5)); gsl_filter_tscheb2_rp (iorder2, theta, steepness, epsilon, roots, poles); band_filter_common (iorder, freq1, freq2, epsilon, roots, poles, a, b, TRUE, FALSE); } /* --- bandstop filters --- */ /** * gsl_filter_butter_bs * @iorder: filter order (must be even) * @freq1: passband end frequency (0..pi) * @freq2: stopband end frequency (0..pi) * @epsilon: fall off at passband frequency (0..1) * @a: root polynomial coefficients a[0..iorder] * @b: pole polynomial coefficients b[0..iorder] * Butterworth bandstop filter. */ void gsl_filter_butter_bs (unsigned int iorder, double freq1, /* 0..pi */ double freq2, /* 0..pi */ double epsilon, double *a, /* [0..iorder] */ double *b) { unsigned int iorder2 = iorder >> 1; GslComplex *roots = g_newa (GslComplex, iorder2); GslComplex *poles = g_newa (GslComplex, iorder2); double theta; g_return_if_fail ((iorder & 0x01) == 0); g_return_if_fail (freq1 > 0); g_return_if_fail (freq1 < freq2); g_return_if_fail (freq2 < GSL_PI); theta = 2. * atan2 (1., tan ((freq2 - freq1) * 0.5)); gsl_filter_butter_rp (iorder2, theta, epsilon, roots, poles); band_filter_common (iorder, freq1, freq2, epsilon, roots, poles, a, b, FALSE, FALSE); } /** * gsl_filter_tscheb1_bs * @iorder: filter order (must be even) * @freq1: passband end frequency (0..pi) * @freq2: stopband end frequency (0..pi) * @epsilon: fall off at passband frequency (0..1) * @a: root polynomial coefficients a[0..iorder] * @b: pole polynomial coefficients b[0..iorder] * Tschebyscheff type 1 bandstop filter. */ void gsl_filter_tscheb1_bs (unsigned int iorder, double freq1, /* 0..pi */ double freq2, /* 0..pi */ double epsilon, double *a, /* [0..iorder] */ double *b) { unsigned int iorder2 = iorder >> 1; GslComplex *roots = g_newa (GslComplex, iorder2); GslComplex *poles = g_newa (GslComplex, iorder2); double theta; g_return_if_fail ((iorder & 0x01) == 0); g_return_if_fail (freq1 > 0); g_return_if_fail (freq1 < freq2); g_return_if_fail (freq2 < GSL_PI); theta = 2. * atan2 (1., tan ((freq2 - freq1) * 0.5)); gsl_filter_tscheb1_rp (iorder2, theta, epsilon, roots, poles); band_filter_common (iorder, freq1, freq2, epsilon, roots, poles, a, b, FALSE, TRUE); } /** * gsl_filter_tscheb2_bs * @iorder: filter order (must be even) * @freq1: passband end frequency (0..pi) * @freq2: stopband end frequency (0..pi) * @steepness: frequency steepness factor * @epsilon: fall off at passband frequency (0..1) * @a: root polynomial coefficients a[0..iorder] * @b: pole polynomial coefficients b[0..iorder] * Tschebyscheff type 2 bandstop filter. */ void gsl_filter_tscheb2_bs (unsigned int iorder, double freq1, /* 0..pi */ double freq2, /* 0..pi */ double steepness, double epsilon, double *a, /* [0..iorder] */ double *b) { unsigned int iorder2 = iorder >> 1; GslComplex *roots = g_newa (GslComplex, iorder2); GslComplex *poles = g_newa (GslComplex, iorder2); double theta; g_return_if_fail ((iorder & 0x01) == 0); g_return_if_fail (freq1 > 0); g_return_if_fail (freq1 < freq2); g_return_if_fail (freq2 < GSL_PI); theta = 2. * atan2 (1., tan ((freq2 - freq1) * 0.5)); gsl_filter_tscheb2_rp (iorder2, theta, steepness, epsilon, roots, poles); band_filter_common (iorder, freq1, freq2, epsilon, roots, poles, a, b, FALSE, FALSE); } /* --- tschebyscheff type 1 via generic root-finding --- */ #if 0 static void tschebyscheff_poly (unsigned int degree, double *v) { /* construct all polynomial koefficients */ if (degree == 0) v[0] = 1; else if (degree == 1) { v[1] = 1; v[0] = 0; } else { double *u = g_newa (double, 1 + degree); u[degree] = 0; u[degree - 1] = 0; tschebyscheff_poly (degree - 2, u); v[0] = 0; tschebyscheff_poly (degree - 1, v + 1); gsl_poly_scale (degree - 1, v + 1, 2); gsl_poly_sub (degree, v, u); } } static void gsl_filter_tscheb1_test (unsigned int iorder, double zomega, double epsilon, double *a, /* [0..iorder] */ double *b) { GslComplex *roots = g_newa (GslComplex, iorder * 2), *r; GslComplex *zf = g_newa (GslComplex, 1 + iorder); double *vk = g_newa (double, 1 + iorder), norm; double *q = g_newa (double, 2 * (1 + iorder)); double O = gsl_trans_freq2s (zomega); unsigned int i; /* calc Vk() */ tschebyscheff_poly (iorder, vk); /* calc q=1+e^2*Vk()^2 */ gsl_poly_mul (q, iorder >> 1, vk, iorder >> 1, vk); iorder *= 2; gsl_poly_scale (iorder, q, epsilon * epsilon); q[0] += 1; /* find roots, fix roots by 1/(jO) */ gsl_poly_complex_roots (iorder, q, roots); for (i = 0; i < iorder; i++) roots[i] = gsl_complex_mul (roots[i], gsl_complex (0, O)); /* choose roots from the left half-plane */ if (0) g_print ("zhqr-root:\n%s\n", gsl_complex_list (iorder, roots, " ")); r = roots; for (i = 0; i < iorder; i++) if (roots[i].re < 0) { r->re = roots[i].re; r->im = roots[i].im; r++; } iorder /= 2; /* assert roots found */ if (!(r - roots == iorder)) { g_print ("ERROR: n_roots=%u != iorder=%u\n", r - roots, iorder); abort (); } /* s => z */ for (i = 0; i < iorder; i++) roots[i] = gsl_trans_s2z (roots[i]); /* z denominator polynomial */ gsl_cpoly_from_roots (iorder, zf, roots); for (i = 0; i <= iorder; i++) b[i] = zf[i].re; /* z nominator polynomial */ for (i = 0; i < iorder; i++) { roots[i].re = -1; roots[i].im = 0; } gsl_cpoly_from_roots (iorder, zf, roots); for (i = 0; i <= iorder; i++) a[i] = zf[i].re; /* scale for b[0]==1.0 */ gsl_poly_scale (iorder, b, 1.0 / b[0]); /* scale maximum to 1.0 */ norm = gsl_poly_eval (iorder, a, 1) / gsl_poly_eval (iorder, b, 1); if ((iorder & 0x01) == 0) /* norm is fluctuation minimum */ norm /= sqrt (1.0 / (1.0 + epsilon * epsilon)); gsl_poly_scale (iorder, a, 1.0 / norm); } #endif /* --- windowed fir approximation --- */ /* returns a blackman window: x is supposed to be in the interval [0..1] */ static inline double gsl_blackman_window (double x) { if (x < 0) return 0; if (x > 1) return 0; return 0.42 - 0.5 * cos (GSL_PI * x * 2) + 0.08 * cos (4 * GSL_PI * x); } /** * gsl_filter_fir_approx * * @iorder: order of the filter (must be oven, >= 2) * @freq: the frequencies of the transfer function * @value: the desired value of the transfer function * * Approximates a given transfer function with an iorder-coefficient FIR filter. * It is recommended to provide enough frequency values, so that * @n_points >= @iorder. */ void gsl_filter_fir_approx (unsigned int iorder, double *a, /* [0..iorder] */ unsigned int n_points, const double *freq, const double *value) { /* TODO: * * a) does fft_size matter for the quality of the approximation? i.e. do * larger fft_sizes produce better filters? * b) generalize windowing */ unsigned int fft_size = 8; unsigned int point = 0, i; double lfreq = -2, lval = 1.0, rfreq = -1, rval = 1.0; double *fft_in, *fft_out; double ffact; g_return_if_fail (iorder >= 2); g_return_if_fail ((iorder & 1) == 0); while (fft_size / 2 <= iorder) fft_size *= 2; fft_in = g_newa (double, fft_size*2); fft_out = fft_in+fft_size; ffact = 2.0 * GSL_PI / (double)fft_size; for (i = 0; i <= fft_size / 2; i++) { double f = (double) i * ffact; double pos, val; while (f > rfreq && point != n_points) { lfreq = rfreq; rfreq = freq[point]; lval = rval; rval = value[point]; point++; } pos = (f - lfreq) / (rfreq - lfreq); val = lval * (1.0 - pos) + rval * pos; if (i != fft_size / 2) { fft_in[2 * i] = val; fft_in[2 * i + 1] = 0.0; } else fft_in[1] = val; } gsl_power2_fftsr (fft_size, fft_in, fft_out); for (i = 0; i <= iorder / 2; i++) { double c = fft_out[i] * gsl_blackman_window (0.5 + (double) i / (iorder + 2.0)); a[iorder / 2 - i] = c; a[iorder / 2 + i] = c; } } /* --- filter evaluation --- */ void gsl_iir_filter_setup (GslIIRFilter *f, guint order, const gdouble *a, const gdouble *b, gdouble *buffer) /* 4*(order+1) */ { guint i; g_return_if_fail (f != NULL && a != NULL && b != NULL && buffer != NULL); g_return_if_fail (order > 0); f->order = order; f->a = buffer; f->b = f->a + order + 1; f->w = f->b + order + 1; memcpy (f->a, a, sizeof (a[0]) * (order + 1)); for (i = 0; i <= order; i++) f->b[i] = -b[i]; memset (f->w, 0, sizeof (f->w[0]) * (order + 1) * 2); g_return_if_fail (fabs (b[0] - 1.0) < 1e-14); } void gsl_iir_filter_change (GslIIRFilter *f, guint order, const gdouble *a, const gdouble *b, gdouble *buffer) { guint i; g_return_if_fail (f != NULL && a != NULL && b != NULL && buffer != NULL); g_return_if_fail (order > 0); /* there's no point in calling this function if f wasn't setup properly * and it's only the As and Bs that changed */ g_return_if_fail (f->a == buffer && f->b == f->a + f->order + 1 && f->w == f->b + f->order + 1); /* if the order changed there's no chance preserving state */ if (f->order != order) { gsl_iir_filter_setup (f, order, a, b, buffer); return; } memcpy (f->a, a, sizeof (a[0]) * (order + 1)); for (i = 0; i <= order; i++) f->b[i] = -b[i]; /* leaving f->w to preserve state */ g_return_if_fail (fabs (b[0] - 1.0) < 1e-14); } static inline gdouble /* Y */ filter_step_direct_canon_2 (GslIIRFilter *f, gdouble X) { guint n = f->order; gdouble *a = f->a, *b = f->b, *w = f->w; gdouble x, y, v; v = w[n]; x = b[n] * v; y = a[n] * v; while (--n) { gdouble t1, t2; v = w[n]; t1 = v * b[n]; t2 = v * a[n]; w[n+1] = v; x += t1; y += t2; } x += X; w[1] = x; y += x * a[0]; /* w[0] unused */ return y; } static inline gdouble /* Y */ filter_step_direct_canon_1 (GslIIRFilter *f, gdouble X) { guint n = f->order; gdouble *a = f->a, *b = f->b, *w = f->w; gdouble y, v; /* w[n] unused */ y = X * a[0] + w[0]; v = X * a[n] + y * b[n]; while (--n) { gdouble t = w[n]; w[n] = v; t += X * a[n]; v = y * b[n]; v += t; } w[0] = v; return y; } #define filter_step filter_step_direct_canon_1 void gsl_iir_filter_eval (GslIIRFilter *f, guint n_values, const gfloat *x, gfloat *y) { const gfloat *bound; g_return_if_fail (f != NULL && x != NULL && y != NULL); g_return_if_fail (f->order > 0); bound = x + n_values; while (x < bound) { *y = filter_step (f, *x); x++; y++; } } /* --- biquad filters --- */ void gsl_biquad_config_init (GslBiquadConfig *c, GslBiquadType type, GslBiquadNormalize normalize) { g_return_if_fail (c != NULL); memset (c, 0, sizeof (*c)); c->type = type; c->normalize = normalize; gsl_biquad_config_setup (c, 0.5, 3, 1); c->approx_values = TRUE; /* need _setup() */ } void gsl_biquad_config_setup (GslBiquadConfig *c, gfloat f_fn, gfloat gain, gfloat quality) { g_return_if_fail (c != NULL); g_return_if_fail (f_fn >= 0 && f_fn <= 1); if (c->type == GSL_BIQUAD_RESONANT_HIGHPASS) f_fn = 1.0 - f_fn; c->f_fn = f_fn; /* nyquist relative (0=DC, 1=nyquist) */ c->gain = gain; c->quality = quality; /* FIXME */ c->k = tan (c->f_fn * GSL_PI / 2.); c->v = pow (10, c->gain / 20.); /* v=10^(gain[dB]/20) */ c->dirty = TRUE; c->approx_values = FALSE; } void gsl_biquad_config_approx_freq (GslBiquadConfig *c, gfloat f_fn) { g_return_if_fail (f_fn >= 0 && f_fn <= 1); if (c->type == GSL_BIQUAD_RESONANT_HIGHPASS) f_fn = 1.0 - f_fn; c->f_fn = f_fn; /* nyquist relative (0=DC, 1=nyquist) */ c->k = tan (c->f_fn * GSL_PI / 2.); /* FIXME */ c->dirty = TRUE; c->approx_values = TRUE; } void gsl_biquad_config_approx_gain (GslBiquadConfig *c, gfloat gain) { c->gain = gain; c->v = gsl_approx_exp2 (c->gain * GSL_LOG2POW20_10); c->dirty = TRUE; c->approx_values = TRUE; } static void biquad_lpreso (GslBiquadConfig *c, GslBiquadFilter *f) { gdouble kk, sqrt2_reso, denominator; gdouble r2p_norm = 0; /* resonance gain to peak gain (pole: -sqrt2_reso+-j) */ kk = c->k * c->k; sqrt2_reso = 1 / c->v; denominator = 1 + (c->k + sqrt2_reso) * c->k; switch (c->normalize) { case GSL_BIQUAD_NORMALIZE_PASSBAND: r2p_norm = kk; break; case GSL_BIQUAD_NORMALIZE_RESONANCE_GAIN: r2p_norm = kk * sqrt2_reso; break; case GSL_BIQUAD_NORMALIZE_PEAK_GAIN: r2p_norm = (GSL_SQRT2 * sqrt2_reso - 1.0) / (sqrt2_reso * sqrt2_reso - 0.5); r2p_norm = r2p_norm > 1 ? kk * sqrt2_reso : kk * r2p_norm * sqrt2_reso; break; } f->xc0 = r2p_norm / denominator; f->xc1 = 2 * f->xc0; f->xc2 = f->xc0; f->yc1 = 2 * (kk - 1) / denominator; f->yc2 = (1 + (c->k - sqrt2_reso) * c->k) / denominator; } void gsl_biquad_filter_config (GslBiquadFilter *f, GslBiquadConfig *c, gboolean reset_state) { g_return_if_fail (f != NULL); g_return_if_fail (c != NULL); if (c->dirty) { switch (c->type) { case GSL_BIQUAD_RESONANT_LOWPASS: biquad_lpreso (c, f); break; case GSL_BIQUAD_RESONANT_HIGHPASS: biquad_lpreso (c, f); f->xc1 = -f->xc1; f->yc1 = -f->yc1; break; default: g_assert_not_reached (); } c->dirty = FALSE; } if (reset_state) f->xd1 = f->xd2 = f->yd1 = f->yd2 = 0; } void gsl_biquad_filter_eval (GslBiquadFilter *f, guint n_values, const gfloat *x, gfloat *y) { const gfloat *bound; gdouble xc0, xc1, xc2, yc1, yc2, xd1, xd2, yd1, yd2; g_return_if_fail (f != NULL && x != NULL && y != NULL); xc0 = f->xc0; xc1 = f->xc1; xc2 = f->xc2; yc1 = f->yc1; yc2 = f->yc2; xd1 = f->xd1; xd2 = f->xd2; yd1 = f->yd1; yd2 = f->yd2; bound = x + n_values; while (x < bound) { gdouble k0, k1, k2; k2 = xd2 * xc2; k1 = xd1 * xc1; xd2 = xd1; xd1 = *x++; k2 -= yd2 * yc2; k1 -= yd1 * yc1; yd2 = yd1; k0 = xd1 * xc0; yd1 = k2 + k1; *y++ = yd1 += k0; } f->xd1 = xd1; f->xd2 = xd2; f->yd1 = yd1; f->yd2 = yd2; } #if 0 void gsl_biquad_lphp_reso (GslBiquadFilter *c, gfloat f_fn, /* nyquist relative (0=DC, 1=nyquist) */ float gain, gboolean design_highpass, GslBiquadNormalize normalize) { double k, kk, v; double sqrt2_reso; double denominator; double r2p_norm = 0; /* resonance gain to peak gain (pole: -sqrt2_reso+-j) */ g_return_if_fail (c != NULL); g_return_if_fail (f_fn >= 0 && f_fn <= 1); if (design_highpass) f_fn = 1.0 - f_fn; v = pow (10, gain / 20.); /* v=10^(gain[dB]/20) */ k = tan (f_fn * GSL_PI / 2.); kk = k * k; sqrt2_reso = 1 / v; denominator = 1 + (k + sqrt2_reso) * k; if (0) g_printerr ("BIQUAD-lp: R=%f\n", GSL_SQRT2 * sqrt2_reso); switch (normalize) { case GSL_BIQUAD_NORMALIZE_PASSBAND: r2p_norm = kk; break; case GSL_BIQUAD_NORMALIZE_RESONANCE_GAIN: r2p_norm = kk * sqrt2_reso; break; case GSL_BIQUAD_NORMALIZE_PEAK_GAIN: r2p_norm = (GSL_SQRT2 * sqrt2_reso - 1.0) / (sqrt2_reso * sqrt2_reso - 0.5); g_print ("BIQUAD-lp: (peak-gain) r2p_norm = %f \n", r2p_norm); r2p_norm = r2p_norm > 1 ? kk * sqrt2_reso : kk * r2p_norm * sqrt2_reso; break; } c->xc0 = r2p_norm / denominator; c->xc1 = 2 * c->xc0; c->xc2 = c->xc0; c->yc1 = 2 * (kk - 1) / denominator; c->yc2 = (1 + (k - sqrt2_reso) * k) / denominator; if (design_highpass) { c->xc1 = -c->xc1; c->yc1 = -c->yc1; } /* normalization notes: * pole: -sqrt2_reso+-j * freq=0.5: reso->peak gain=8adjust:0.9799887, 9adjust:0.98415 * resonance gain = 1/(1-R)=sqrt2_reso * sqrt2_reso*(1-R)=1 * 1-R=1/sqrt2_reso * R= 1-1/sqrt2_reso * peak gain = 2/(1-R^2) * = 2 * (1 - (1 - 1 / sqrt2_reso) * (1 - 1 / sqrt2_reso)) * = 2 - 2 * (1 - 1 / sqrt2_reso)^2 */ } #endif /* --- filter scanning -- */ #define SINE_SCAN_SIZE 1024 /** * gsl_filter_sine_scan * * @order: order of the iir filter * @a: root polynomial coefficients of the filter a[0..order] * @b: pole polynomial coefficients of the filter b[0..order] * @freq: frequency to test * @n_values: number of samples * * This function sends a sine signal of the desired frequency through an IIR * filter, to test the value of the transfer function at a given point. It uses * gsl_iir_filter_eval to do so. * * Compared to a "mathematical approach" of finding the transfer function, * this function makes it possible to see the effects of finite arithmetic * during filter evaluation. * * The first half of the output signal is not considered, since a lot of IIR * filters have a transient phase where also overshoot is possible. * * For n_values, you should specify a reasonable large value. It should be * a lot larger than the filter order, and large enough to let the input * signal become (close to) 1.0 multiple times. */ gdouble gsl_filter_sine_scan (guint order, const gdouble *a, const gdouble *b, gdouble freq, guint n_values) { gfloat x[SINE_SCAN_SIZE], y[SINE_SCAN_SIZE]; gdouble pos = 0.0; gdouble result = 0.0; GslIIRFilter filter; gdouble *filter_state; guint scan_start = n_values / 2; g_return_val_if_fail (order > 0, 0.0); g_return_val_if_fail (a != NULL, 0.0); g_return_val_if_fail (b != NULL, 0.0); g_return_val_if_fail (freq > 0 && freq < GSL_PI, 0.0); g_return_val_if_fail (n_values > 0, 0.0); filter_state = g_newa (double, (order + 1) * 4); gsl_iir_filter_setup (&filter, order, a, b, filter_state); while (n_values) { guint todo = MIN (n_values, SINE_SCAN_SIZE); guint i; for (i = 0; i < todo; i++) { x[i] = sin (pos); pos += freq; } gsl_iir_filter_eval (&filter, SINE_SCAN_SIZE, x, y); for (i = 0; i < todo; i++) if (n_values - i < scan_start) result = MAX (y[i], result); n_values -= todo; } return result; } /* vim:set ts=8 sts=2 sw=2: */