iir_filt.c

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

/*
 * This material contains unpublished, proprietary software of 
 * Entropic Research Laboratory, Inc. Any reproduction, distribution, 
 * or publication of this work must be authorized in writing by Entropic 
 * Research Laboratory, Inc., and must bear the notice: 
 *
 *    "Copyright (c) 1986-1990  Entropic Speech, Inc. 
 *    "Copyright (c) 1990-1992  Entropic Research Laboratory, Inc. 
 *                   All rights reserved"
 *
 * The copyright notice above does not evidence any actual or intended 
 * publication of this source code.     
 *
 * Written by:  David Burton
 * Modified to FEAFILT and double precision by Bill Bryne
 * Corrected numerical problems, corrected numbers of output zerso, and
 *      added Chebyshev2, elliptical filters by Derek Lin, 12/3/92
 * Checked by:
 * Revised by:
 *
 * Brief description:This program designs a recursive filter.
 *                   Butterworth, Chebyshev1, Chevyshev2, and Elliptical
 *                   filters are supported at this time.
 */

static char *sccs_id = "@(#)iir_filt.c	1.18 01 Oct 1998 ERL";

# include <stdio.h>
# include <math.h>
# include <esps/esps.h>
# include <esps/feafilt.h>
# include <esps/unix.h>
# include <esps/constants.h>

# define SYNTAX	USAGE ("iir_filt [-P param_file][-x debug_level] filt_file");
#define ERROR_EXIT(text) {(void) fprintf(stderr, "%s: %s - exiting\n", \
					 argv[0], text); exit(1);}
# define SQRD(x) ((x)*(x))
# define ARCSINH(x) log((x)+sqrt(SQRD(x)+1))
# define ARCCOSH(x) log((x)+sqrt(SQRD(x)-1))
# define COSH(x) (0.5*(exp(x)+exp(-x)))
char *get_cmd_line();
double_cplx cdiv(), cmult(), cadd(), csub(), realmult(), csqrt();
int pz_to_co_d();

/*
 * Globals
*/
int debug_level = 0;

main (argc, argv)
     int argc;
     char *argv[];
{
  extern char *optarg;
  extern optind;
  char *Version = "2.0";
  char *Date = "12/03/92";
  FILE *fpout;		    /*output stream pointer*/
  struct header *oh;		    /*output file header pointer*/
  struct feafilt *frec;	    /*output data rescord structure*/
  char *param_file = NULL;        /*parameter file name*/
  char *filt_file = NULL;	    /*output file name*/
  filtdesparams *fdp;

  short order;		    /*holds final filter order*/
  int nroots_num, nroots_den;	    /*# roots in final filter*/

  double *num_coeff, *den_coeff;   /*holds designed coefficients*/ 
  double_cplx *poles, *zeros;
  double gain = 1.0;		    /*scaling gain*/
  float sf = 8000;		    /*sampling frequency*/
  char *filt_mthd = NULL;	    /*filter polynomial type*/
  int filter_poly = 0;	    /* integer representation of type*/
  char *filt_resp_type =  NULL;   /*filter response type*/
  int filter_response = 0;	     /* integer representation of type*/
  char *tmp = NULL;		    /* temporary character ptr*/

  int i, c;
  int filter_order = 0;	    /* filter polynomial order */
  float band_edges[4];	    /* holds band edges for ELLIPTICAL*/
  float warped_edges[4];	    /* holds prewarped edges for ELLIPTICAL*/
  int num_of_band_edges;	    /*number of band edge freqs*/
  float pass_loss;		    /*max. pass band loss*/
  float stop_loss;		    /*min. stop band loss*/
  short num_num_co, num_den_co;   /* number of filter coeffs*/
  double gain_factor;		    /*values used to scale numerator coeffs*/
  float val;
  
  float prewarp();
  void low_pass_proto();
  void poles_and_zeros();
  int elliptical_p_and_z();
  void freq_xfrm();
  void blt();
  void pz_to_numden();
  void set_gain();
  int determ_order();

/*
 * Check the command line options. 
 */
  while ((c = getopt (argc, argv, "P:x:")) != EOF)
    {
      switch (c)
	{
	case 'P':
	  param_file = optarg;
	  break;
	case 'x':
	  debug_level = atoi(optarg);
	  break;
	default:
	  SYNTAX
	  }
    }
  
  
  /* allocate space for filt_resp_type */
  filt_resp_type = (char *) calloc((unsigned)8, sizeof(char));

  /* initialize the band edge info arrays */
  for (i=0; i<4; i++) {
      band_edges[i] = warped_edges[i] = 0;
  }
  
/*
 * Get the output filter filename 
 */
  if (optind < argc)
    {
      filt_file = argv [optind];
      if (strcmp (filt_file, "-") == 0) 
	{
	  filt_file = "<stdout>";
	  fpout = stdout;
	}
      else TRYOPEN("iir_filt", filt_file, "w", fpout);
      if (debug_level) 
	Fprintf (stderr,"%s: Output file is %s\n", argv[0],filt_file);
    }
  else
    {
      Fprintf (stderr,"%s: No output file specified.\n", argv[0]);
      SYNTAX
	exit (1);
    }

/* 
 *Read parameter file and get design parameters 
 */
  
  if (debug_level)
    Fprintf (stderr,"%s: Reading parameter file %s\n", argv[0],param_file);
  
  if (read_params (param_file, SC_NOCOMMON, (char *)NULL) != 0)
    ERROR_EXIT("read_params could not read the params file.");
  
  if (symtype("filt_method") != ST_UNDEF)
    filt_mthd = getsym_s ("filt_method");
  else ERROR_EXIT("Filt_method not specified in params file.");
  if((filter_poly = lin_search(filt_method, filt_mthd)) == -1){
    Fprintf(stderr, "%s: Invalid filt_method (%s) specified - exiting.\n",
	    argv[0], filt_mthd);
    exit(1);
  }
  
  if (symtype("filt_type") != ST_UNDEF){
    tmp = getsym_s ("filt_type");
    /* add FILT_ prefix*/
    (void)strcat(filt_resp_type, "FILT_");
    (void)strcat(filt_resp_type, tmp);
  }
  else ERROR_EXIT("Filt_type not specified in params file.");
  if((filter_response = lin_search(filt_type, filt_resp_type)) == -1){
    Fprintf(stderr,"%s: Invalid filt_type (%s) specified - exiting.\n", 
	    argv[0], tmp);
    exit(1);
  }
  
  if (symtype("samp_freq") != ST_UNDEF) sf = (float)getsym_d ("samp_freq");
  else ERROR_EXIT("Sampling frequency not specified in params file.");
  if(sf <= 0.) ERROR_EXIT("Sampling Frequency must be > 0");
  
/*  
 * Get filter design parameters - 
 * 
 */
  if(filter_response == FILT_LP){
    num_of_band_edges = 2;
    if(symtype("p_freq1") != ST_UNDEF) band_edges[0] = getsym_d("p_freq1");
    else ERROR_EXIT("p_freq1 not specified.");
    if(symtype("s_freq1") != ST_UNDEF) band_edges[1] = getsym_d("s_freq1");
    else ERROR_EXIT("s_freq1 not specified.");
  }
  if(filter_response == FILT_HP){
    num_of_band_edges = 2;
    if(symtype("s_freq1") != ST_UNDEF) band_edges[0] = getsym_d("s_freq1");
    else ERROR_EXIT("s_freq1 not specified.");
    if(symtype("p_freq1") != ST_UNDEF) band_edges[1] = getsym_d("p_freq1");
    else ERROR_EXIT("p_freq1 not specified.");
  }
  if(filter_response == FILT_BP){
    num_of_band_edges = 4;
    if(symtype("s_freq1") != ST_UNDEF) band_edges[0] = getsym_d("s_freq1");
    else ERROR_EXIT("s_freq1 not specified.");
    if(symtype("p_freq1") != ST_UNDEF) band_edges[1] = getsym_d("p_freq1");
    else ERROR_EXIT("p_freq1 not specified.");
    if(symtype("p_freq2") != ST_UNDEF) band_edges[2] = getsym_d("p_freq2");
    else ERROR_EXIT("p_freq2 not specified.");
    if(symtype("s_freq2") != ST_UNDEF) band_edges[3] = getsym_d("s_freq2");
    else ERROR_EXIT("s_freq2 not specified.");
  }
  if(filter_response == FILT_BS){
    num_of_band_edges = 4;
    if(symtype("p_freq1") != ST_UNDEF) band_edges[0] = getsym_d("p_freq1");
    else ERROR_EXIT("p_freq1 not specified.");
    if(symtype("s_freq1") != ST_UNDEF) band_edges[1] = getsym_d("s_freq1");
    else ERROR_EXIT("s_freq1 not specified.");
    if(symtype("s_freq2") != ST_UNDEF) band_edges[2] = getsym_d("s_freq2");
    else ERROR_EXIT("s_freq2 not specified.");
    if(symtype("p_freq2") != ST_UNDEF) band_edges[3] = getsym_d("p_freq2");
    else ERROR_EXIT("p_freq2 not specified.");
  }
  for(val=0, i=0; i< num_of_band_edges; i++){
    if(band_edges[i] <= 0)
      ERROR_EXIT("critical frequencies must be >0");
    if(val >= band_edges[i])
      ERROR_EXIT("inconsistent bandedges specified for filter response type selected");
    val = band_edges[i];
  }      

  if(symtype("pass_band_loss") != ST_UNDEF)
    pass_loss = (float)getsym_d("pass_band_loss");
  else ERROR_EXIT("Pass_band_loss not specified.");
  if( pass_loss <= 0.) ERROR_EXIT("Pass band loss must be > 0.");	
  if(symtype("stop_band_loss") != ST_UNDEF)
    stop_loss = (float)getsym_d("stop_band_loss");
  else ERROR_EXIT("Stop_band_loss not specified.");
  if( stop_loss <= 0.) ERROR_EXIT("Stop band loss must be > 0.");

  if (symtype("gain") != ST_UNDEF) gain = (float)getsym_d ("gain");
  else ERROR_EXIT("Pass band gain not specified in params file.");
  if(gain <= 0) ERROR_EXIT("Gain must be > 0");

  if(debug_level > 0){
    Fprintf(stderr, "\n");
    Fprintf(stderr, "\t%s: LISTING OF SPECIFIED PARAMETERS\n\n",argv[0]);
    Fprintf(stderr, "\n\n%s: Samp. freq. = %f, pass band gain = %f\n",
	    argv[0],sf, gain);
    Fprintf(stderr, "%s: filt_method = %s, filt_type = %s\n",argv[0],
	    filt_mthd, filt_resp_type);
    Fprintf(stderr,"%s: pass band loss = %f dB,\nband edges = %f, %f, %f and %f Hz\n",argv[0], pass_loss, band_edges[0], band_edges[1], 
	    band_edges[2], band_edges[3]);
    Fprintf(stderr,"%s: stop band loss = %f dB\n", argv[0],stop_loss);
  }

/*********** Start the filter design processs ***********/
    /*  The basic design process is the following, labelled by A,B...F*/
      /*************************************************************
	A. Prewarp the critical frequencies to the analog frequency
	    domain - this is done by prewarp().

	B. Find critical frequencies for analog low pass
	    prototype filter - this is done by low_pass_proto().
	 
        C. Determine order of the filter.
      **************************************************************/

/* 
 * A. prewarp frequencies 
 */

    for(i=0; i<num_of_band_edges; i++)
      warped_edges[i] = prewarp(2*PI*band_edges[i], sf);

  if (debug_level == 1 || debug_level > 9){
    Fprintf(stderr, "\n\t\tPREWARP FREQUENCIES\n");
    Fprintf(stderr,"%s: Convert from Digital to Analog Frequency Domain\n",
	    argv[0]);
    for(i=0;i<num_of_band_edges; i++)
      Fprintf(stderr,
	      "critical freq[%d] = %f Hz, warped freq[%d] = %f radians\n",
	      i, band_edges[i], i, warped_edges[i]);
  }

/* 
 * B. compute low pass prototype critical frequency 
 */

  low_pass_proto(warped_edges, filter_response, filter_poly);

  if(debug_level == 2 || debug_level >9){
    Fprintf(stderr, "\n\n%s: ANALOG LOWPASS PROTOTYPE FREQUENCIES\n", argv[0]);
      Fprintf(stderr,"pass freq = %g radian, stop freq = %g radian\n",
	      warped_edges[0], warped_edges[1]);
  }

/*
 * C. determine the order of the filter, possible only after A, B 
 */

  filter_order = determ_order(filter_poly, pass_loss, stop_loss, warped_edges);

  if(debug_level){
    Fprintf(stderr,"%s: Lowpass prototype filter order computed by program: %d\n", argv[0], filter_order);
    if( filter_response == FILT_HP || filter_response == FILT_LP )
      Fprintf(stderr,"%s: The final filter order is: %d\n", 
	      argv[0], filter_order);
    else
      Fprintf(stderr,"%s: The final filter order is: %d\n", 
	      argv[0], 2*filter_order);
  }

  if(symtype("filt_order") != ST_UNDEF){
    if( filter_response == FILT_HP || filter_response == FILT_LP )
      Fprintf(stderr,"%s: The optimal filter order by the program is: %d\n", 
	      argv[0], filter_order);
    else
      Fprintf(stderr,"%s: The optimal filter order by the program is: %d\n", 
	      argv[0], 2*filter_order);
    Fprintf(stderr,"%s: The parameter filt_order in the parameter file is present.\n\tThe final filter order is set to it\n",argv[0]);
    filter_order = getsym_i("filt_order");
    if(filter_order <= 0) 
      ERROR_EXIT("filter_order in parameter file must be > 0");
    Fprintf(stderr,"%s: Lowpass prototype filter order is set by filt_order: %d \n", argv[0], filter_order);
    if(filter_response == FILT_BP || filter_response == FILT_BS){
      if(filter_order != 2 * (filter_order/2)){
	Fprintf(stderr,"%s: The parameter filt_order must be an even number for bandpass or bandstop filters", argv[0]);
	ERROR_EXIT(" ");
      }
      else 
	filter_order = filter_order/2;
    }
  }

     /*************************************************************
	D. Find the poles and zeros for the prototype low pass
	    filter - this is done by poles_and_zeros() for 
	    BUTTERWORTH, CHEBYSHEV1, and CHEBYSHEV2 filters and by
	    elliptical_p_and_z() for ELLIPTICAL fitlers.

	E. Find the poles and zeros of the analog version of the
	    desired filter type (LP, HP, BP, or BS) - this is done
	    by freq_xfrm().

	F. Find the corresponding poles and zeros in digital frequency
	    domain -  this is done by blt().

	G. Find the numerator and demoninator coeffs corressponding to
	    the poles and zeros - this is done by pz_to_co_d().

	H. Modify the numerator coefficients so that the gain equal
	    to the value specified - this is done by set_gain().

	F. Write out the appropriate generics and the data record.

     ***************************************************************/


/* 
 * Allocate space for pole and zero coefficients - real and imaginary parts
 */

    poles = (double_cplx *)calloc((unsigned)2*(filter_order), 
				  sizeof(double_cplx));
    spsassert(poles != NULL,"Couldn't allocate space for poles\n");
    zeros = (double_cplx *)calloc((unsigned)2*(filter_order), 
				 sizeof(double_cplx));
    spsassert(zeros != NULL,"Couldn't allocate space for zeros\n");

/* 
 * find poles and zeros for prototype low pass filter 
 */
  switch(filter_poly){
  case BUTTERWORTH:
  case CHEBYSHEV1:
  case CHEBYSHEV2:
    poles_and_zeros(poles, zeros, warped_edges,
		    filter_order, pass_loss, stop_loss, filter_poly, 
		    &nroots_den, &nroots_num );
    break;
  case ELLIPTICAL:
    elliptical_p_and_z(0, poles, zeros, warped_edges, filter_order, 
		       pass_loss, stop_loss, &nroots_den, &nroots_num);
    break;
  default:
    ERROR_EXIT("Invalid filter polynomial type");
  }

  if(debug_level == 3 || debug_level >9){
    Fprintf(stderr, "\n\n%s: ANALOG LOW PASS PROTOTYPE POLES AND ZEROS\n\n",
	    argv[0]);
    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\treal_p = %lf, imag_p = %lf\n\treal_z = %lf, imag_z = %lg\n\n",
	  i, poles[i].real, poles[i].imag, zeros[i].real, zeros[i].imag);
  }

/* 
 * transform low pass prototype to required response type 
 */
  /*first zeros */
  freq_xfrm(filter_order,filter_response, zeros, 
	    warped_edges[2], &nroots_num);
  /* now poles*/
  freq_xfrm(filter_order, filter_response, poles, 
	    warped_edges[2], &nroots_den);

  if(debug_level == 4 || debug_level > 9){