ex5_6.c

利用FFT变换

/* Program for IIR */

#include <math.h>
#include <stdlib.h>

#define pi 3.1415925

float fp,fr,fs;		/* 通带截止频率、阻带截止频率、抽样频率 */
float ap, ar;		/* 容限(dB) */
float ptr_a[50],ptr_b[50]; /* 系统函数H(z)的分母系数和分子系数 */
float hwdb[50];				/* 幅频响应值 */
float X[50],H[50],Y[50],Ydb[50];
float b_real[50],b_imag[50];
float b1_real[50],b1_imag[50],b2_real[50],b2_imag[50];
float d[50],e[50],g[50];
float f1[2],f2[2],h[50];


/* 函数定义 */

void bcg(float ap,float as,float wp,float ws,int *n,float *h)                     /*求H(s)分母系数*/
{
   int i,k;
   float a,p,wc,cs1,cs2;
   float c;

   c=(pow(10.0,0.1*as)-1.0)/(pow(10.0,0.1*ap)-1.0);
   *n=(int)(fabs(log10(c)/log10(ws/wp)/2.0)+0.99999);                                     /*求N*/
   wc=wp;
   a=pow(wc,(double)(*n));

   for(i=0;i<*n;i++)                           /*求极点*/
   {
      p=pi*(0.5+(2.0*i+1.0)/(2.0*(*n)));
      b_real[i]=wc*cos(p);
      b_imag[i]=wc*sin(p);
   }     

   b1_real[0]=-(b_real[0]);
   b1_imag[0]=-(b_imag[0]);
   b1_real[1]=1.0;
   b1_imag[1]=0.0;

   if(*n!=1)
   {
      for(i=1;i<*n;i++)
      {
         for(k=0;k<i;k++)
         {
            cs1=b1_real[k]-b1_real[k+1]*b_real[i];
            cs2=b1_imag[k]-b1_real[k+1]*b_imag[i];
            b2_real[k+1]=cs1+b1_imag[k+1]*b_imag[i];
            b2_imag[k+1]=cs2-b1_imag[k+1]*b_real[i];
         }

         b2_real[0]=-(b1_real[0]*b_real[i]-b1_imag[0]*b_imag[i]);
         b2_imag[0]=-(b1_real[0]*b_imag[i]+b1_imag[0]*b_real[i]);
         b2_real[i+1]=b1_real[i];
         b2_imag[i+1]=b1_imag[i];

         for(k=0;k<=i+1;k++)
         {
            b1_real[k]=b2_real[k];
            b1_imag[k]=b2_imag[k];
            b2_real[k]=0.0;
            b2_imag[k]=0.0;
         }
      }
   }

   for(i=0;i<=*n;i++)
      h[i]=b1_real[i]/a;
}

void pnpe(float *a,int m,int n,float *b,int *mn)                               
{	
	/*多项式相乘的展开系数*/
   
   int i,j,k,nk;
   float c[50];

   *mn=m*n;

   for(i=0;i<*mn+1;i++)
   {
      c[i]=0;
      b[i]=0;
   }

   if(n==0)
      b[0]=1.00;
   else
   {
      for(i=0;i<=m;i++)
         b[i]=a[i];
      if(n==1)
      {
         for(i=0;i<=m;i++)
            b[i]=a[i];
      }
      else
      {
         nk=m+1;
         for(i=1;i<n;i++)
         {
            for(j=0;j<=m;j++)
            {
               for(k=0;k<nk;k++)
                  c[k+j]+=a[j]*b[k];
            }

            nk+=m;

            for(k=0;k<nk;k++)
            {
               b[k]=c[k];
               c[k]=0;
            }
         }
      }
   }
}



void ypmp(float *a,int m,float *b,int n,float *c,int *mn)
{
   int i,j;

   *mn=m+n;

   for(i=0;i<*mn+1;i++)
      c[i]=0;

   for(i=0;i<=m;i++)
   {
      for(j=0;j<=n;j++)
         c[i+j]+=a[i]*b[j];
   }
}


void bsf(float *c,int ni,float *f1,float *f2,int nf,float *ptr_a,float *ptr_b,int *no)
{
   
   int i,j,nd,ne,ng;

   /*计算分子系数*/
   pnpe(f2,nf,ni,ptr_a,no);                                            

   for(i=0;i<*no+2;i++)
      ptr_b[i]=0;

   /*计算分母系数*/
   for(i=0;i<=ni;i++)                                                           
   {
      pnpe(f1,nf,i,d,&nd);
      pnpe(f2,nf,ni-i,e,&ne);
      ypmp(d,nd,e,ne,g,&ng);

      for(j=0;j<=ng;j++)
         ptr_b[j]+=c[i]*g[j];
   }
}


//*** 主函数 ***//
main()
{
   int N,nf,ns,nz,i,k;
   float hwdb1_real,hwdb1_imag;	/* 系统函数的分子部分 */
   float hwdb2_real,hwdb2_imag; /* 系统函数的分母部分 */
   float wp,ws,jw,amp1,amp2;

   N=50;

   /* 初始化数据空间 */
   for(i=0;i<50;i++)
   {
      b_real[i]=0;
      b_imag[i]=0;
      b1_real[i]=0;
      b1_imag[i]=0;
      b2_real[i]=0;
      b2_imag[i]=0;
      d[i]=0;
      e[i]=0;
      g[i]=0;
      ptr_a[i]=0;
      ptr_b[i]=0;
      hwdb[i]=0;
      X[i]=0;
      Y[i]=0;
      H[i]=0;
      Ydb[i]=0;
   }

	/* 滤波器设计参量 */
   fp=100;	/* Hz */
   fr=300;
   fs=1000;
   ap=3;	/* db */
   ar=20;
   wp=tan(pi*fp/fs);	/* 双线性变换 */
   ws=tan(pi*fr/fs);
   *f1=-1.0;
   *(f1+1)=1.0;
   *f2=1.0;
   *(f2+1)=1.0;
   nf=1;
   
   /* 求系统函数 H(s) */
   bcg(ap,ar,wp,ws,&ns,h);            
   
   /* 由H(s)转换成H(z) */
   bsf(h,ns,f1,f2,nf,ptr_b,ptr_a,&nz);           

   /* 用exp(jw)代替z */
   for(k=0;k<N;k++)
    {
      jw=k*pi/N;
      hwdb1_real=0;  hwdb1_imag=0;
      hwdb2_real=0;  hwdb2_imag=0;

      for(i=0;i<=nz;i++)
      {
		/* 分子 */      
         hwdb1_real+=ptr_b[i]*cos(i*jw);
         hwdb1_imag+=ptr_b[i]*sin(i*jw);
        /* 分母 */
         hwdb2_real+=ptr_a[i]*cos(i*jw);
         hwdb2_imag+=ptr_a[i]*sin(i*jw);
      }
      
      /* 求幅频响应 */
      amp1=sqrt(pow(hwdb1_real,2)+pow(hwdb1_imag,2));
      amp2=sqrt(pow(hwdb2_real,2)+pow(hwdb2_imag,2));
      H[k]=amp1/amp2;          
      hwdb[k]=10*log10(amp1/amp2);
      if(hwdb[k]<-200)  hwdb[k]=-200;
    }  
   for(i=0;i<N;i++)  
   {
      Y[i]=X[i]*H[i];
      Ydb[i]=10*log10(Y[i]);
   }
   
}