iir_filt.c.sta

来自「speech signal process tools」· STA 代码 · 共 1,535 行 · 第 1/3 页

/*********** Start the filter design processs ***********/

    /* 
    The basic design process is the following:

	A. Prewarp the critical frequencies to the analog frequency
	    domain - this is done by prewarp().

	B. Find find critical frequencies for analog low pass
	    prototype filter - this is done by low_pass_proto().

	C. 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.

	D. 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().

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

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

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

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

    */


/* prewarp frequencies */
    /*Convert digital frequencies (Hz.) to analog frequencies (radians)*/

    switch(filter_poly){
	case BUTTERWORTH:
	case CHEBYSHEV1:
	case CHEBYSHEV2:
	    for(i=0; i<num_of_3dBs; i++)
		warped_3dBs[i] = prewarp(2*PI*three_dBs[i], sf);
	    break;
	case ELLIPTICAL:
	    Fprintf(stderr, 
	       "iir_filt: Elliptical filters not supported yet - exiting\n");
	    exit(1);
/*	    for(i=0; i<num_of_band_edges; i++)
		warped_edges[i] = prewarp(2*PI*band_edges[i], sf);
	    break;*/
	default:
	    /*should never get here*/
	    Fprintf(stderr,"iir_filt: Invalid filt_method\n");
	    exit(1);
    }


/*DEBUG*/
    if (debug_level == 1 || debug_level > 9){
	Fprintf(stderr, "\n\t\tPREWARP FREQUENCIES\n");
	Fprintf(stderr, 
	"iir_filt: Convert from Digital to Analog Frequency Domain\n");
	if(filter_poly == ELLIPTICAL)
	    for(i=0;i<num_of_band_edges; i++)
		    Fprintf(stderr, 
    "iir_filt: critical freq[%d] = %f Hz, warped freq[%d] = %f radians\n",
		     i, band_edges[i], i, warped_edges[i]);
	else
	    for(i=0; i<num_of_3dBs; i++)
		Fprintf(stderr,
		"critical freq[%d] = %f Hz., warped freq[%d] = %f radians\n",
		    i, three_dBs[i], i, warped_3dBs[i]);
    }
/*END DEBUG*/


/* compute low pass prototype 3 dB frequency */

    low_pass_proto(warped_3dBs, warped_edges, filter_response, filter_poly);

/*DEBUG*/
    if(debug_level == 2 || debug_level >9){
	Fprintf(stderr, 
		"\n\niir_filt: ANALOG LOWPASS PROTOTYPE FREQUENCIES\n");
	if(filter_poly == ELLIPTICAL)
	    Fprintf(stderr,"pass freq = %g radian, stop freq = %g radian\n",
		warped_edges[0], warped_edges[1]);
	else
	    Fprintf(stderr,"pass freq = %f radian, stop freq = %f radian\n",
		warped_3dBs[0], warped_3dBs[1]);
    }
/*END DEBUG*/



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

    r_pole = (double *)calloc((unsigned)2*(filter_order)+4, sizeof(double));
    spsassert(r_pole != NULL,
			"Couldn't allocate space for real part poles\n");
    r_zero = (double *)calloc((unsigned)2*(filter_order)+4, sizeof(double));
    spsassert(r_zero != NULL,
			"Couldn't allocate space for real part zeros\n");
    i_pole = (double *)calloc((unsigned)2*(filter_order)+4, sizeof(double));
    spsassert(i_pole != NULL,
			"Couldn't allocate space for imag part poles\n");
    i_zero = (double *)calloc((unsigned)2*(filter_order)+4, sizeof(double));
    spsassert(i_zero != NULL,
			"Couldn't allocate space for imag part zeros\n");

/* find poles and zeros for prototype low pass filter */

    switch(filter_poly){
	case BUTTERWORTH:
	case CHEBYSHEV1:
	case CHEBYSHEV2:
poles_and_zeros(r_pole, r_zero, i_pole, i_zero, warped_3dBs,filter_order, pass_loss, stop_loss, filter_poly);
	    break;
	case ELLIPTICAL:
elliptical_p_and_z(r_pole, r_zero, i_pole, i_zero, warped_edges,filter_order, pass_loss, stop_loss);
	    break;
	default:
	    Fprintf(stderr, "iir_filt: Invalid filter polynomial type\n");
	    exit(1);
    }

/*DEBUG*/
    if(debug_level == 3 || debug_level >9){
	Fprintf(stderr, 
	    "\n\niir_filt: ANALOG LOW PASS PROTOTYPE POLES AND ZEROS\n\n");
	for(i=0; i<=(filter_order+1)/2; i++)
	    Fprintf(stderr, 
    "Index %d\treal_p = %lf, imag_p = %lf\n\treal_z = %lf, imag_z = %lg\n\n",
		i, r_pole[i], i_pole[i], r_zero[i], i_zero[i]);
    }
/*END DEBUG*/


/* transform low pass prototype to required response type*/

    switch(filter_poly){
	case ELLIPTICAL:
	/*first zeros */
   freq_xfrm(filter_response, r_zero, i_zero, warped_edges[3], filter_order);
	/* now poles*/
   freq_xfrm(filter_response, r_pole, i_pole, warped_edges[3], filter_order);
	    break;
	case BUTTERWORTH:
	case CHEBYSHEV1:
	case CHEBYSHEV2:
	/* first zeros */
    freq_xfrm(filter_response, r_zero, i_zero, warped_3dBs[2], filter_order);
	/* now poles*/
    freq_xfrm(filter_response, r_pole, i_pole, warped_3dBs[2], filter_order);
	    break;
	default:
	    Fprintf(stderr, "iir_filt: Invalid filter polynomial type\n");
	    exit(1);
    }

/*DEBUG*/
    if(debug_level == 4 || debug_level > 9){
	Fprintf(stderr, 
	    "\n\niir_filt: Desired Response type = %s\n", filt_resp_type);
	Fprintf(stderr, "Poles and Zeros for ANALOG Filter\n\n");
	for(i=0; i<=filter_order+1; i++)
	    Fprintf(stderr,
   "Index = %d, real_p = %lf, imag_p = %lf,\n\treal_z = %lf, imag_z = %lg\n",
		i, r_pole[i], i_pole[i], r_zero[i], i_zero[i]);
    }
/*END DEBUG*/


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

    /*first zeros*/
    blt(filter_order, filter_response, sf, r_zero, i_zero);

    /*now poles*/
    blt(filter_order, filter_response, sf, r_pole, i_pole);

/*DEBUG*/
    if(debug_level == 5 || debug_level >9){
	Fprintf(stderr, "\n\nPoles and ZEROS for DIGITAL FILTER\n\n");
	Fprintf(stderr, "\tFirst Poles:\n");
	for(i=0; i< 2*((filter_order+1)/2); i++)
	    Fprintf(stderr,
  "Index = %d:\treal_pole = %lf, imag_pole = %lf\n",i, r_pole[i], i_pole[i]);
	    Fprintf(stderr, "\nNow Zeros:\n");
	    for(i=0; i< 2*((filter_order+1)/2); i++)
		Fprintf(stderr,
  "Index = %d:\treal_zero = %lf, imag_zero = %lf\n",i, r_zero[i], i_zero[i]);
    }
/*END DEBUG*/


/* Convert poles and zeros into numerator and denominator polynomials*/

/* Allocate space for numerator and denominator coefficients*/

    num_coeff = (float *)calloc((unsigned)2*filter_order+4, sizeof(float));
    spsassert(num_coeff != NULL, "Couldn't allocate space for numerator");
    den_coeff = (float *)calloc((unsigned)2*filter_order+4, sizeof(float));
    spsassert(den_coeff != NULL, "Couldn't allocate space for denominator");


    nroots_num = (filter_order+1)/2;
    nroots_den = (filter_order+1)/2;
    if(filter_response == FILT_BP || filter_response == FILT_BS){
	nroots_num = filter_order;
	nroots_den = nroots_num;
    }

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

    (void)pz_to_filt_co(filter_response, filter_order, nroots_num, 
	    nroots_den, r_zero, i_zero, r_pole, i_pole, &num_den_co, 
	    den_coeff, &num_num_co, num_coeff);

/* set gain factor */

    (void)set_gain(filter_response, gain, num_num_co, num_coeff, num_den_co,
    den_coeff, &gain_factor, three_dBs, band_edges, sf, filter_poly);

/*DEBUG*/
    if(debug_level == 6 || debug_level >9){
	Fprintf(stderr, "\n\niir_filt: Gain factor = %f\n\n", 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] = %f\n", i, num_coeff[i]);
	Fprintf(stderr, "\nDenominator Coefficients\n");
	for(i=0; i<num_den_co; i++)
	    Fprintf(stderr, "\tden_coeff[%d] = %f\n", i, den_coeff[i]);
    }
/*END DEBUG*/

/* set key values in output header. */

    oh->common.tag = NO;
    oh->hd.filt->max_num = (short)(2*filter_order+4);
    oh->hd.filt->max_den = (short)(2*filter_order+4);
    oh->hd.filt->func_spec = (short)IIR;
    oh->hd.filt->type = filter_response;
    oh->hd.filt->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*/



/*
 * 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);
    switch(filter_poly){
	case BUTTERWORTH:
	    (void)add_genhd_f("3dB_freqs", three_dBs, (int)2, oh);
	    break;
	case CHEBYSHEV1:
	    (void)add_genhd_f("3dB_freqs", three_dBs, (int)2, oh);
	    (void)add_genhd_f("pass_band_loss", &pass_loss, 1, oh);
	    break;
	case CHEBYSHEV2:
	    (void)add_genhd_f("3dB_freqs", three_dBs, (int)2, oh);
	    (void)add_genhd_f("stop_band_loss", &stop_loss, 1, oh);
	    break;
	case ELLIPTICAL:
	    (void)add_genhd_f("3dB_freqs", band_edges, (int)4, oh);
	    (void)add_genhd_f("pass_band_loss", &pass_loss, 1, oh);
	    (void)add_genhd_f("stop_band_loss", &stop_loss, 1, oh);
	    break;
    }

/*
 * Warn if filter order is too high
*/
    if(order >= 14)
	Fprintf(stderr, 
"iir_filt: Filter design order probably too large to reliably design a\nfilter; please check filter design by plotting response.\n");

    write_header (oh, fpout);
    frec = allo_filt_rec (oh);

/* Write the data to the output file. */

    frec->filt_func.nsiz = num_num_co;
    frec->filt_func.dsiz = num_den_co;
    frec->filt_func.zeros = num_coeff;
    frec->filt_func.poles  = den_coeff;

    put_filt_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(three_dBs, b_edges, filter_response, filter_poly)
/* find critical low pass frequencies corresponding to filter specification*/
float *three_dBs;	    /*cut off frequencies*/
float *b_edges;		    /*band edges for elliptical filter*/
int filter_response;	    /* filter response type*/
int filter_poly;	    /* filter design polynomial*/
{
    float ctr;

    switch(filter_response){
	case FILT_LP:
	    /*low pass cutoff already defined*/
	    three_dBs[1] = three_dBs[0];
	    break;
	case FILT_HP:
	    /* replace S by 1/S */
	    if(filter_poly == ELLIPTICAL){
		b_edges[0] = 1./b_edges[0];
		b_edges[1] = 1./b_edges[1];
	    }
	    else{ /*BUTTERWORTH, CHEBYSHEV1, or CHEBYSHEV2*/
		three_dBs[0] = 1./three_dBs[0];	    
		three_dBs[1] = three_dBs[0];
	    }
	    break;
	case FILT_BP:
	    /* use Eqn 7.97 to get center freq (ctr) 
		and Eqn. 7.98 to get cutoff freq */
	    
	    if(filter_poly == ELLIPTICAL){
		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];
		b_edges[3] = (SQRD(ctr) - SQRD(b_edges[0]))/b_edges[0];
		if(b_edges[3] < b_edges[2])
			b_edges[2] = b_edges[3];
		/*shift all the values circularly around one place*/
		b_edges[0] = b_edges[1];
		b_edges[1] = b_edges[2];
		b_edges[2] = b_edges[3];
		b_edges[3] = ctr;
	    }
	    else{/* everything else*/
		ctr = (float)sqrt((double)(three_dBs[0] * three_dBs[1]));
             three_dBs[0] = (SQRD(three_dBs[1]) -ctr*ctr)/three_dBs[1];
		three_dBs[1] = three_dBs[0];
		three_dBs[2] = ctr;
	    }
	    break;
	case FILT_BS:
	    /* use Eqn. 7.97 to get center freq (ctr) and Eqn. 7.100
		to get the cutoff frequency */

	    if(filter_poly == ELLIPTICAL){
		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]));
		b_edges[2] = b_edges[2]/(SQRD(b_edges[2]) - ctr*ctr);
		b_edges[3] = ctr;
		if(b_edges[2] < b_edges[1])
		    b_edges[1] = b_edges[2];
	    }
	    else{/*do the rest*/
		ctr = sqrt(three_dBs[0]*three_dBs[1]);
		three_dBs[0] = three_dBs[1]/(SQRD(three_dBs[1]) - SQRD(ctr));
		three_dBs[1] = three_dBs[0];
		three_dBs[2] = ctr;
	    }
	    break;
	default:
	    Fprintf(stderr, "iir_filt: Invalid filter response type.\n");
	    exit(1);
    }
}

void
poles_and_zeros(r_p, r_z, i_p, i_z, cut_freq, order, p_loss, s_loss, type)
/* find poles and zeros corresponding to low pass response*/
double *r_p, *r_z, *i_p, *i_z;	/*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 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;
    else
	count = 0;

    switch(type){
	case BUTTERWORTH:
	    /* Use Eqn. 7.13 for poles; zeros are at frequency = infinity*/

	    for(i = 0; i < half_order; i++){
		arg = (PI/2.)*count/order;
		count += 2;
		r_z[i] = 0.;
		i_z[i] = FLT_MAX;
		r_p[i] = cut_freq[0]*-cos(arg);
		i_p[i] = cut_freq[0]*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;
	    epsilon = sqrt(pow((double)10., (.1*p_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;
		count += 2;
		r_z[i] = 0.;
		i_z[i] = FLT_MAX;
		r_p[i] = cut_freq[0]*sinhm*-cos(arg);
		i_p[i] = cut_freq[0]*coshm*sin(arg);
	    }
	    }
	    break;
	case CHEBYSHEV2:
	    {
	    double epsilon, v, sinhm, coshm;
	    epsilon = 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;
		count += 2;

		r_z[i] = 0.;
		if(count != 0)
		    i_z[i] = cut_freq[1]/sin(arg);
		else
		    i_z[i] = FLT_MAX;

		temp_r = sinhm*-cos(arg);
		temp_i = coshm*sin(arg);

		r_p[i] = cut_freq[1]*temp_r/(SQRD((temp_r)+SQRD(temp_i)));
		i_p[i] = cut_freq[1]*temp_i/(SQRD((temp_r)+SQRD(temp_i)));
	    }
	    }
	    break;
    }
}





void
elliptical_p_and_z(r_p, r_z, i_p, i_z, b_edges, order, p_loss, s_loss)
double *r_p, *r_z, *i_p, *i_z;	/*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*/
{
    return;
}


void
freq_xfrm(type, Real, Imag, cntr, order)
/* change low pass prototype to desired response type*/
int type;	    /* filter response type */
double Real[];	    /*real part of coefficients*/
double Imag[];	    /*imaginary part of coefficients*/
float cntr;	    /*warped geometric center frequency*/
int order;	    /*desired filter order */
{
    int i, upper_order;
    double_cplx a, b, c;


    upper_order = 2*((order-1)/2)+1;
    switch(type){
	case FILT_LP: