remez.c

remez算法设计FIR滤波器源码

	printf("ITERATION %2d: ",niter);

	DOloop(j,1,nz) {
	    x[j] = cos(grid[iext[j]]*TWOPI);
	}
	jet = (nfcns-1) / 15 + 1;

	DOloop(j,1,nz) {
	    ad[j] = d(j,nz,jet,x);
	}

	dnum = 0.0;
	dden = 0.0;
	k = 1;

	DOloop(j,1,nz) {
	    l = iext[j];
	    dnum += ad[j] * des[l];
	    dden += (double)k * ad[j] / wt[l];
	    k = -k;
	}
	*dev = dnum / dden;

	printf("DEVIATION = %lg\n",*dev);

	nu = 1;
	if ( (*dev) > 0.0 ) nu = -1;
	(*dev) = -(double)nu * (*dev);
	k = nu;
	DOloop(j,1,nz) {
	    l = iext[j];
	    y[j] = des[l] + (double)k * (*dev) / wt[l];
	    k = -k;
	}
	if ( (*dev) <= devl ) {
	    /* finished */
	    ouch(niter);
	    break;
	}
	devl = (*dev);
	jchnge = 0;
	k1 = iext[1];
	knz = iext[nz];
	klow = 0;
	nut = -nu;
	j = 1;

    /*
     * SEARCH FOR THE EXTREMAL FREQUENCIES OF THE BEST APPROXIMATION
     */

    L200:
	if (j == nzz) ynz = comp;
	if (j >= nzz) goto L300;
	kup = iext[j+1];
	l = iext[j]+1;
	nut = -nut;
	if (j == 2) y1 = comp;
	comp = (*dev);
	if (l >= kup) goto L220;
	err = (gee(l,nz,grid,x,y,ad)-des[l]) * wt[l];
	if (((double)nut*err-comp) <= 0.0) goto L220;
	comp = (double)nut * err;
    L210:
	if (++l >= kup) goto L215;
	err = (gee(l,nz,grid,x,y,ad)-des[l]) * wt[l];
	if (((double)nut*err-comp) <= 0.0) goto L215;
	comp = (double)nut * err;
	goback L210;

    L215:
	iext[j++] = l - 1;
	klow = l - 1;
	++jchnge;
	goback L200;

    L220:
	--l;
    L225:
	if (--l <= klow) goto L250;
	err = (gee(l,nz,grid,x,y,ad)-des[l]) * wt[l];
	if (((double)nut*err-comp) > 0.0) goto L230;
	if (jchnge <= 0) goto L225;
	goto L260;

    L230:
	comp = (double)nut * err;
    L235:
	if (--l <= klow) goto L240;
	err = (gee(l,nz,grid,x,y,ad)-des[l]) * wt[l];
	if (((double)nut*err-comp) <= 0.0) goto L240;
	comp = (double)nut * err;
	goback L235;
    L240:
	klow = iext[j];
	iext[j] = l+1;
	++j;
	++jchnge;
	goback L200;

    L250:
	l = iext[j]+1;
	if (jchnge > 0) goback L215;

    L255:
	if (++l >= kup) goto L260;
	err = (gee(l,nz,grid,x,y,ad)-des[l]) * wt[l];
	if (((double)nut*err-comp) <= 0.0) goback L255;
	comp = (double)nut * err;

	goback L210;
    L260:
	klow = iext[j++];
	goback L200;

    L300:
	if (j > nzz) goto L320;
	if (k1 > iext[1] ) k1 = iext[1];
	if (knz < iext[nz]) knz = iext[nz];
	nut1 = nut;
	nut = -nu;
	l = 0;
	kup = k1;
	comp = ynz*(1.00001);
	luck = 1;
    L310:
	if (++l >= kup) goto L315;
	err = (gee(l,nz,grid,x,y,ad)-des[l]) * wt[l];
	if (((double)nut*err-comp) <= 0.0) goback L310;
	comp = (double) nut * err;
	j = nzz;
	goback L210;

    L315:
	luck = 6;
	goto L325;

    L320:
	if (luck > 9) goto L350;
	if (comp > y1) y1 = comp;
	k1 = iext[nzz];
    L325:
	l = ngrid+1;
	klow = knz;
	nut = -nut1;
	comp = y1*(1.00001);
    L330:
	if (--l <= klow) goto L340;
	err = (gee(l,nz,grid,x,y,ad)-des[l]) * wt[l];
	if (((double)nut*err-comp) <= 0.0) goback L330;
	j = nzz;
	comp = (double) nut * err;
	luck = luck + 10;
	goback L235;
    L340:
	if (luck == 6) goto L370;
	DOloop(j,1,nfcns) {
	    iext[nzz-j] = iext[nz-j];
	}
	iext[1] = k1;
	goback L100;
    L350:
	kn = iext[nzz];
	DOloop(j,1,nfcns) iext[j] = iext[j+1];
	iext[nz] = kn;

	goback L100;
    L370:
	;
    } while (jchnge > 0);

/*
 *    CALCULATION OF THE COEFFICIENTS OF THE BEST APPROXIMATION
 *    USING THE INVERSE DISCRETE FOURIER TRANSFORM
 */
    nm1 = nfcns - 1;
    fsh = 1.0e-06;
    gtemp = grid[1];
    x[nzz] = -2.0;
    cn  = 2*nfcns - 1;
    delf = 1.0/cn;
    l = 1;
    kkk = 0;
/*XX*/
#if 0
    if (grid[1] < 0.01 && grid[ngrid] > 0.49) kkk = 1;
#else
    if (edge[1] == 0.0 && edge[2*nbands] == 0.5) kkk = 1;
#endif
    if (nfcns <= 3) kkk = 1;
    if (kkk !=     1) {
	dtemp = cos(TWOPI*grid[1]);
	dnum  = cos(TWOPI*grid[ngrid]);
	aa    = 2.0/(dtemp-dnum);
	bb    = -(dtemp+dnum)/(dtemp-dnum);
    }

    DOloop(j,1,nfcns) {
	ft = (j - 1) * delf;
	xt = cos(TWOPI*ft);
	if (kkk != 1) {
	    xt = (xt-bb)/aa;
#if 0
	    /*XX* ckeck up !! */
	    xt1 = sqrt(1.0-xt*xt);
	    ft = atan2(xt1,xt)/TWOPI;
#else
	    ft = acos(xt)/TWOPI;
#endif
	}
L410:
	xe = x[l];
	if (xt > xe) goto L420;
	if ((xe-xt) < fsh) goto L415;
	++l;
	goback L410;
L415:
	a[j] = y[l];
	goto L425;
L420:
	if ((xt-xe) < fsh) goback L415;
	grid[1] = ft;
	a[j] = gee(1,nz,grid,x,y,ad);
L425:
	if (l > 1) l = l-1;
    }

    grid[1] = gtemp;
    dden = TWOPI / cn;
    DOloop (j,1,nfcns) {
	dtemp = 0.0;
	dnum = (j-1) * dden;
	if (nm1 >= 1) {
	    DOloop(k,1,nm1) {
		dtemp += a[k+1] * cos(dnum*k);
	    }
	}
	alpha[j] = 2.0 * dtemp + a[1];
    }

    DOloop(j,2,nfcns) alpha[j] *= 2.0 / cn;
    alpha[1] /= cn;

    if (kkk != 1) {
	p[1] = 2.0*alpha[nfcns]*bb+alpha[nm1];
	p[2] = 2.0*aa*alpha[nfcns];
	q[1] = alpha[nfcns-2]-alpha[nfcns];
	DOloop(j,2,nm1) {
	    if (j >= nm1) {
		aa *= 0.5;
		bb *= 0.5;
	    }
	    p[j+1] = 0.0;
	    DOloop(k,1,j) {
		a[k] = p[k];
		p[k] = 2.0 * bb * a[k];
	    }
	    p[2] += a[1] * 2.0 *aa;
	    jm1 = j - 1;
	    DOloop(k,1,jm1) p[k] += q[k] + aa * a[k+1];
	    jp1 = j + 1;
	    DOloop(k,3,jp1) p[k] += aa * a[k-1];

	    if (j != nm1) {
		DOloop(k,1,j) q[k] = -a[k];
		q[1] += alpha[nfcns - 1 - j];
	    }
	}
	DOloop(j,1,nfcns) alpha[j] = p[j];
    }

    if (nfcns <= 3) {
	  alpha[nfcns+1] = alpha[nfcns+2] = 0.0;
    }
}

/*
 *-----------------------------------------------------------------------
 * FUNCTION: d
 *  FUNCTION TO CALCULATE THE LAGRANGE INTERPOLATION
 *  COEFFICIENTS FOR USE IN THE FUNCTION gee.
 *-----------------------------------------------------------------------
 */
double d(int k, int n, int m, double *x)
{
    int j, l;
    double q, retval;

    retval = 1.0;
    q = x[k];
    DOloop(l,1,m) {
	for (j = l; j <= n; j += m) {
	    if (j != k)
		retval *= 2.0 * (q - x[j]);
	}
    }
    return 1.0 / retval;
}

/*
 *-----------------------------------------------------------------------
 * FUNCTION: gee
 *  FUNCTION TO EVALUATE THE FREQUENCY RESPONSE USING THE
 *  LAGRANGE INTERPOLATION FORMULA IN THE BARYCENTRIC FORM
 *-----------------------------------------------------------------------
 */
double gee(int k, int n, double *grid, double *x, double *y, double *ad)
{
    int j;
    double p,c,d,xf;

    d = 0.0;
    p = 0.0;
    xf = cos(TWOPI * grid[k]);

    DOloop(j,1,n) {
	c = ad[j] / (xf - x[j]);
	d += c;
	p += c * y[j];
    }

    return p/d;
}

/*
 *-----------------------------------------------------------------------
 * SUBROUTINE: ouch
 *  WRITES AN ERROR MESSAGE WHEN THE ALGORITHM FAILS TO
 *  CONVERGE.  THERE SEEM TO BE TWO CONDITIONS UNDER WHICH
 *  THE ALGORITHM FAILS TO CONVERGE: (1) THE INITIAL
 *  GUESS FOR THE EXTREMAL FREQUENCIES IS SO POOR THAT
 *  THE EXCHANGE ITERATION CANNOT GET STARTED, OR
 *  (2) NEAR THE TERMINATION OF A CORRECT DESIGN,
 *  THE DEVIATION DECREASES DUE TO ROUNDING ERRORS
 *  AND THE PROGRAM STOPS.  IN THIS LATTER CASE THE
 *  FILTER DESIGN IS PROBABLY ACCEPTABLE, BUT SHOULD
 *  BE CHECKED BY COMPUTING A FREQUENCY RESPONSE.
 *-----------------------------------------------------------------------
 */
void ouch(int niter)
{
    printf("Failure to converge after %d iterations.\n", niter);
    printf("Design maybe correct anyways, but verify with FFT.\n");
}

/*************************************************************************/
/*
 *  FFT section
 */

struct complex { double r; double i; } ;

static struct complex a[MAXSIZE+1];

void fft(int m);

/*
 *  prepare for FFT
 *  return minimum dB for plotting
 */ 
double dofft(double h[], int nfcns, int nfilt, int neg, int nodd, double hz)
{
    int j;
    int n,m;
    int k;
    double d;
    double db;
    double min_db;
    double fq;
    double deg;
    FILE *f;

    for (j = 1; j <= nfcns; ++j) {
	k = nfilt + 1 - j;
	a[j].r = h[j];
	a[j].i = 0.0;
	if (neg == 0) {
	    a[k].r = h[j];
	} else {
	    a[k].r = -h[j];
	}
	a[k].i = 0.0;
    }
    if (neg == 1 && nodd == 1) {
	a[nfcns+1].r = 0.0;
	a[nfcns+1].i = 0.0;
    }

    /* select fourier size */
    m = 1;
    for (;;) {
	n = 2 << (m-1);
	if (n >= MAXSIZE) break;
	if (n >= MINFFT && n >= nfilt) break;
	++m;
    }

    /* pad with zeros */
    for (j = nfilt+1; j <= n; ++j) {
	a[j].r = 0.0;
	a[j].i = 0.0;
    }

#if 0
    printf("\n\ndeBUG: h[n]\n");
    for (j = 1; j <= n; ++j) {
	printf("%13lf%c",a[j].r,(j%5) ? ' ':'\n');
    }
#endif

    fft(m);

    /* 
     *  print result to screen
     */

    printf("\nRESULTING FREQUENCY RESPONSE\n");
    printf("%12s%s   %12s %8s    %8s    \n",
	   "FREQ.", hz==1.0 ? "":"  ", 
	   "MAGNITUDE","","PHASE");
    min_db = 0.0;
    for (j = 1; j <= (n/2 + 1); ++j) {
	/* frequency */ 
	fq = (1.0*(j-1))/n;

	/* magnitude */
	d = sqrt(a[j].r*a[j].r + a[j].i*a[j].i);
	if (d == 0.0) {
	    db = -999.0;
	    deg = 0.0;
	} else {
	    db = 20.0 * log10(d);
	    if (db < min_db) min_db = db;

	    /* phase, subtracting fixed delay through filter */
	    deg = atan2(a[j].i,a[j].r) * 360.0 / TWOPI;
	    deg -= (360.0 * (nfilt-1) / 2.0) * fq;
	    while (deg <= -180.0) deg += 360.0;
	}

	printf("%12.4lf%s   %12.8lf %8.3lf dB %8.3lf deg\n",
			fq*hz, hz==1.0 ? "":"Hz",
			d, db, deg);
    }

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

    /* make min_db suitable for plot scaling */
    min_db = (double)(10 * ((((int)min_db) / 10) - 1));

    /* 
     *  print result to file
     *  suitable for gnuplot
     */
    if (file_fft[0]) {
	if (f = fopen(file_fft,"w")) {
	
	    for (j = 1; j <= (n/2 + 1); ++j) {
		/* frequency */
		fq = (1.0*(j-1))/n;
	
		/* magnitude */
		d = sqrt(a[j].r*a[j].r + a[j].i*a[j].i);
		if (d == 0.0) {
		    db = min_db;
		} else {
		    db = 20.0 * log10(d);
		}
		fprintf(f,"%12.4lf, %8.3lf\n", fq*hz, db);
	    }
	    fclose(f);
	}
    }
    return min_db;
}

/*
 *  output input parameters for bandpass filter
 */
void plotparam(double edge[],double fx[],int nbands,double min_db,double hz)
{
    int j;
    double d;
    double db;
    double fq;
    FILE *f;

    /* 
     *  print result to file
     *  suitable for gnuplot
     */
    if (file_param[0]) {
	if (f = fopen(file_param,"w")) {
	    for (j = 1; j <= 2*nbands; ++j) {
		/* frequency */
		fq = edge[j];
	
		/* magnitude */
		d = fx[(j+1)/2];
		if (d == 0.0) {
		    db = min_db;
		} else {
		    db = 20.0 * log10(d);
		}
		fprintf(f,"%12.4lf, %8.3lf\n", fq*hz, db);
	    }
	    fclose(f);
	}
    }
}

/*
 *  fast fourier, 2^m
 *  decimation in time, radix 2, in place
 *  n = 2**m, e.g. 4 -> 16 
 */
void fft(int m)
{
    struct complex u,w,t;
    int n;
    int nv2;
    int i,j,k;
    int l,le,le1;
    int ip;

    n = 2 << (m-1);
    nv2 = n/2;
    j = 1;

    /* swap input values */
    for (i = 1; i < n; ++i) {
	if (i < j) {
	    t.r = a[j].r;
	    t.i = a[j].i;
	    a[j].r = a[i].r;
	    a[j].i = a[i].i;
	    a[i].r = t.r;
	    a[i].i = t.i;
	}
	k = nv2;
	while (k < j) {
	    j -= k;
	    k >>= 1;
	}
	j += k;
    }

    /* loop all m-stages */
    for (l = 1; l <= m; ++l) {
	le = 2 << (l-1);
	le1 = le/2;
	u.r = 1.0;
	u.i = 0.0;
	w.r = cos(PI/le1);
	w.i = sin(PI/le1); /* inverse fft: -sin */

	/* loop all butterflies within stage */
	for (j = 1; j <= le1; ++j) {

	    /* inner loop */
	    for (i = j; i <= n; i += le) {
		ip = i + le1;
		t.r = a[ip].r * u.r - a[ip].i * u.i;
		t.i = a[ip].r * u.i + a[ip].i * u.r;
		a[ip].r = a[i].r - t.r;
		a[ip].i = a[i].i - t.i;
		a[i].r = a[i].r + t.r;
		a[i].i = a[i].i + t.i;
	    }
	    t.r = u.r * w.r - u.i * w.i;
	    t.i = u.r * w.i + u.i * w.r;
	    u.r = t.r;
	    u.i = t.i;
	}
    }
    /* inverse fft: multiply result by 1/n? */
}