firdf.cpp

利用最佳一致逼近设计数字滤波器的程序

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

static int remez1(void); 
static  double d(int j,int k,int m);
static  double gee(int k,int n);
//static void ouch(void);

static  double x[67],y[67],ad[67],alpha[67];
static  double des[1046],grid[1046],wt[1046];
static int iext[67],nfcns,ngrid,niter;
static  double pi2,dev;


//defirdf3: To design FIR filter by Weighted Chebyshev Approximation
//Input parameters:
//N      :滤波器长度 
//Nbands :通带和阻带的总数
//Fx     :数组，Fx[0]~Fx[Nbands-1]依次对应每一个通带和阻带的幅值
//Wtx    :数组，Wtx[0]~Wtx[Nbands-1]依次对应每一个通带和阻带的加权
//Edge   :数组，Edge[0]~Edge[2*Nbands-1]依次对应每一个通带和阻带的下限和上限频率

//Output parameters:

//h  :一维N元实数组，单位抽样响应.

int defirdf3(int N,int Nbands,double *Edge,double *Fx,double *Wtx,double *h)
{
	double *edge,*fx,*wtx;
	int nfilt,nbands;
	int nfmax=128,lgrid=16;
    double  pi;
   
    nfilt=N-1;
	nbands=Nbands;
    edge=(double *)malloc(sizeof(double)*(2*nbands+1));
	fx=(double *)malloc(sizeof(double)*(nbands+1));
	wtx=(double *)malloc(sizeof(double)*(nbands+1)); 

   /* edge=new double(2*nbands+1);
	fx=new double((nbands+1));
	wtx=new double((nbands+1));*/
	for(int i=1;i<=2*nbands;i++)
		edge[i]=Edge[i-1];
    for(i=1;i<=nbands;i++)
	{
		fx[i]=Fx[i-1];
		wtx[i]=Wtx[i-1];
	}
	//pi2=8.0*atan(1.0);
    pi=3.14159265358979;
	pi2=pi*2.0;
    if(nbands>10) return -1;
	if(!((nfilt<=nfmax)&&(nfilt>=3))) return -2;

	int jb,nodd,j,l,lband;
	double delf,fup,temp;

	if(nbands<=0)nbands=1;
	jb=2*nbands;
	nfilt=nfilt+1;
	nodd=nfilt/2;
	nodd=nfilt-2*nodd;
	nfcns=nfilt/2;
	if(nodd==1)nfcns=nfcns+1;
	grid[1]=edge[1];
	delf=lgrid*nfcns;
	delf=0.5/delf;
	if(edge[1]<delf)grid[1]=delf;
	j=1;
	l=1;
	lband=1;
	do
	{
		fup=edge[l+1];
		do
		{
		
			temp=grid[j];
			des[j]=fx[lband];
			wt[j]=wtx[lband];
			j=j+1;
			grid[j]=temp+delf;
			if(grid[j]>fup)break;
		}while(1);
		grid[j-1]=fup;
		des[j-1]=fx[lband];
		wt[j-1]=wtx[lband];
		lband=lband+1;
		l=l+2;
		if(lband>nbands)break;
        grid[j]=edge[l];

	}while(1);

	ngrid=j-1;



	if(nodd==0)
	   if(grid[ngrid]>(0.5-delf))ngrid=ngrid-1;
    
 
	double change;
	if(nodd!=1)
	{
		for(j=1;j<=ngrid;j++)
		{
			change=cos(pi*grid[j]);
			des[j]=des[j]/change;
			wt[j]=wt[j]*change;
		}
	}
	int xt;
	temp=double(ngrid-1)/double(nfcns);
	for(j=1;j<=nfcns;j++)
	{
		xt=j-1;
		iext[j]=xt*temp+1.0;
	}
    iext[nfcns+1]=ngrid;
    
	int nm1,nz;
	nm1=nfcns-1;
	nz=nfcns+1;

	remez1();

    int nzmj,nf2j;
	if(nodd!=0)
	{
		for(j=0;j<=nm1-1;j++)
		{
			nzmj=nfcns-j;
			h[j]=0.5*alpha[nzmj];
			h[nfilt-j-1]=h[j];
		}
		h[nm1]=alpha[1];
	}
	else
	{
		h[0]=0.25*alpha[nfcns];
		h[nfilt-1]=h[0];
        for(j=1;j<=nm1-1;j++)
		{
			nzmj=nfcns-j;
			nf2j=nz-j;
			h[j]=0.25*(alpha[nzmj]+alpha[nf2j]);
			h[nfilt-j-1]=h[j];
		}
		h[nm1]=0.5*alpha[1]+0.25*alpha[2];
		h[nfcns]=h[nm1];
	}
	
	printf("\n              偏差(纹波)     衰减(db)\n");
    double a;
    for(j=1;j<=nbands;j++)
	{
		a=dev/wtx[j];
		printf("带 %d        %f       %f\n",j,a,20.0*log10(a+fx[j]));
	}
	nfilt=nfilt-1;
	
	//for(i=0;i<N;i++)printf("%f ",h[i]);

	free(edge);free(fx);free(wtx);
    
	return 0;

}

int remez1(void)
{
	double a[67],p[67],q[67];


	int itrmax=25;
	double devl=-1.0;
	int nz,nzz;
	nz=nfcns+1;
	nzz=nfcns+2;
	niter=0;
	//100开始
	int jxt,jet,j,k,l,nu,k1,knz,klow,nut,kup,jchnge;
	int nut1,luck,nzzmj,nzmj,kn,nm1,kkk,jm1,jp1,nf1j;
    double dtemp,dnum,dden,ynz,comp,err,fsh,gtemp,cn,delf,y1;
	double aa,bb,ft,xt,xt1,xe,dak,dk;
    do
	{
		iext[nzz]=ngrid+1;
		niter=niter+1;
		if(niter>itrmax)break;
		//110开始
		for(j=1;j<=nz;j++)
		{
			jxt=iext[j];
			dtemp=grid[jxt];
			dtemp=cos(dtemp*pi2);
			x[j]=dtemp;
		}
		//110 end
		jet=(nfcns-1)/15+1;
		for(j=1;j<=nz;j++)
			ad[j]=d(j,nz,jet);
		dnum=0.0;
		dden=0.0;
		k=1;
		//130 begin
		for(j=1;j<=nz;j++)
		{
			l=iext[j];
			dtemp=ad[j]*des[l];
			dnum=dnum+dtemp;
			dtemp=k*ad[j]/wt[l];
			dden=dden+dtemp;
			k=-k;
		}
		//130 end
		dev=dnum/dden;
		nu=1;
		if(dev>0.0)nu=-1;
		dev=-nu*dev;
		k=nu;
		//140 begin
		for(j=1;j<=nz;j++)
		{
			l=iext[j];
			dtemp=k*dev/wt[l];
			y[j]=des[l]+dtemp;
			k=-k;
		}
		//140 end;
		if(dev<=devl)
		{
		//	ouch();
			break;
		}
		devl=dev;
		jchnge=0;
		k1=iext[1];
		knz=iext[nz];
		klow=0;
		nut=-nu;
		j=1;
		//200 begin
n200:
		
			if(j==nzz)ynz=comp;
			if(j>=nzz)goto n300;
			kup=iext[j+1];
			l=iext[j]+1;
			nut=-nut;
			if(j==2) y1=comp;
			comp=dev;
			if(l>=kup)goto n220;
			err=gee(l,nz);
			err=(err-des[l])*wt[l];
			dtemp=nut*err-comp;
			if(dtemp<=0.0)goto n220;
			comp=nut*err;
n210:
			//210 begin
			do
			{
				l=l+1;
				if(l>=kup) break;//goto n215;
				err=gee(l,nz);
				err=(err-des[l])*wt[l];
				dtemp=nut*err-comp;
				if(dtemp<=0)break;//goto n215;
				comp=nut*err;
			}while(1);
			
n215:
				iext[j]=l-1;
				j=j+1;
				klow=l-1;
				jchnge=jchnge+1;
				goto n200;
n220:
				l=l-1;
n225:
				l=l-1;
				if(l<=klow)goto n250;
				err=gee(l,nz);
				err=(err-des[l])*wt[l];
				dtemp=nut*err-comp;
				if(dtemp>0)goto n230;
				if(jchnge<=0) goto n225;
				goto n260;

n230:
				comp=nut*err;
n235:		
				while(1)
				{
					l=l-1;
					if(l<=klow)break;
					err=gee(l,nz);
					err=(err-des[l])*wt[l];
					dtemp=nut*err-comp;
					if(dtemp<=0)break;
					comp=nut*err;
				}
//n240:
				klow=iext[j];
				iext[j]=l+1;
				j=j+1;
				jchnge=jchnge+1;
				goto n200;
n250:
				l=iext[j]+1;
				if(jchnge>0)goto n215;
n255:
				l=l+1;
				if(l>=kup)goto n260;
				err=gee(l,nz);
				err=(err-des[l])*wt[l];
				dtemp=nut*err-comp;
				if(dtemp<=0)goto n255;
				comp=nut*err;
				goto n210;
n260:
				klow=iext[j];
				j=j+1;
				goto n200;
n300:
				if(j>nzz)goto n320;
				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.0000001;
				luck=1;
n310:
				l=l+1;
				if(l>=kup)goto n315;
				err=gee(l,nz);
				err=(err-des[l])*wt[l];
				dtemp=nut*err-comp;
				if(dtemp<=0)goto n310;
				comp=nut*err;
				j=nzz;
				goto n210;
n315:
				luck=6;
				goto n325;
n320:
				if(luck>9) goto n350;
				if(comp>y1) y1=comp;
				k1=iext[nzz];
n325:
				l=ngrid+1;
				klow=knz;
				nut=-nut1;
				comp=y1*1.0000001;
n330:
				l=l-1;
				if(l<=klow)goto n340;
				err=gee(l,nz);
				err=(err-des[l])*wt[l];
				dtemp=nut*err-comp;
				if(dtemp<=0)goto n330;
				j=nzz;
				comp=nut*err;
				luck=luck+10;
				goto n235;
				
n340:
				if(luck==6) goto n370;
				for(j=1;j<=nfcns;j++)
				{
					nzzmj=nzz-j;
					nzmj=nz-j;
					iext[nzzmj]=iext[nzmj];
				}
				iext[1]=k1;
				continue;
				//goto n100;

n350:
				kn=iext[nzz];
				for(j=1;j<=nfcns;j++)
					iext[j]=iext[j+1];

				iext[nz]=kn;
				continue;
n370:
		        if(jchnge<=0)break;




	}while(1);
	// 100 end ;400 begin 

	nm1=nfcns-1;
	fsh=1.0e-6;
	gtemp=grid[1];
	x[nzz]=-2.0;
	cn=2*nfcns-1;
	delf=1.0/cn;
	l=1;
	kkk=0;
	if((grid[1]<=0.01)&&(grid[ngrid]>0.49))kkk=1;
	if(nfcns<=3)kkk=1;
	if(kkk!=1)
	{
		dtemp=cos(pi2*grid[1]);
		dnum=cos(pi2*grid[ngrid]);
		aa=2.0/(dtemp-dnum);
		bb=-(dtemp+dnum)/(dtemp-dnum);

	}
	//430 begin
	for(j=1;j<=nfcns;j++)
	{
		ft=j-1;
		ft=ft*delf;
		xt=cos(pi2*ft);
		if(kkk!=1)
		{
			xt=(xt-bb)/aa;
			xt1=sqrt(1.0-xt*xt);
			ft=atan2(xt1,xt)/pi2;  
		}
n410:
		xe=x[l];
		if(xt>xe)goto n420;
		if((xe-xt)<fsh)goto n415;
		l=l+1;
		goto n410;
n415:
		a[j]=y[l];
		goto n425;
n420:
		if((xt-xe)<fsh)goto n415;
		grid[1]=ft;
		a[j]=gee(1,nz);
n425:
		if(l>1)l=l-1;
	}
	//430 end
	grid[1]=gtemp;
	dden=pi2/cn;
	//510 begin
	for(j=1;j<=nfcns;j++)
	{
		dtemp=0.0;
		dnum=j-1;
		dnum=dnum*dden;
		if(nm1>=1)
		{
			for(k=1;k<=nm1;k++)
			{
				dak=a[k+1];
				dk=k;
				dtemp=dtemp+dak*cos(dnum*dk);
			}
		}
		dtemp=dtemp*2.0+a[1];
		alpha[j]=dtemp;
	}
     //510 end
	for(j=2;j<=nfcns;j++)
		alpha[j]=alpha[j]*2.0/cn;
	alpha[1]=alpha[1]/cn;
	
	if(kkk!=1)//goto n545;
	{
		p[1]=2.0*alpha[nfcns]*bb+alpha[nm1];
		p[2]=2.0*alpha[nfcns]*aa;
		q[1]=alpha[nfcns-2]-alpha[nfcns];
		//540 begin
		for(j=2;j<=nm1;j++)
		{
			if(j>=nm1){aa=0.5*aa;bb=0.5*bb;}
			p[j+1]=0.0;
			for(k=1;k<=j;k++)
			{
				a[k]=p[k];
				p[k]=2.0*bb*a[k];
			}
			p[2]=p[2]+a[1]*2.0*aa;
			jm1=j-1;
			for(k=1;k<=jm1;k++)
				p[k]=p[k]+q[k]+aa*a[k+1];
			jp1=j+1;
			for(k=3;k<=jp1;k++)
				p[k]=p[k]+aa*a[k-1];
			if(j==nm1)continue;//goto 540
			for(k=1;k<=j;k++)
				q[k]=-a[k];
			nf1j=nfcns-j-1;
			q[1]=q[1]+alpha[nf1j];
		}
		//540 end
		for(j=1;j<=nfcns;j++)
			alpha[j]=p[j];
	}
	//545 end if
	if(nfcns>3)return 1;
	alpha[nfcns+1]=0.0;
	alpha[nfcns+2]=0.0;
	return 0;

}


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

 double gee(int k,int n)
{
    double p,xf,c,d1;
	p=0.0;
	xf=grid[k];
	xf=cos(pi2*xf);
	d1=0.0;
	for(int j=1;j<=n;j++)
	{
		c=xf-x[j];
		c=ad[j]/c;
		d1=d1+c;
		p=p+c*y[j];
	}
	return (p/d1);

}

/*void ouch(void)
{
 // printf("niter=%d",niter);
} */