EEPW论坛

void iirbcf(short ifilt,short band,short ns,short n,double f1,double f2,double f3,double f4,double db,double b[],double a[])
{    
	short k;
     double omega,lamda,epslon,fl,fh;
     double d[5],c[5];
	 if((band==1)||(band==4)) fl=f1;                                                                   
	 if((band==2)||(band==3)) fl=f2;
	 if(band<=3) fh=f3;
	 if(band==4) fh=f4;
 	 if(ifilt<3)
      { switch (band)
	       { case 1:
			 case 2:
			   { omega=warp(f2)/warp(f1);
			     break;
			  }
			case 3:
			   { omega=omin(bpsub(warp(f1),fh,fl),bpsub(warp(f4),fh,fl));
			   break;  
			   }
            case 4:
	{ omega=omin(1.0/bpsub(warp(f2),fh,fl),1.0/bpsub(warp(f3),fh,fl));} 
	     }
	lamda=pow(10.0,(db/20.0));
	epslon=lamda/cosh(2*ns*cosh1(omega));
	}
	for(k=0;k<ns;k++)
	   { switch(ifilt)
	         { case 1:
			     { chebyi(2*ns,k,4,epslon,d,c);
				   break;
				 }
			    case 2:
				   { chebyii(2*ns,k,4,omega,lamda,d,c);
				   break;
				    }
				case 3:
				   { bwtf(2*ns,k,4,d,c);
				   break;
				   }
			}
		   fblt(d,c,n,band,fl,fh,&b[k*(n+1)+0],&a[k*(n+1)+0]);
		  }
	 }

	  double cosh1(double x)

	 { double z;
	    z=log(x+sqrt(x*x-1.0));
		return(z);
	  }

double warp(double f)

	 { double pi,z;
        pi=4.0*atan(1.0);
		z=tan(pi*f);
		return(z);
	  }

    double bpsub(double om,double fh,double fl)

   { double z;
	   z=(om*om-warp(fh)*warp(fl))/(warp(fh)-warp(fl)*om);
	   return(z);
	 }


   double omin(double om1,double om2)

  { double z,z1,z2;
    z1=fabs(om1);
	z2=fabs(om2);
	z=(z1<z2) ? z1:z2;
	return(z);
}
 void bwtf(short ln,short k,short n,double d[],double c[])

{ short i;
	double pi,tmp;
	pi=4.0*atan(1.0);
	d[0]=1.0;
	c[0]=1.0;
	for(i=1;i<=n;i++)
	{
		d[i]=0.0;
		c[i]=0.0;
	}
	tmp=(k+1)-(ln+1.0)/2.0;
	if(tmp==0.0)
	{ c[1]=1.0;
	}
	else
	{
		c[1]=-2.0*cos((2*(k+1)+ln-1)*pi/(2*ln));
		c[2]=1.0;
	}
}

 void chebyi(short ln,short k,short n,double ep,double d[],double c[])
{ short i;
  double pi,gam,omega,sigma;
  pi=4.0*atan(1.0);
  gam=pow(((1.0+sqrt(1.0+ep*ep))/ep),1.0/ln);
  sigma=0.5*(1.0/gam-gam)*sin((2*(k+1)-1)*pi/(2*ln));
  omega=0.5*(1.0/gam+gam)*cos((2*(k+1)-1)*pi/(2*ln));
  for (i=0;i<=n;i++)
     { d[i]=0.0;
	   c[i]=0.0;
	 }
  if (((ln%2)==1)&&((k+1)==(ln+1)/2))
     { d[0]=-sigma; 
	   c[0]=d[0];
	   c[1]=1.0;
	}
else
   { c[0]=sigma*sigma+omega*omega;
     c[1]=-2.0*sigma;
	 c[2]=1.0;
	 d[0]=c[0];
	 if(((ln%2)==0)&&(k==0))
	    d[0]=d[0]/sqrt(1.0+ep*ep);
	}
}

 void chebyii(short ln,short k,short n,double ws,double att,double d[],double c[])
{ short i;
  double pi,gam,alpha,beta,sigma,omega,scln,scld;
  pi=4.0*atan(1.0);
  gam=pow((att+sqrt(att*att-1.0)),1.0/ln);
  alpha=0.5*(1.0/gam-gam)*sin((2*(k+1)-1)*pi/(2*ln));
  beta=0.5*(1.0/gam+gam)*cos((2*(k+1)-1)*pi/(2*ln));
  sigma=ws*alpha/(alpha*alpha+beta*beta);
  omega=-1.0*ws*beta/(alpha*alpha+beta*beta);
  for (i=0;i<=n;i++)
    { d[i]=0.0;
	  c[i]=0.0;
	}
  if(((ln%2)==1)&&((k+1)==(ln+1)/2))
    { d[0]=-1.0*sigma;
	  c[0]=d[0];
	  c[1]=1.0;
	}
 else
   { scln=sigma*sigma+omega*omega;
     scld=pow((ws/cos((2*(k+1)-1)*pi/(2*ln))),2);
	 d[0]=scln*scld;
	 d[2]=scln;
	 c[0]=d[0];
	 c[1]=-2.0*sigma*scld;
	 c[2]=scld;
   }
}
 void fblt(double d[],double c[],short n,short band,double fln,double fhn,double b[],double a[])

{
	short i,k,m,n1,n2,ls;
	double pi,w,w0,w1,w2,tmp,tmpd,tmpc,*work;
	pi=4.0*atan(1.0);
	w1=tan(pi*fln);
	for(i=n;i>=0;i--)
	{ if((c[i]!=0.0)||(d[i]!=0.0))
		break;
	}
	m=i;
	switch (band)
	{
	case 1:
	case 2:
		{ n2=m;
		n1=n2+1;
		if(band==2)
		{ for(i=0;i<=m/2;i++)
		{ tmp=d[i];
				d[i]=d[m-i];
				d[m-i]=tmp;
				tmp=c[i];
				c[i]=c[m-i];
				c[m-i]=tmp;
			}
		}
		for(i=0;i<=m;i++)
		{
			d[i]=d[i]/pow(w1,i);
			c[i]=c[i]/pow(w1,i);
		}
		break;
		}
	case 3:
	case 4:
		{	n2=2*m;
		n1=n2+1;
	work=malloc(n1*n1*sizeof(double));
		w2=tan(pi*fhn);
		w=w2-w1;
		w0=w1*w2;
		if(band==4)
		{ for(i=0;i<=m/2;i++)
		{ tmp=d[i];
				d[i]=d[m-i];
				d[m-i]=tmp;
				tmp=c[i];
				c[i]=c[m-i];
				c[m-i]=tmp;
			}
		}
		for(i=0;i<=n2;i++)
		{ work[0*n1+i]=0.0;
		  work[1*n1+i]=0.0;
		}
		for(i=0;i<=m;i++)
		{	tmpd=d[i]*pow(w,(m-i));
			tmpc=c[i]*pow(w,(m-i));
			for(k=0;k<=i;k++)
			{ ls=m+i-2*k;
		    	tmp=combin(i,i)/(combin(k,k)*combin(i-k,i-k));
				work[0*n1+ls]+=tmpd*pow(w0,k)*tmp;
				work[1*n1+ls]+=tmpc*pow(w0,k)*tmp;
			}
		}
		for(i=0;i<=n2;i++)
		{	d[i]=work[0*n1+i];
			c[i]=work[1*n1+i];
		}
	free(work);
	   }
   }
 bilinear(d,c,b,a,n);

 }
 double combin(short i1,short i2)

{ short i;
  double s;
  s=1.0;
  if(i2==0) return (s);
  for(i=i1;i>(i1-i2);i--)
  { s*=i; }
	return (s);
}

 
void bilinear(double d[],double c[],double b[],double a[],short n)
{	short i,j,n1;
	double sum,atmp,scale, *temp;
	n1=n+1;
	temp=malloc(n1*n1*sizeof(double));
	for(j=0;j<=n;j++)
	{
		temp[j*n1+0]=1.0;
	}		
	sum=1.0;
	for(i=1;i<=n;i++)
	{
		sum=sum*(double)(n-i+1)/(double)i;
		temp[0*n1+i]=sum;
	}
	for(i=1;i<=n;i++)
	for(j=1;j<=n;j++)
	{ temp[j*n1+i]=temp[(j-1)*n1+i]-temp[j*n1+i-1]-temp[(j-1)*n1+i-1];
		}
	for(i=n;i>=0;i--)
	{ b[i]=0.0;
	  atmp=0.0;
	  for(j=0;j<=n;j++)
	  { b[i]=b[i]+temp[j*n1+i]*d[j];
		atmp=atmp+temp[j*n1+i]*c[j];
		}
		scale=atmp;
		if(i!=0) a[i]=atmp;
	}
	for(i=0;i<=n;i++)
	{ b[i]=b[i]/scale;
	  a[i]=a[i]/scale;
	}
	a[0]=1.0;
	free(temp);
}