remez.c

remez算法设计FIR滤波器源码

/*  TAB P   VER 068  $Id: remez.c,v 1.3 1998/06/15 09:04:37 egil Exp $
 *
 *  Remez exchange algorithm
 *
 *  the core of this program is converted from
 *  an original Fortran source. see below for details.
 *
 *  adaption for the PC and addition of FFT by:
 *      egil kvaleberg
 *      husebybakken 14a
 *      0379 oslo, norway
 *  Email:
 *      [e-mail]
 *  Web:
 *      http://www.kvaleberg.com/
 *
 *  Bugs:
 *  converted from Fortran, and no attempt has been made of hiding the fact
 *  the user interface is pretty dismal, the error handling horrible, but
 *  at least you should get your moneys worth   
 */

#include <stdio.h>
#include <string.h>
#include <math.h>

/*
 *-----------------------------------------------------------------------
 * MAIN PROGRAM: FIR LINEAR PHASE FILTER DESIGN PROGRAM
 *
 * AUTHORS: JAMES H. MCCLELLAN
 *
 *         DEPARTMENT OF ELECTRICAL ENGINEERING AND COMPUTER SCIENCE
 *         MASSACHUSETTS INSTITUTE OF TECHNOLOGY
 *         CAMBRIDGE, MASS. 02139
 *
 *         THOMAS W. PARKS
 *         DEPARTMENT OF ELECTRICAL ENGINEERING
 *         RICE UNIVERSITY
 *         HOUSTON, TEXAS 77001
 *
 *         LAWRENCE R. RABINER
 *         BELL LABORATORIES
 *         MURRAY HILL, NEW JERSEY 07974
 *
 * INPUT:
 * hz--    Sampling frequency (1.0 gives unity frequency operation)
 * nfilt-- FILTER LENGTH
 * jtype-- TYPE OF FILTER
 *         1 = MULTIPLE PASSBAND/STOPBAND FILTER
 *         2 = DIFFERENTIATOR
 *         3 = HILBERT TRANSFORM FILTER
 * nbands-- NUMBER OF BANDS
 * lgrid-- GRID DENSITY, WILL BE SET TO 16 UNLESS
 *         SPECIFIED OTHERWISE BY A POSITIVE CONSTANT.
 *
 * edge(2*nbands)-- BANDEDGE ARRAY, LOWER AND UPPER EDGES FOR EACH BAND
 *                  WITH A MAXIMUM OF 10 BANDS.
 *
 * fx(nbands)-- DESIRED FUNCTION ARRAY (OR DESIRED SLOPE IF A
 *              DIFFERENTIATOR) FOR EACH BAND.
 *
 * wtx(nbands)-- WEIGHT FUNCTION ARRAY IN EACH BAND.  FOR A
 *               DIFFERENTIATOR, THE WEIGHT FUNCTION IS INVERSELY
 *               PROPORTIONAL TO F.
 *
 * SAMPLE INPUT DATA SETUP:
 *      1
 *      32
 *      1
 *      3
 *      0
 *      0.0 0.1   0.2 0.35   0.425 0.5
 *      0.0       1.0        0.0
 *      10.0      1.0        10.0
 *
 *    THIS DATA SPECIFIES A LENGTH 32 BANDPASS FILTER WITH
 *    STOPBANDS 0 TO 0.1 AND 0.425 TO 0.5, AND PASSBAND FROM
 *    0.2 TO 0.35 WITH WEIGHTING OF 10 IN THE STOPBANDS AND 1
 *    IN THE PASSBAND.  THE GRID DENSITY DEFAULTS TO 16.
 *    THIS IS THE FILTER IN FIGURE 10.
 *
 *    THE FOLLOWING INPUT DATA SPECIFIES A LENGTH 32 FULLBAND
 *    DIFFERENTIATOR WITH SLOPE 1 AND WEIGHTING OF 1/F.
 *    THE GRID DENSITY WILL BE SET TO 20.
 *      1
 *      32 
 *      2 
 *      1 
 *      20
 *      0 0.5
 *      1.0
 *      1.0
 *
 *-----------------------------------------------------------------------
 */

/*
 *  program maximum values
 *  tune as required
 */
#ifndef MAXSIZE
#define MAXSIZE  4096  /* NOTE: max 128 on a 16-bit machine */
#endif
#define MAXBANDS 10
#define MINFFT   128
#define ITRMAX   25    /* maximum number of iterations */

/*
 *  keep these..
 */
#define goback goto
#define DOloop(a,from,to) for ( (a) = (from); (a) <= (to); ++(a))
#define TWOPI (PI+PI)

void remez(double *dev, double des[], double grid[], double edge[], 
	   double wt[], int ngrid, int nbands, int iext[], double alpha[],
	   int nfcns);
void error(void);
void toomuch(void);
void ouch(int niter);
double eff(double freq, double *fx, int lband, int jtype);
double wate(double freq, double *fx, double *wtx, int lband, int jtype);
double gee(int k, int n, double *grid, double *x, double *y, double *ad);
double d(int k, int n, int m, double *x);
double dofft(double h[], int nfcns, int nfilt, int neg, int nodd, double hz);
void plotparam(double edge[],double fx[],int nbands, double min_db, double hz);

#define DIMSIZE (MAXSIZE/2 + 2)
#define WRKSIZE (16 * DIMSIZE)
#define MINFILT   2 /* was 3, but 2 seems to work (although pretty useless) */

char file_coef[256];
char file_fft[256];
char file_param[256];

/*
 *
 */
main()
{
    int jtype = 1;
    int nbands = 0;
    int nfilt = 0;
    int lgrid = 16;
    int nz;          
    int neg, nodd, nm1;
    int j, jb, k, kup, l, lband;
    double delf, change, fup, temp;
    static double edge[MAXBANDS+MAXBANDS+1],fx[MAXBANDS+1],
		  wtx[MAXBANDS+1],deviat[MAXBANDS+1];

    /*
     * THE FOLLOWING PARAMETERS ARE PASSED TO REMEZ
     */
    int ngrid;
    double dev;
    static double h[DIMSIZE+1], des[WRKSIZE+1],
		  grid[WRKSIZE+1], wt[WRKSIZE+1];

    static int iext[DIMSIZE+1];
    int nfcns;
    static double alpha[DIMSIZE+1];

    char buf[256];
    double hz = 1.0;
    /*
     * THE PROGRAM IS SET UP FOR A MAXIMUM LENGTH OF 128, BUT
     * THIS UPPER LIMIT CAN BE CHANGED BY REDIMENSIONING THE
     * ARRAYS iext, ad, alpha, x, y, h TO BE MAXSIZE/2 + 2.
     * THE ARRAYS des, grid, AND wt MUST DIMENSIONED 16(MAXSIZE/2 + 2).
     */

    /*
     * PROGRAM INPUT SECTION
     */
    printf("sampling frequency (1 for unity frequency)\n");
    printf("Freq: ");  
    fgets(buf,sizeof(buf),stdin);
    sscanf(buf,"%lf",&hz);
    printf("\n");
    if (hz == 0.0) hz = 1.0;

    printf("number of sampling points (%d..%d)\n",MINFILT,MAXSIZE);
    printf("NFILT: "); 
    fgets(buf,sizeof(buf),stdin);
    sscanf(buf,"%d",&nfilt );
    printf("\n");
    if (nfilt < MINFILT) error();
    if (nfilt > MAXSIZE) toomuch();

    printf("filter type: 1=bandpass (etc.), 2=differentiator, 3=Hilbert\n");
    printf("JTYPE: ");    
    fgets(buf,sizeof(buf),stdin);
    sscanf(buf,"%d",&jtype );
    printf("\n");
    if (jtype <= 0 || jtype > 3) error();

    if (jtype==1) printf("filter bands, e.g. 2 for simple low/highpass, 3 for simple bandpass\n");
    else printf("filter bands, normally %d\n",nbands=1);
    printf("NBANDS: "); 
    fgets(buf,sizeof(buf),stdin);
    sscanf(buf,"%d", &nbands);
    printf("\n");
    if (nbands <= 0) error();
    if (nbands > MAXBANDS) error();

    /*
     * GRID DENSITY IS ASSUMED TO BE 16 UNLESS SPECIFIED
     */
    lgrid = 16;
    printf("grid interpolation (default %d)\n",lgrid);
    printf("LGRID: ");    
    fgets(buf,sizeof(buf),stdin);
    sscanf(buf,"%d", &lgrid );
    printf("\n");
    if (lgrid <= 0) error();

    jb = 2 * nbands;

    printf("%sfrequency band edges",hz==1.0 ? "relative ":"absolute ");
    if (jtype==1) printf(" (low and high) for each band");
    printf(", %s0.0 to %.1f%s\n",hz==1.0?"scale ":"",0.5*hz,hz==1.0?"":"Hz");
    printf("ENTER EDGES: ");
    DOloop(j,1,jb) {
	printf("band %d, %s: ",(j+1)/2,(j&1)?"low":"high");
	scanf("%lf",&edge[j]);
	edge[j] /= hz;
    }
    printf("\n");

    if (jtype==1) printf("desired gain for each band, scale 0.0 to 1.0 (or more)\n");
    else if (jtype==2) printf("desired slope for each band, normally 1\n");
    else printf("desired gain for each band, normally 1\n");
    printf("ENTER FX: ");
    DOloop(j,1,nbands) {
	printf("band %d gain: ",j);
	scanf("%lf",&fx[j]);
    }
    printf("\n");

    if (jtype==1) printf("desired ripple weight for each band (e.g. 1 for passband, 10 for stopband)\n");
    else printf("desired ripple weight for each band (normally 1)\n");
    printf("ENTER WTX: ");
    DOloop(j,1,nbands) {
	printf("band %d ripple weight: ",j);
	scanf("%lf",&wtx[j]);
    }
    printf("\n");

    {
	char *p;
	printf("\nresult file for filter coefficients: ");
	fgets(file_coef,sizeof(file_coef),stdin);
	if (file_coef[0]=='\n') /* hack, to flush previous scanf() */
	     fgets(file_coef,sizeof(file_coef),stdin);
	if (p = strchr(file_coef,'\n')) *p = '\0';

	printf("\nplot result file for FFT result: ");
	fgets(file_fft,sizeof(file_fft),stdin);
	if (p = strchr(file_fft,'\n')) *p = '\0';

	printf("\nplot result file for parameters: ");
	fgets(file_param,sizeof(file_param),stdin);
	if (p = strchr(file_param,'\n')) *p = '\0';
    }
    printf("\n");

    neg = 1;
    if (jtype == 1) neg = 0;
    nodd = nfilt % 2;
    nfcns = nfilt / 2;
    if (nodd == 1 && neg == 0) nfcns = nfcns + 1;

    /*
     * SET UP THE DENSE GRID. THE NUMBER OF POINTS IN THE GRID
     * IS (FILTER LENGTH + 1)*GRID DENSITY/2
     */
    grid[1] = edge[1];
    delf = lgrid * nfcns;
    delf = 0.5 / delf;
    if (neg != 0) {
	if (edge[1] < delf) grid[1] = delf;
    }
    j = 1;
    l = 1;
    lband = 1;

    /*
     * CALCULATE THE DESIRED MAGNITUDE RESPONSE AND THE WEIGHT
     * FUNCTION ON THE GRID
     */
    for (;;) {
	fup = edge[l + 1];
	do {
	    temp = grid[j];
	    des[j] = eff(temp,fx,lband,jtype);
	    wt[j] = wate(temp,fx,wtx,lband,jtype);
	    if (++j > WRKSIZE) toomuch(); /* too many points, or too dense grid */
	    grid[j] = temp + delf;
	} while (grid[j] <= fup);

	grid[j-1] = fup;
	des[j-1] = eff(fup,fx,lband,jtype);
	wt[j-1] = wate(fup,fx,wtx,lband,jtype);
	++lband;
	l += 2;
	if (lband > nbands) break;
	grid[j] = edge[l];
    }

    ngrid = j - 1;
    if (neg == nodd) {
	if (grid[ngrid] > (0.5-delf)) --ngrid;
    }

    /*
     * SET UP A NEW APPROXIMATION PROBLEM WHICH IS EQUIVALENT
     * TO THE ORIGINAL PROBLEM
     */
    if (neg <= 0) {
	if (nodd != 1) {
	    DOloop(j,1,ngrid) {
		change = cos(PI*grid[j]);
		des[j] = des[j] / change;
		wt[j]  = wt[j] * change;
	    }
	}
    } else {
	if (nodd != 1) {
	    DOloop(j,1,ngrid) {
		change = sin(PI*grid[j]);
		des[j] = des[j] / change;
		wt[j]  = wt[j]  * change;
	    }
	} else {
	    DOloop(j,1,ngrid) {
		change = sin(TWOPI * grid[j]);
		des[j] = des[j] / change;
		wt[j]  = wt[j]  * change;
	    }
	}
    }

    /*XX*/
    temp = (double)(ngrid-1) / (double)nfcns;
    DOloop(j,1,nfcns) {
	iext[j] = (int)((j-1)*temp) + 1; /* round? !! */
    }
    iext[nfcns+1] = ngrid;
    nm1 = nfcns - 1;
    nz  = nfcns + 1;

    /*
     * CALL THE REMEZ EXCHANGE ALGORITHM TO DO THE APPROXIMATION PROBLEM
     */
    remez(&dev, des, grid, edge, wt, ngrid, nbands, iext, alpha, nfcns);

    /*
     * CALCULATE THE IMPULSE RESPONSE.
     */
    if (neg <= 0) {

	if (nodd != 0) {
	    DOloop(j,1,nm1) {
		h[j] = 0.5 * alpha[nz-j];
	    }
	    h[nfcns] = alpha[1];
	} else {
	    h[1] = 0.25 * alpha[nfcns];
	    DOloop(j,2,nm1) {
		h[j] = 0.25 * (alpha[nz-j] + alpha[nfcns+2-j]);
	    }
	    h[nfcns] = 0.5*alpha[1] + 0.25*alpha[2];
	}
    } else {
	if (nodd != 0) {
	    h[1] = 0.25 * alpha[nfcns];
	    h[2] = 0.25 * alpha[nm1];
	    DOloop(j,3,nm1) {
		h[j] = 0.25 * (alpha[nz-j] - alpha[nfcns+3-j]);
	    }
	    h[nfcns] = 0.5 * alpha[1] - 0.25 * alpha[3];
	    h[nz] = 0.0;
	} else {
	    h[1] = 0.25 * alpha[nfcns];
	    DOloop(j,2,nm1) {
		h[j] = 0.25 * (alpha[nz-j] - alpha[nfcns+2-j]);
	    }
	    h[nfcns] = 0.5 * alpha[1] - 0.25 * alpha[2];
	}
    }

/*XX*/
    printf("\n**********************************************************\n");
    printf("               FINITE IMPULSE RESPONSE (FIR)\n");
    printf("             LINEAR PHASE DIGITAL FILTER DESIGN\n");
    printf("                  REMEZ EXCHANGE ALGORITHM\n");

    if (jtype == 1) printf("                      BANDPASS FILTER\n");
    if (jtype == 2) printf("                      DIFFERENTIATOR\n");
    if (jtype == 3) printf("                      HILBERT TRANSFORMER\n");
    printf("                    FILTER LENGTH = %3d\n",nfilt);
    printf("               ***** IMPULSE RESPONSE *****\n");

    DOloop(j,1,nfcns) {
	k = nfilt + 1 - j;
	if (neg == 0)
	    printf("             h(%2d) = %13lf =   h(%2d)\n",j,h[j],k);
	else
	    printf("             h(%2d) = %13lf = - h(%2d)\n",j,h[j],k);
    }
    if (neg == 1 && nodd == 1)
	printf("             h(%2d) = %13lf\n",nz,0.0);

    /* 
     *  print result to file
     */
    if (file_coef[0]) {
	FILE *f;

	if (f = fopen(file_coef,"w")) {
	    for (j=1; j <= nfcns; ++j) {
		fprintf(f,"%13lf\n",h[j]);
	    }
	    if (neg == 1 && nodd == 1)
		fprintf(f,"%13lf\n",0.0); /* corr. by [e-mail] */
	    for (j=nfcns; j >= 1; --j) {
		fprintf(f,"%13lf\n",neg == 0 ? h[j] : -h[j]);
	    }
	    fclose(f);
	}
    }

    for (k=1; k <= nbands; k += 4) {

	kup = k + 3;
	if (kup > nbands) kup = nbands;
	printf("\n                       ");
	DOloop(j,k,kup) printf("      BAND%3d ",j);

	printf("\nLOWER BAND EDGE         ");
	DOloop(j,k,kup) printf("%13lf%s ", edge[2*j-1]*hz,hz==1.0 ? "":"Hz");

	printf("\nUPPER BAND EDGE         ");
	DOloop(j,k,kup) printf("%13lf%s ", edge[2*j]*hz,hz==1.0 ? "":"Hz");

	if (jtype != 2) printf("\nDESIRED VALUE           ");
	else printf("\nDESIRED SLOPE           ");
	DOloop(j,k,kup) printf("%13lf ", fx[j]);

	printf("\nWEIGHTING               ");
	DOloop(j,k,kup) printf("%13lf ", wtx[j]);

	DOloop(j,k,kup) deviat[j] = dev / wtx[j];

	printf("\nDEVIATION               ");
	DOloop(j,k,kup) printf("%13lf ", deviat[j]);

	if (jtype == 1) {
	    printf("\nDEVIATION IN dB         ");
	    DOloop(j,k,kup) printf("%13lf ", 20.0 * log10(deviat[j]));
	}
    }

    DOloop(j,1,nz)      grid[j] = grid[iext[j]];
    printf("\n\nEXTREMAL FREQUENCIES (MAXIMA OF THE ERROR CURVE)\n");
    DOloop(j,1,nz) {
	printf("%13lf%s%c",grid[j]*hz,hz==1.0 ? "":"Hz",(j%5) ? ' ':'\n');
    }

    printf("\n************************************************************\n");

    temp = dofft(h,nfcns,nfilt,neg,nodd,hz);
    plotparam(edge,fx,nbands,temp,hz);
}

/*
 *-----------------------------------------------------------------------
 * FUNCTION: eff
 *  FUNCTION TO CALCULATE THE DESIRED MAGNITUDE RESPONSE
 *  AS A FUNCTION OF FREQUENCY.
 *  AN ARBITRARY FUNCTION OF FREQUENCY CAN BE
 *  APPROXIMATED IF THE USER REPLACES THIS FUNCTION
 *  WITH THE APPROPRIATE CODE TO EVALUATE THE IDEAL
 *  MAGNITUDE.  NOTE THAT THE PARAMETER FREQ IS THE
 *  VALUE OF NORMALIZED FREQUENCY NEEDED FOR EVALUATION.
 *-----------------------------------------------------------------------
 */
double eff(double freq, double *fx, int lband, int jtype)
{
      if (jtype != 2) return fx[lband];
      else            return fx[lband] * freq;
}

/*
 *-----------------------------------------------------------------------
 * FUNCTION: wate
 *  FUNCTION TO CALCULATE THE WEIGHT FUNCTION AS A FUNCTION
 *  OF FREQUENCY.  SIMILAR TO THE FUNCTION eff, THIS FUNCTION CAN
 *  BE REPLACED BY A USER-WRITTEN ROUTINE TO CALCULATE ANY
 *  DESIRED WEIGHTING FUNCTION.
 *-----------------------------------------------------------------------
 */
double wate(double freq, double *fx, double *wtx, int lband, int jtype)
{
      if (jtype != 2)          return wtx[lband];
      if (fx[lband] >= 0.0001) return wtx[lband] / freq;
      return                          wtx[lband];
}

/*
 *-----------------------------------------------------------------------
 * SUBROUTINE: error
 *  THIS ROUTINE WRITES AN ERROR MESSAGE IF AN
 *  ERROR HAS BEEN DETECTED IN THE INPUT DATA.
 *-----------------------------------------------------------------------
 */
void error(void)
{
    printf("Error in input data.\n");
    exit(2);
}

/*
 *-----------------------------------------------------------------------
 * SUBROUTINE: toomuch
 *  THIS ROUTINE WRITES AN ERROR MESSAGE IF TOO
 *  MANY SAMPLE POINTS OR TOO DENSE GRID IS ENTERED.
 *-----------------------------------------------------------------------
 */
void toomuch(void)
{
    printf("Too many sample points or too dense grid.\n");
    exit(2);
}

/*
 *-----------------------------------------------------------------------
 * SUBROUTINE: remez
 *  THIS SUBROUTINE IMPLEMENTS THE REMEZ EXCHANGE ALGORITHM
 *  FOR THE WEIGHTED CHEBYSHEV APPROXIMATION OF A CONTINUOUS
 *  FUNCTION WITH A SUM OF COSINES.  INPUTS TO THE SUBROUTINE
 *  ARE A DENSE GRID WHICH REPLACES THE FREQUENCY AXIS, THE
 *  DESIRED FUNCTION ON THIS GRID, THE WEIGHT FUNCTION ON THE
 *  GRID, THE NUMBER OF COSINES, AND AN INITIAL GUESS OF THE
 *  EXTREMAL FREQUENCIES.  THE PROGRAM MINIMIZES THE CHEBYSHEV
 *  ERROR BY DETERMINING THE BEST LOCATION OF THE EXTREMAL
 *  FREQUENCIES (POINTS OF MAXIMUM ERROR) AND THEN CALCULATES
 *  THE COEFFICIENTS OF THE BEST APPROXIMATION.
 *-----------------------------------------------------------------------
 */
void remez(double *dev, double des[], double grid[], double edge[],  
	   double wt[], int ngrid, int nbands, int iext[], double alpha[],
	   int nfcns)
		/* dev, iext, alpha                         are output types */
		/* des, grid, edge, wt, ngrid, nbands, nfcns are input types */
{
    int i, k, k1, kkk, kn, knz, klow, kup, nz, nzz, nm1;
    int cn;
    int j, jchnge, jet, jm1, jp1;
    int l, luck, nu, nut, nut1, niter;

    double ynz, comp, devl, gtemp, fsh, y1, err, dtemp, delf, dnum, dden;
    double aa, bb, ft, xe, xt, xt1, temp;

    static double a[DIMSIZE+1], p[DIMSIZE+1], q[DIMSIZE+1];
    static double ad[DIMSIZE+1], x[DIMSIZE+1], y[DIMSIZE+1];

    devl = -1.0;
    nz  = nfcns+1;
    nzz = nfcns+2;
    niter = 0;

    do {
    L100:
	iext[nzz] = ngrid + 1;
	++niter;

	if (niter > ITRMAX) break;