TDE
/
arts
mirror of https://mirror.git.trinitydesktop.org/gitea/TDE/arts
0
0
Fork 0
Code Issues Releases Wiki Activity
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from '9d57e3ae1b'
${ noResults }
arts/flow/gsl/gslfilter.c

1380 lines
36 KiB
Raw Blame History

/* 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 <string.h>
/* --- 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: */
View Git Blame Copy Permalink