Mercurial > mplayer.hg
view libao2/remez.c @ 12925:2abd8e24eb69
10l to the author of this longstanding and obscure bug. Each language
should be removed only once from the list.
Thanks to Chris White for pointing out that there was a problem.
author | diego |
---|---|
date | Sat, 31 Jul 2004 22:17:23 +0000 |
parents | b608086bf84e |
children |
line wrap: on
line source
/************************************************************************** * Parks-McClellan algorithm for FIR filter design (C version) *------------------------------------------------- * Copyright (c) 1995,1998 Jake Janovetz (janovetz@uiuc.edu) * * This library is free software; you can redistribute it and/or * modify it under the terms of the GNU Library 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 * Library General Public License for more details. * * You should have received a copy of the GNU Library General Public * License along with this library; if not, write to the Free * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA * *************************************************************************/ #include "config.h" #include "remez.h" #include <stdio.h> #ifdef HAVE_MALLOC_H #include <malloc.h> #endif #include <stdlib.h> #include <math.h> /******************* * CreateDenseGrid *================= * Creates the dense grid of frequencies from the specified bands. * Also creates the Desired Frequency Response function (D[]) and * the Weight function (W[]) on that dense grid * * * INPUT: * ------ * int r - 1/2 the number of filter coefficients * int numtaps - Number of taps in the resulting filter * int numband - Number of bands in user specification * double bands[] - User-specified band edges [2*numband] * double des[] - Desired response per band [numband] * double weight[] - Weight per band [numband] * int symmetry - Symmetry of filter - used for grid check * * OUTPUT: * ------- * int gridsize - Number of elements in the dense frequency grid * double Grid[] - Frequencies (0 to 0.5) on the dense grid [gridsize] * double D[] - Desired response on the dense grid [gridsize] * double W[] - Weight function on the dense grid [gridsize] *******************/ void CreateDenseGrid(int r, int numtaps, int numband, double bands[], double des[], double weight[], int *gridsize, double Grid[], double D[], double W[], int symmetry) { int i, j, k, band; double delf, lowf, highf; delf = 0.5/(GRIDDENSITY*r); /* * For differentiator, hilbert, * symmetry is odd and Grid[0] = max(delf, band[0]) */ if ((symmetry == NEGATIVE) && (delf > bands[0])) bands[0] = delf; j=0; for (band=0; band < numband; band++) { Grid[j] = bands[2*band]; lowf = bands[2*band]; highf = bands[2*band + 1]; k = (int)((highf - lowf)/delf + 0.5); /* .5 for rounding */ for (i=0; i<k; i++) { D[j] = des[band]; W[j] = weight[band]; Grid[j] = lowf; lowf += delf; j++; } Grid[j-1] = highf; } /* * Similar to above, if odd symmetry, last grid point can't be .5 * - but, if there are even taps, leave the last grid point at .5 */ if ((symmetry == NEGATIVE) && (Grid[*gridsize-1] > (0.5 - delf)) && (numtaps % 2)) { Grid[*gridsize-1] = 0.5-delf; } } /******************** * InitialGuess *============== * Places Extremal Frequencies evenly throughout the dense grid. * * * INPUT: * ------ * int r - 1/2 the number of filter coefficients * int gridsize - Number of elements in the dense frequency grid * * OUTPUT: * ------- * int Ext[] - Extremal indexes to dense frequency grid [r+1] ********************/ void InitialGuess(int r, int Ext[], int gridsize) { int i; for (i=0; i<=r; i++) Ext[i] = i * (gridsize-1) / r; } /*********************** * CalcParms *=========== * * * INPUT: * ------ * int r - 1/2 the number of filter coefficients * int Ext[] - Extremal indexes to dense frequency grid [r+1] * double Grid[] - Frequencies (0 to 0.5) on the dense grid [gridsize] * double D[] - Desired response on the dense grid [gridsize] * double W[] - Weight function on the dense grid [gridsize] * * OUTPUT: * ------- * double ad[] - 'b' in Oppenheim & Schafer [r+1] * double x[] - [r+1] * double y[] - 'C' in Oppenheim & Schafer [r+1] ***********************/ void CalcParms(int r, int Ext[], double Grid[], double D[], double W[], double ad[], double x[], double y[]) { int i, j, k, ld; double sign, xi, delta, denom, numer; /* * Find x[] */ for (i=0; i<=r; i++) x[i] = cos(Pi2 * Grid[Ext[i]]); /* * Calculate ad[] - Oppenheim & Schafer eq 7.132 */ ld = (r-1)/15 + 1; /* Skips around to avoid round errors */ for (i=0; i<=r; i++) { denom = 1.0; xi = x[i]; for (j=0; j<ld; j++) { for (k=j; k<=r; k+=ld) if (k != i) denom *= 2.0*(xi - x[k]); } if (fabs(denom)<0.00001) denom = 0.00001; ad[i] = 1.0/denom; } /* * Calculate delta - Oppenheim & Schafer eq 7.131 */ numer = denom = 0; sign = 1; for (i=0; i<=r; i++) { numer += ad[i] * D[Ext[i]]; denom += sign * ad[i]/W[Ext[i]]; sign = -sign; } delta = numer/denom; sign = 1; /* * Calculate y[] - Oppenheim & Schafer eq 7.133b */ for (i=0; i<=r; i++) { y[i] = D[Ext[i]] - sign * delta/W[Ext[i]]; sign = -sign; } } /********************* * ComputeA *========== * Using values calculated in CalcParms, ComputeA calculates the * actual filter response at a given frequency (freq). Uses * eq 7.133a from Oppenheim & Schafer. * * * INPUT: * ------ * double freq - Frequency (0 to 0.5) at which to calculate A * int r - 1/2 the number of filter coefficients * double ad[] - 'b' in Oppenheim & Schafer [r+1] * double x[] - [r+1] * double y[] - 'C' in Oppenheim & Schafer [r+1] * * OUTPUT: * ------- * Returns double value of A[freq] *********************/ double ComputeA(double freq, int r, double ad[], double x[], double y[]) { int i; double xc, c, denom, numer; denom = numer = 0; xc = cos(Pi2 * freq); for (i=0; i<=r; i++) { c = xc - x[i]; if (fabs(c) < 1.0e-7) { numer = y[i]; denom = 1; break; } c = ad[i]/c; denom += c; numer += c*y[i]; } return numer/denom; } /************************ * CalcError *=========== * Calculates the Error function from the desired frequency response * on the dense grid (D[]), the weight function on the dense grid (W[]), * and the present response calculation (A[]) * * * INPUT: * ------ * int r - 1/2 the number of filter coefficients * double ad[] - [r+1] * double x[] - [r+1] * double y[] - [r+1] * int gridsize - Number of elements in the dense frequency grid * double Grid[] - Frequencies on the dense grid [gridsize] * double D[] - Desired response on the dense grid [gridsize] * double W[] - Weight function on the desnse grid [gridsize] * * OUTPUT: * ------- * double E[] - Error function on dense grid [gridsize] ************************/ void CalcError(int r, double ad[], double x[], double y[], int gridsize, double Grid[], double D[], double W[], double E[]) { int i; double A; for (i=0; i<gridsize; i++) { A = ComputeA(Grid[i], r, ad, x, y); E[i] = W[i] * (D[i] - A); } } /************************ * Search *======== * Searches for the maxima/minima of the error curve. If more than * r+1 extrema are found, it uses the following heuristic (thanks * Chris Hanson): * 1) Adjacent non-alternating extrema deleted first. * 2) If there are more than one excess extrema, delete the * one with the smallest error. This will create a non-alternation * condition that is fixed by 1). * 3) If there is exactly one excess extremum, delete the smaller * of the first/last extremum * * * INPUT: * ------ * int r - 1/2 the number of filter coefficients * int Ext[] - Indexes to Grid[] of extremal frequencies [r+1] * int gridsize - Number of elements in the dense frequency grid * double E[] - Array of error values. [gridsize] * OUTPUT: * ------- * int Ext[] - New indexes to extremal frequencies [r+1] ************************/ void Search(int r, int Ext[], int gridsize, double E[]) { int i, j, k, l, extra; /* Counters */ int up, alt; int *foundExt; /* Array of found extremals */ /* * Allocate enough space for found extremals. */ foundExt = (int *)malloc((2*r) * sizeof(int)); k = 0; /* * Check for extremum at 0. */ if (((E[0]>0.0) && (E[0]>E[1])) || ((E[0]<0.0) && (E[0]<E[1]))) foundExt[k++] = 0; /* * Check for extrema inside dense grid */ for (i=1; i<gridsize-1; i++) { if (((E[i]>=E[i-1]) && (E[i]>E[i+1]) && (E[i]>0.0)) || ((E[i]<=E[i-1]) && (E[i]<E[i+1]) && (E[i]<0.0))) foundExt[k++] = i; } /* * Check for extremum at 0.5 */ j = gridsize-1; if (((E[j]>0.0) && (E[j]>E[j-1])) || ((E[j]<0.0) && (E[j]<E[j-1]))) foundExt[k++] = j; /* * Remove extra extremals */ extra = k - (r+1); while (extra > 0) { if (E[foundExt[0]] > 0.0) up = 1; /* first one is a maxima */ else up = 0; /* first one is a minima */ l=0; alt = 1; for (j=1; j<k; j++) { if (fabs(E[foundExt[j]]) < fabs(E[foundExt[l]])) l = j; /* new smallest error. */ if ((up) && (E[foundExt[j]] < 0.0)) up = 0; /* switch to a minima */ else if ((!up) && (E[foundExt[j]] > 0.0)) up = 1; /* switch to a maxima */ else { alt = 0; break; /* Ooops, found two non-alternating */ } /* extrema. Delete smallest of them */ } /* if the loop finishes, all extrema are alternating */ /* * If there's only one extremal and all are alternating, * delete the smallest of the first/last extremals. */ if ((alt) && (extra == 1)) { if (fabs(E[foundExt[k-1]]) < fabs(E[foundExt[0]])) l = foundExt[k-1]; /* Delete last extremal */ else l = foundExt[0]; /* Delete first extremal */ } for (j=l; j<k; j++) /* Loop that does the deletion */ { foundExt[j] = foundExt[j+1]; } k--; extra--; } for (i=0; i<=r; i++) { Ext[i] = foundExt[i]; /* Copy found extremals to Ext[] */ } free(foundExt); } /********************* * FreqSample *============ * Simple frequency sampling algorithm to determine the impulse * response h[] from A's found in ComputeA * * * INPUT: * ------ * int N - Number of filter coefficients * double A[] - Sample points of desired response [N/2] * int symmetry - Symmetry of desired filter * * OUTPUT: * ------- * double h[] - Impulse Response of final filter [N] *********************/ void FreqSample(int N, double A[], double h[], int symm) { int n, k; double x, val, M; M = (N-1.0)/2.0; if (symm == POSITIVE) { if (N%2) { for (n=0; n<N; n++) { val = A[0]; x = Pi2 * (n - M)/N; for (k=1; k<=M; k++) val += 2.0 * A[k] * cos(x*k); h[n] = val/N; } } else { for (n=0; n<N; n++) { val = A[0]; x = Pi2 * (n - M)/N; for (k=1; k<=(N/2-1); k++) val += 2.0 * A[k] * cos(x*k); h[n] = val/N; } } } else { if (N%2) { for (n=0; n<N; n++) { val = 0; x = Pi2 * (n - M)/N; for (k=1; k<=M; k++) val += 2.0 * A[k] * sin(x*k); h[n] = val/N; } } else { for (n=0; n<N; n++) { val = A[N/2] * sin(Pi * (n - M)); x = Pi2 * (n - M)/N; for (k=1; k<=(N/2-1); k++) val += 2.0 * A[k] * sin(x*k); h[n] = val/N; } } } } /******************* * isDone *======== * Checks to see if the error function is small enough to consider * the result to have converged. * * INPUT: * ------ * int r - 1/2 the number of filter coeffiecients * int Ext[] - Indexes to extremal frequencies [r+1] * double E[] - Error function on the dense grid [gridsize] * * OUTPUT: * ------- * Returns 1 if the result converged * Returns 0 if the result has not converged ********************/ short isDone(int r, int Ext[], double E[]) { int i; double min, max, current; min = max = fabs(E[Ext[0]]); for (i=1; i<=r; i++) { current = fabs(E[Ext[i]]); if (current < min) min = current; if (current > max) max = current; } if (((max-min)/max) < 0.0001) return 1; return 0; } /******************** * remez *======= * Calculates the optimal (in the Chebyshev/minimax sense) * FIR filter impulse response given a set of band edges, * the desired reponse on those bands, and the weight given to * the error in those bands. * * INPUT: * ------ * int numtaps - Number of filter coefficients * int numband - Number of bands in filter specification * double bands[] - User-specified band edges [2 * numband] * double des[] - User-specified band responses [numband] * double weight[] - User-specified error weights [numband] * int type - Type of filter * * OUTPUT: * ------- * double h[] - Impulse response of final filter [numtaps] ********************/ void remez(double h[], int numtaps, int numband, double bands[], double des[], double weight[], int type) { double *Grid, *W, *D, *E; int i, iter, gridsize, r, *Ext; double *taps, c; double *x, *y, *ad; int symmetry; if (type == BANDPASS) symmetry = POSITIVE; else symmetry = NEGATIVE; r = numtaps/2; /* number of extrema */ if ((numtaps%2) && (symmetry == POSITIVE)) r++; /* * Predict dense grid size in advance for memory allocation * .5 is so we round up, not truncate */ gridsize = 0; for (i=0; i<numband; i++) { gridsize += (int)(2*r*GRIDDENSITY*(bands[2*i+1] - bands[2*i]) + .5); } if (symmetry == NEGATIVE) { gridsize--; } /* * Dynamically allocate memory for arrays with proper sizes */ Grid = (double *)malloc(gridsize * sizeof(double)); D = (double *)malloc(gridsize * sizeof(double)); W = (double *)malloc(gridsize * sizeof(double)); E = (double *)malloc(gridsize * sizeof(double)); Ext = (int *)malloc((r+1) * sizeof(int)); taps = (double *)malloc((r+1) * sizeof(double)); x = (double *)malloc((r+1) * sizeof(double)); y = (double *)malloc((r+1) * sizeof(double)); ad = (double *)malloc((r+1) * sizeof(double)); /* * Create dense frequency grid */ CreateDenseGrid(r, numtaps, numband, bands, des, weight, &gridsize, Grid, D, W, symmetry); InitialGuess(r, Ext, gridsize); /* * For Differentiator: (fix grid) */ if (type == DIFFERENTIATOR) { for (i=0; i<gridsize; i++) { /* D[i] = D[i]*Grid[i]; */ if (D[i] > 0.0001) W[i] = W[i]/Grid[i]; } } /* * For odd or Negative symmetry filters, alter the * D[] and W[] according to Parks McClellan */ if (symmetry == POSITIVE) { if (numtaps % 2 == 0) { for (i=0; i<gridsize; i++) { c = cos(Pi * Grid[i]); D[i] /= c; W[i] *= c; } } } else { if (numtaps % 2) { for (i=0; i<gridsize; i++) { c = sin(Pi2 * Grid[i]); D[i] /= c; W[i] *= c; } } else { for (i=0; i<gridsize; i++) { c = sin(Pi * Grid[i]); D[i] /= c; W[i] *= c; } } } /* * Perform the Remez Exchange algorithm */ for (iter=0; iter<MAXITERATIONS; iter++) { CalcParms(r, Ext, Grid, D, W, ad, x, y); CalcError(r, ad, x, y, gridsize, Grid, D, W, E); Search(r, Ext, gridsize, E); if (isDone(r, Ext, E)) break; } if (iter == MAXITERATIONS) { printf("Reached maximum iteration count.\nResults may be bad.\n"); } CalcParms(r, Ext, Grid, D, W, ad, x, y); /* * Find the 'taps' of the filter for use with Frequency * Sampling. If odd or Negative symmetry, fix the taps * according to Parks McClellan */ for (i=0; i<=numtaps/2; i++) { if (symmetry == POSITIVE) { if (numtaps%2) c = 1; else c = cos(Pi * (double)i/numtaps); } else { if (numtaps%2) c = sin(Pi2 * (double)i/numtaps); else c = sin(Pi * (double)i/numtaps); } taps[i] = ComputeA((double)i/numtaps, r, ad, x, y)*c; } /* * Frequency sampling design with calculated taps */ FreqSample(numtaps, taps, h, symmetry); /* * Delete allocated memory */ free(Grid); free(W); free(D); free(E); free(Ext); free(x); free(y); free(ad); }