/******************************************************************** * * * THIS FILE IS PART OF THE Ogg Vorbis SOFTWARE CODEC SOURCE CODE. * * USE, DISTRIBUTION AND REPRODUCTION OF THIS SOURCE IS GOVERNED BY * * THE GNU PUBLIC LICENSE 2, WHICH IS INCLUDED WITH THIS SOURCE. * * PLEASE READ THESE TERMS DISTRIBUTING. * * * * THE OggSQUISH SOURCE CODE IS (C) COPYRIGHT 1994-2000 * * by Monty and The XIPHOPHORUS Company * * http://www.xiph.org/ * * * ******************************************************************** function: Direct Form I, II IIR filters, plus some specializations last mod: $Id$ ********************************************************************/ /* LPC is actually a degenerate case of form I/II filters, but we need both */ #ifdef HAVE_CONFIG_H #include "config.h" #endif #include #include #include #include "iir.h" void IIR_init (IIR_state * s, int stages, double gain, double *A, double *B) { memset (s, 0, sizeof (IIR_state)); s->stages = stages; s->gain = gain; s->coeff_A = malloc (stages * sizeof (double)); s->coeff_B = malloc ((stages + 1) * sizeof (double)); s->z_A = calloc (stages * 2, sizeof (double)); s->z_B = calloc (stages * 2, sizeof (double)); memcpy (s->coeff_A, A, stages * sizeof (double)); memcpy (s->coeff_B, B, (stages + 1) * sizeof (double)); } void IIR_clear (IIR_state * s) { if (s) { free (s->coeff_A); free (s->coeff_B); free (s->z_A); free (s->z_B); memset (s, 0, sizeof (IIR_state)); } } double IIR_filter (IIR_state * s, double in) { int stages = s->stages, i; double newA; double newB = 0; double *zA = s->z_A + s->ring; newA = in /= s->gain; for (i = 0; i < stages; i++) { newA += s->coeff_A[i] * zA[i]; newB += s->coeff_B[i] * zA[i]; } newB += newA * s->coeff_B[stages]; zA[0] = zA[stages] = newA; if (++s->ring >= stages) s->ring = 0; return (newB); } /* this assumes the symmetrical structure of the feed-forward stage of a Chebyshev bandpass to save multiplies */ double IIR_filter_ChebBand (IIR_state * s, double in) { int stages = s->stages, i; double newA; double newB = 0; double *zA = s->z_A + s->ring; newA = in /= s->gain; newA += s->coeff_A[0] * zA[0]; for (i = 1; i < (stages >> 1); i++) { newA += s->coeff_A[i] * zA[i]; newB += s->coeff_B[i] * (zA[i] - zA[stages - i]); } newB += s->coeff_B[i] * zA[i]; for (; i < stages; i++) newA += s->coeff_A[i] * zA[i]; newB += newA - zA[0]; zA[0] = zA[stages] = newA; if (++s->ring >= stages) s->ring = 0; return (newB); } #ifdef _V_SELFTEST /* z^-stage, z^-stage+1... */ static double cheb_bandpass_B[] = { -1., 0., 5., 0., -10., 0., 10., 0., -5., 0., 1 }; static double cheb_bandpass_A[] = { -0.6665900311, 1.0070146601, -3.1262875409, 3.5017171569, -6.2779211945, 5.2966481740, -6.7570216587, 4.0760335768, -3.9134284363, 1.3997338886 }; static double data[128] = { 0.0426331, 0.0384521, 0.0345764, 0.0346069, 0.0314636, 0.0310059, 0.0318604, 0.0336304, 0.036438, 0.0348511, 0.0354919, 0.0343628, 0.0325623, 0.0318909, 0.0263367, 0.0225525, 0.0195618, 0.0160828, 0.0168762, 0.0145569, 0.0126343, 0.0127258, 0.00820923, 0.00787354, 0.00558472, 0.00204468, 3.05176e-05, -0.00357056, -0.00570679, -0.00991821, -0.0101013, -0.00881958, -0.0108948, -0.0110168, -0.0119324, -0.0161438, -0.0194702, -0.0229187, -0.0260315, -0.0282288, -0.0306091, -0.0330505, -0.0364685, -0.0385742, -0.0428772, -0.043457, -0.0425415, -0.0462341, -0.0467529, -0.0489807, -0.0520325, -0.0558167, -0.0596924, -0.0591431, -0.0612793, -0.0618591, -0.0615845, -0.0634155, -0.0639648, -0.0683594, -0.0718079, -0.0729675, -0.0791931, -0.0860901, -0.0885315, -0.088623, -0.089386, -0.0899353, -0.0886841, -0.0910645, -0.0948181, -0.0919495, -0.0891418, -0.0916443, -0.096344, -0.100464, -0.105499, -0.108612, -0.112213, -0.117676, -0.120911, -0.124329, -0.122162, -0.120605, -0.12326, -0.12619, -0.128998, -0.13205, -0.134247, -0.137939, -0.143555, -0.14389, -0.14859, -0.153717, -0.159851, -0.164551, -0.162811, -0.164276, -0.156952, -0.140564, -0.123291, -0.10321, -0.0827637, -0.0652466, -0.053772, -0.0509949, -0.0577698, -0.0818176, -0.114929, -0.148895, -0.181122, -0.200714, -0.21048, -0.203644, -0.179413, -0.145325, -0.104492, -0.0658264, -0.0332031, -0.0106201, -0.00363159, -0.00909424, -0.0244141, -0.0422058, -0.0537415, -0.0610046, -0.0609741, -0.0547791 }; /* comparison test code from http://www-users.cs.york.ac.uk/~fisher/mkfilter/ (the above page kicks ass, BTW)*/ #define NZEROS 10 #define NPOLES 10 #define GAIN 4.599477515e+02 static float xv[NZEROS + 1], yv[NPOLES + 1]; static double filterloop (double next) { xv[0] = xv[1]; xv[1] = xv[2]; xv[2] = xv[3]; xv[3] = xv[4]; xv[4] = xv[5]; xv[5] = xv[6]; xv[6] = xv[7]; xv[7] = xv[8]; xv[8] = xv[9]; xv[9] = xv[10]; xv[10] = next / GAIN; yv[0] = yv[1]; yv[1] = yv[2]; yv[2] = yv[3]; yv[3] = yv[4]; yv[4] = yv[5]; yv[5] = yv[6]; yv[6] = yv[7]; yv[7] = yv[8]; yv[8] = yv[9]; yv[9] = yv[10]; yv[10] = (xv[10] - xv[0]) + 5 * (xv[2] - xv[8]) + 10 * (xv[6] - xv[4]) + (-0.6665900311 * yv[0]) + (1.0070146601 * yv[1]) + (-3.1262875409 * yv[2]) + (3.5017171569 * yv[3]) + (-6.2779211945 * yv[4]) + (5.2966481740 * yv[5]) + (-6.7570216587 * yv[6]) + (4.0760335768 * yv[7]) + (-3.9134284363 * yv[8]) + (1.3997338886 * yv[9]); return (yv[10]); } #include int main () { /* run the pregenerated Chebyshev filter, then our own distillation through the generic and specialized code */ double *work = malloc (128 * sizeof (double)); IIR_state iir; int i; for (i = 0; i < 128; i++) work[i] = filterloop (data[i]); { FILE *out = fopen ("IIR_ref.m", "w"); for (i = 0; i < 128; i++) fprintf (out, "%g\n", work[i]); fclose (out); } IIR_init (&iir, NPOLES, GAIN, cheb_bandpass_A, cheb_bandpass_B); for (i = 0; i < 128; i++) work[i] = IIR_filter (&iir, data[i]); { FILE *out = fopen ("IIR_gen.m", "w"); for (i = 0; i < 128; i++) fprintf (out, "%g\n", work[i]); fclose (out); } IIR_clear (&iir); IIR_init (&iir, NPOLES, GAIN, cheb_bandpass_A, cheb_bandpass_B); for (i = 0; i < 128; i++) work[i] = IIR_filter_ChebBand (&iir, data[i]); { FILE *out = fopen ("IIR_cheb.m", "w"); for (i = 0; i < 128; i++) fprintf (out, "%g\n", work[i]); fclose (out); } IIR_clear (&iir); return (0); } #endif