| /******************************************************************** |
| * * |
| * 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 <monty@xiph.org> 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 <stdlib.h> |
| #include <string.h> |
| #include <math.h> |
| #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 <stdio.h> |
| 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 |
