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.
1374 lines
36 KiB
1374 lines
36 KiB
/* 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;
|
|
}
|