iir_filt.c

来自「speech signal process tools」· C语言 代码 · 共 1,232 行 · 第 1/3 页

    Fprintf(stderr, "\n\n%s: Desired Response type = %s\n", 
	    argv[0],filt_resp_type);
    Fprintf(stderr, "Poles and Zeros for ANALOG Filter\n\n");
    Fprintf(stderr,"pole_size %d, zero_size %d\n", nroots_den, nroots_num);
    for(i=0; i<2*filter_order; i++)
      Fprintf(stderr,  "Index = %d, real_p = %f, imag_p = %f,\n\treal_z = %f, imag_z = %f\n", i, poles[i].real, poles[i].imag, zeros[i].real, zeros[i].imag);
  }

/*
 * Warp the frequencies back to the digital domain 
 * This is done by using the digital bilinear transform
 */

  /*first zeros*/
  blt(filter_response, sf, zeros, nroots_num);
  
  /*now poles*/
  blt(filter_response, sf, poles, nroots_den);
  
  if(debug_level == 5 || debug_level >9){
    Fprintf(stderr, "\n\nPoles and ZEROS for DIGITAL FILTER\n\n");
    Fprintf(stderr,"pole_size %d, zero_size %d\n", nroots_den, nroots_num);
    Fprintf(stderr, "\tFirst Poles:\n");
    for(i=0; i< 2*filter_order; i++)
      Fprintf(stderr,"Index = %d:\treal_pole = %lf, imag_pole = %lf\n",
	      i, poles[i].real, poles[i].imag);
    Fprintf(stderr, "\nNow Zeros:\n");
    for(i=0; i< 2*filter_order; i++)
      Fprintf(stderr,"Index = %d:\treal_zero = %lf, imag_zero = %lf\n",
	      i, zeros[i].real, zeros[i].imag);
  }

/* 
 *Allocate space for numerator and denominator coefficients
 */
    num_coeff = (double *)calloc((unsigned)2*filter_order+1, sizeof(double));
    spsassert(num_coeff != NULL, "Couldn't allocate space for numerator");
    den_coeff = (double *)calloc((unsigned)2*filter_order+1, sizeof(double));
    spsassert(den_coeff != NULL, "Couldn't allocate space for denominator");

/* 
 * Convert from pole and zeros to filter cooefficients 
 */

    (void)pz_to_numden(nroots_den, poles, &num_den_co, den_coeff);
    (void)pz_to_numden(nroots_num, zeros, &num_num_co, num_coeff);

/*
 * Create output header 
 */
    oh = new_header (FT_FEA);

    fdp = (filtdesparams *) calloc( 1, sizeof(filtdesparams));
    spsassert(fdp!=NULL, "can't allocate memory for header params");

    oh->common.tag = NO;
    fdp->define_pz = YES; 
    fdp->func_spec = (short)IIR;
    fdp->type = filter_response;
    fdp->method = filter_poly;

    (void)strcpy (oh->common.prog, "iir_filt");
    (void)strcpy (oh->common.vers, Version);
    (void)strcpy (oh->common.progdate, Date);
    (void)add_comment(oh, get_cmd_line(argc, argv));/*store command line*/

    init_feafilt_hd( oh, (long)(2*filter_order+1), (long)(2*filter_order+1),
		    fdp);

/*
 * Add generic header items: samp_freq, notch_freq, and band_width
 */
  (void)add_genhd_f("samp_freq", &sf, 1, oh);
  order = filter_order;
  if(filter_response == FILT_BS || filter_response == FILT_BP)
    order = 2*order;
  (void)add_genhd_s("filt_order", &order, 1, oh);
  (void)add_genhd_f("pass_band_loss", &pass_loss, 1, oh);
  (void)add_genhd_f("stop_band_loss", &stop_loss, 1, oh);
  (void)add_genhd_f("band_edges", band_edges, (int)4, oh);
 
  write_header (oh, fpout);  

  
/* 
 * Allocate space for filter data record 
 */
    frec = allo_feafilt_rec (oh);

/* 
 * Write the data to the output file. 
 */
    *frec->num_size = num_num_co;
    for (i=0; i<num_num_co; i++)
      frec->re_num_coeff[i] = num_coeff[i];

    *frec->denom_size = num_den_co;
    for (i=0; i<num_den_co; i++)
      frec->re_denom_coeff[i] = den_coeff[i];

    *frec->zero_dim = nroots_num;
    for (i=0; i<nroots_num; i++)
      frec->zeros[i] = zeros[i];

    *frec->pole_dim = nroots_den;
    for (i=0; i<nroots_den; i++)
      frec->poles[i] = poles[i];

/* 
 * set gain factor 
 */

  (void)set_gain(filter_response, frec, gain, &gain_factor,
		 band_edges, sf, filter_poly);

  if(debug_level == 6 || debug_level >9){
    Fprintf(stderr, "\n\n%s: Gain factor = %e\n\n", argv[0], gain_factor);
    Fprintf(stderr, "\t\tNumerator and Denominator Coefficients\n\n");
    Fprintf(stderr, "Numerator Coefficients\n");
    for(i=0; i<num_num_co; i++)
      Fprintf(stderr, "\tnum_coeff[%d] = %e\n", i, frec->re_num_coeff[i]);
    Fprintf(stderr, "\nDenominator Coefficients\n");
    for(i=0; i<num_den_co; i++)
      Fprintf(stderr, "\tfrec->denom_coeff[%d] = %e\n", 
	      i, frec->re_denom_coeff[i]);
  }


 
/*
 * Warn if filter order is too high, write output, and exit
 */
  if(order >= 40 )
    Fprintf(stderr, "%s: Filter design order probably too large to reliably design a\nfilter; please check filter design by plotting response.\n",argv[0]);

  put_feafilt_rec (frec, oh, fpout);
  
  (void) fclose (fpout);
  exit(0);
  /*NOTREACHED*/
}

float
  prewarp(rad_freq, samp_freq)
/*assumes that frequencies are expressed in radians/sec*/
/*implements Eqn. 7.112 in DFD by Parks and Burrus*/

float rad_freq;
float samp_freq;
{
  return((float)(2.*samp_freq*tan((double)(rad_freq/(2*samp_freq)))));
}

void
  low_pass_proto(b_edges, filter_response, filter_poly)
/* find critical low pass frequencies corresponding to filter specification*/
float *b_edges;		    /*band edges for elliptical filter*/
int filter_response;	    /* filter response type*/
int filter_poly;	    /* filter design polynomial*/
{
  float ctr;
  float tmp;
  
  switch(filter_response){
	case FILT_LP:
    /*low pass cutoff already defined*/
    break;
  case FILT_HP:
    /* replace S by 1/S */
    tmp = b_edges[1];
    b_edges[1] = 1./b_edges[0];   /* lowpass proto pass */
    b_edges[0] = 1./tmp;   /* lowpass proto stop */
    break;
  case FILT_BP:
    /* use Eqn 7.97 to get center freq (ctr) 
       and Eqn. 7.98 to get cutoff freq */
    
    ctr = (float)sqrt((double)(b_edges[1]*b_edges[2]));
    b_edges[1] = (SQRD(b_edges[2]) - SQRD(ctr))/b_edges[2];
    b_edges[2] = (SQRD(b_edges[3]) - SQRD(ctr))/b_edges[3];
    if(b_edges[2] < 0) b_edges[2] = FLT_MAX;
    b_edges[3] = (SQRD(ctr) - SQRD(b_edges[0]))/b_edges[0];
    if(b_edges[3] < 0) b_edges[3] = FLT_MAX;
    
    if(b_edges[3] < b_edges[2])
      b_edges[2] = b_edges[3];

    b_edges[0] = b_edges[1];    /* lowpass proto pass band */
    b_edges[1] = b_edges[2];    /* lowpass proto stop band */
    b_edges[2] = ctr;           /* orig. band pass center */
    break;
  case FILT_BS:
    /* use Eqn. 7.97 to get center freq (ctr) and Eqn. 7.100
       to get the cutoff frequency */
    
    ctr = (float)sqrt((double)(b_edges[0]*b_edges[3]));
    b_edges[0] = b_edges[0]/(ctr*ctr - SQRD(b_edges[0]));
    b_edges[1] = b_edges[1]/(ctr*ctr - SQRD(b_edges[1]));
    if(b_edges[1] < 0) b_edges[1] = FLT_MAX;
    b_edges[2] = b_edges[2]/(SQRD(b_edges[2]) - ctr*ctr);
    if(b_edges[2] < 0) b_edges[2] = FLT_MAX;
    if(b_edges[2] < b_edges[1])
      b_edges[1] = b_edges[2];
    b_edges[2] = ctr;           /* orig. band pass center */
    break;
  default:
    Fprintf(stderr, "low_pass_pro: Invalid filter response type.\n");
    exit(1);
  }
}

int determ_order(filter_poly, pass_loss, stop_loss, freq)
int filter_poly;
float stop_loss, pass_loss;            /* assumed in dBs */
float *freq;
{
  float c1, c2;
  float epsilon, tmp, junk;
  int order;
  c1 = pow(10., -pass_loss * .05);
  c2 = pow(10., -stop_loss * .05);
  switch(filter_poly){
  case BUTTERWORTH:
    c1 = 1./SQRD(c1) - 1.;
    c2 = 1./SQRD(c2) - 1.;
    tmp =  log(c2/c1) / (2 * log(freq[1]/freq[0])); /*order */
    order =  1 + tmp;                             /*order */
    freq[3] = pow(10., -log10(c1)/(2*tmp)) * freq[0];    /* 3dB frequecny */
    return(order);
    break;
  case CHEBYSHEV1:
    epsilon = sqrt(pow(10., pass_loss*.1) - 1);
    tmp =  sqrt(1-c2*c2) / (epsilon * c2*c2);
    order = 1 + ARCCOSH(tmp)/ARCCOSH(freq[1]/freq[0]);
    return(order);
    break;
  case CHEBYSHEV2:
    epsilon = 1. / sqrt(pow(10., stop_loss*.1) - 1);
    tmp = c1 / ( epsilon * sqrt( 1-c1*c1 ));
    order = 1 + ARCCOSH(tmp)/ARCCOSH(freq[1]/freq[0]);
    return(order);
    break;
  case ELLIPTICAL:
    order = elliptical_p_and_z(1, NULL, NULL, freq, NULL,
			       pass_loss, stop_loss, NULL, NULL);
    return(order);
    break;
  default:
    Fprintf(stderr,"determ_order: Invalid filter polynomial type.\n");
    exit(1);
  }
}

void
poles_and_zeros( poles, zeros, cut_freq, order, p_loss, s_loss, type, np, nz)
/* find poles and zeros corresponding to low pass response*/
double_cplx *poles, *zeros;	/*real and imag poles and zeros*/
float *cut_freq;		/*cutoff frequency*/
int order;			/*desired filter order*/
float p_loss;			/* pass band loss*/
float s_loss;			/*stop band loss*/
int type;			/*filter polynomial type*/
int *np, *nz;                   /* size of poles and zeros, real is a single,
				   complex is as double */
{
    int count, i, half_order;
    double arg, temp_r, temp_i;

    half_order = (order+1)/2; /* take advantage of integer division*/

    if (2*half_order == order)
	count = 1;            /* order is even */
    else
	count = 0;            /* order is odd */

    switch(type){
    case BUTTERWORTH:
      /* Use Eqn. 7.13 for poles; zeros are at frequency = infinity*/
      *nz = order;
      for( i=0; i< *nz; i++ ){      /* treate real zero as a single */
	zeros[i].real = 0;
	zeros[i].imag = FLT_MAX;
      }
      *np = half_order;
      for(i = 0; i < *np; i++){
	arg = (PI/2.)*count/order;
	count += 2;
	poles[i].real = cut_freq[3]*-cos(arg);
	poles[i].imag = cut_freq[3]*sin(arg);
      }
      break;
    case CHEBYSHEV1:
      /* Use Eqn. 7.36 to calculate epsilon, eqn 7.29 to
	 calculate v, and 7.33 to calculate poles.
	 The zeros all occur at infinity. */
      {
	double epsilon, v, sinhm, coshm;
	*nz = order;
	for( i=0; i< *nz; i++ ){      /* treate real zero as a single */
	  zeros[i].real = 0;
	  zeros[i].imag = FLT_MAX;
	}
	epsilon = sqrt(pow((double)10., (.1*p_loss)) - 1);
	v = ARCSINH(1./epsilon)/order;
	sinhm = sinh(v);
	coshm = cosh(v);
	*np = half_order;
	for(i = 0; i < *np; i++){
	  arg = (PI/2.)*count/order;
	  count += 2;
	  poles[i].real = cut_freq[0]*sinhm*-cos(arg);
	  poles[i].imag = cut_freq[0]*coshm*sin(arg);
	}
      }
      break;
    case CHEBYSHEV2:
      {
	double epsilon, v, sinhm, coshm;
	*nz = *np = half_order;
	epsilon = 1.0 / sqrt(pow((double)10., (.1*s_loss)) - 1);
	v = ARCSINH(1./epsilon)/order;
	sinhm = sinh(v);
	coshm = cosh(v);
	for(i = 0; i < half_order; i++){
	  arg = (PI/2.)*count/order;
	  zeros[i].real = 0.;
	  if(count != 0)
	    zeros[i].imag = cut_freq[1]/sin(arg);
	  else	   /* odd order, get a real zero at inf. and a real pole */
	    zeros[i].imag = FLT_MAX;
	  count += 2;

	  temp_r = sinhm*-cos(arg);
	  temp_i = coshm*sin(arg);
	  poles[i].real = cut_freq[1] *temp_r/(SQRD(temp_r)+SQRD(temp_i));
	  poles[i].imag = cut_freq[1] *temp_i/(SQRD(temp_r)+SQRD(temp_i));
	}
      }
      break;
    }
}



int
elliptical_p_and_z(init, poles, zeros, b_edges, order, p_loss, s_loss, np, nz)
int init;                       /* a flag to find order */
double_cplx *poles, *zeros;	/*real and imag poles and zeros*/
float *b_edges;			/*pass and stop band frequencies*/
int order;			/*desired filter order*/
float p_loss;			/* pass band loss*/
float s_loss;			/*stop band loss*/
int *np, *nz;                   /* size of poles and zeros */
{
  static double epsilon, k, kc, K, KC, k1, k1c, K1, K1C;
  double v0, snc, sn, cnc, cn, dnc, dn;
  double cei(), arcsc(), fk();
  void elp();
  int half_order, count, i, j;
  
  if(init){
    epsilon = sqrt(pow((double)10., (.1*p_loss)) - 1);
    k = b_edges[0] / b_edges[1];             /* passband over stop band */
    kc = sqrt(1-k*k);
    k1 = epsilon / sqrt( pow((double)10., (.1*s_loss)) -1 );
    k1c = sqrt(1-k1*k1);
    K = cei(kc);
    KC = cei(k);
    K1 = cei(k1c);
    K1C = cei(k1);
    
    /* recalculate k1 */
    return( (int) K*K1C/(K1*KC) + 1.);
  }
  k1 = fk(order*KC/K);
  k1c = sqrt(1-k1*k1);
  K1 = cei(k1c);

  half_order = (order+1)/2; /* take advantage of integer division*/
  
  if (2*half_order == order)
    count = 1;            /* order is even */
  else
    count = 0;            /* order is odd */
  
  v0 = (K/(K1* order)) * arcsc(1/epsilon, k1);
  elp(v0, k, &snc, &cnc, &dnc);

  if(debug_level > 20)
    fprintf(stderr,"epsilon %f k %f kc %f K %f KC %f\n\tk1 %f k1c %f K1 %f K1C %f\n\tv0 %f snc %f cnc %f dnc %f\n\torder %d \n", epsilon, k, kc, K, KC, k1, k1c, K1, K1C, v0, snc, cnc, dnc, order);
  zeros[0].imag = FLT_MAX;

  for(i=0, j=0; i< half_order; i++, j++){
    elp(K*count/order, kc, &sn, &cn, &dn);
    if(debug_level >20 )
      fprintf(stderr,"sn %f cn %f dn %f\n", sn, cn, dn);
    zeros[j].real = 0;
    if( count != 0 ) zeros[j].imag = b_edges[1]/sn;
/*    else {
      zeros[++j].real = 0;
      zeros[j].imag = FLT_MAX;
    }*/
    poles[i].real = -b_edges[0] * snc*cnc*cn*dn/(1-dn*snc*dn*snc);
    poles[i].imag = b_edges[0] * dnc*sn/(1-dn*snc*dn*snc);
    count += 2;
  }
  *np = i;
  *nz = j;
}

double fk(u)