iir_butterworth_band.cpp

数字滤波器的iir的Butterworth带通滤波器

#include <stdio.h>
#include <math.h>

//N:the max times of the filter equation.
//M:the length of the filter.
#define pi 3.1415926
#define N 20
#define M 2000
#define Fs 2000

//Butterworth:the filter of Butterworth generator.
void Butterworth(double *butter_real,double *butter_imag,int n,double q2,double qbw);

//return_z:from p to z. (H(p)->H(z))
void return_z(double *z_real,double *z_imag,double q2,double qbw);

//complex_***:the base operation of th complex.
void complex_add(double * out_real,double * out_imag,double in_real,double in_imag);
void complex_sub(double * out_real,double * out_imag,double in_real,double in_imag);
void complex_mul(double * out_real,double * out_imag,double in_real,double in_imag);
void complex_div(double * out_real,double * out_imag,double in_real,double in_imag);
void complex_change1(double *out_ampl,double *out_phase,double in_real,double in_imag);
void complex_change2(double *out_real,double *out_imag,double in_ampl,double in_phase);

//return_z:get the times of filter equation.
int return_n(double as,double ap,double ls);

//digit_low_filter:the digit low pass filter generator.
//wp:the end frequence.
//ws:the bigin frequence of fobid band.
//as:the max decay of pass band.
//ap:the min decay of close band.  
void digit_band_filter(double * out_real,double * out_imag,double as,double ap,double w1,double w3,double wsl,double wsh);

void read_file(double *u,char *file_name,int *n);
void write_file(double *u,char *file_name,int n);

void Butterworth(double *butter_real,double *butter_imag,int n,double q2,double qbw)
{
	int i,j;
	double k;
	double z_real[M],z_imag[M];
	double p_real[N],p_imag[N];
	for (i=0;i<n;i++)
	{
		k=((2*(i+1)+n-1)*pi)/(2*n);
		p_real[i]=cos(k);
		p_imag[i]=sin(k);
	}
    return_z(z_real,z_imag,q2,qbw);
	for (i=0;i<M;i++)
	{
        butter_real[i]=1; 
        butter_imag[i]=0;
		for (j=0;j<n;j++)
		{
			double *temp1,*temp2,*temp3,*temp4,t1,t2,t3,t4;
			temp1=&t1;
			temp2=&t2;
			temp3=&t3;
			temp4=&t4;
			*temp1=1;
			*temp2=0;
            complex_div(temp1,temp2,(z_real[i]-p_real[j]),(z_imag[i]-p_imag[j]));
			*temp3=butter_real[i];
            *temp4=butter_imag[i];
			complex_mul(temp3,temp4,t1,t2);
            butter_real[i]=*temp3;
            butter_imag[i]=*temp4; 
		}
	}
}

void return_z(double *z_real,double *z_imag,double q2,double qbw)
{
	int i;
	for (i=0;i<M;i++)
	{
		double temp1_real,temp1_imag,*temp2_real,*temp2_imag,t1,t2;
		temp2_real=&t1;
        temp2_imag=&t2;
		temp1_real=cos(i*2*pi/Fs)-1;
        temp1_imag=sin(i*2*pi/Fs);
        *temp2_real=cos(i*2*pi/Fs)-1;
        *temp2_imag=sin(i*2*pi/Fs);
		complex_mul(temp2_real,temp2_imag,temp1_real,temp1_imag);
		z_real[i-1]=*temp2_real;
        z_imag[i-1]=*temp2_imag;
        temp1_real=(cos(i*2*pi/Fs)+1)*q2*q2;
        temp1_imag=sin(i*2*pi/Fs)*q2*q2;
        *temp2_real=cos(i*2*pi/Fs)+1;
        *temp2_imag=sin(i*2*pi/Fs);
		complex_mul(temp2_real,temp2_imag,temp1_real,temp1_imag);
		temp1_real=z_real[i-1];
		temp1_imag=z_imag[i-1];
		complex_add(temp2_real,temp2_imag,temp1_real,temp1_imag);
        z_real[i-1]=*temp2_real;
        z_imag[i-1]=*temp2_imag;
        temp1_real=(cos(i*2*pi/Fs)+1)*qbw;
        temp1_imag=sin(i*2*pi/Fs)*qbw;
        *temp2_real=cos(i*2*pi/Fs)-1;
        *temp2_imag=sin(i*2*pi/Fs);
		complex_mul(temp2_real,temp2_imag,temp1_real,temp1_imag);
		temp1_real=*temp2_real;
		temp1_imag=*temp2_imag;
		*temp2_real=z_real[i-1];
		*temp2_imag=z_imag[i-1];
        complex_div(temp2_real,temp2_imag,temp1_real,temp1_imag);
		z_real[i-1]=*temp2_real;
        z_imag[i-1]=*temp2_imag;
	}
}

void complex_add(double * out_real,double * out_imag,double in_real,double in_imag)
{
	*out_real=*out_real+in_real;
	*out_imag=*out_imag+in_imag;
}

void complex_sub(double * out_real,double * out_imag,double in_real,double in_imag)
{
	*out_real=*out_real-in_real;
	*out_imag=*out_imag-in_imag;
}

void complex_mul(double * out_real,double * out_imag,double in_real,double in_imag)
{
	double temp_real,temp_imag;
	temp_real=(*out_real)*in_real-(*out_imag)*in_imag;
	temp_imag=(*out_real)*in_imag+(*out_imag)*in_real;
	*out_real=temp_real;
	*out_imag=temp_imag;
}

void complex_div(double * out_real,double * out_imag,double in_real,double in_imag)
{
	double temp;
	complex_mul(out_real,out_imag,in_real,-in_imag);
	temp=in_real*in_real+in_imag*in_imag;
	*out_real=*out_real/temp;
	*out_imag=*out_imag/temp;
}

void complex_change1(double *out_ampl,double *out_phase,double in_real,double in_imag)
{
	*out_ampl=sqrt(in_real*in_real+in_imag*in_imag);
    *out_phase=atan(in_imag/in_real);
}

void complex_change2(double *out_real,double *out_imag,double in_ampl,double in_phase)
{
	*out_real=in_ampl*cos(in_phase);
    *out_imag=in_ampl*sin(in_phase);
}

int return_n(double as,double ap,double ls)
{
	int n;
	double temp1,temp2,temp3,temp4,temp5;
	temp1=pow(10,as/10)-1;
    temp2=pow(10,ap/10)-1;
	temp3=sqrt(temp1*temp2);
	temp4=log10(temp3);
	temp5=log10(ls);
    n=(int)(temp4/temp5+1);
	return n;
}

void digit_band_filter(double * out_real,double * out_imag,double as,double ap,double w1,double w3,double wsl,double wsh)
{
	double q1,q3,qsl,qsh,ls,lp,q2,qbw;
	double n1,n2,n3,nsl,nsh,nbw;
	double temp1,temp2;
	int n;
	q1=tan(w1/2);
	q3=tan(w3/2);
	qsl=tan(wsl/2);
	qsh=tan(wsh/2);
	q2=sqrt(q1*q3);
	qbw=q3-q1;
	n1=q1/qbw;
    n3=q3/qbw;
	n2=q2/qbw;
	nsl=qsl/qbw;
	nsh=qsh/qbw;
	nbw=qbw/qbw;
	lp=1;
	temp1=fabs((nsl*nsl-n2*n2)/nsl);
	temp2=fabs((nsh*nsh-n2*n2)/nsh);
	if ((temp1-temp2)>0.000001)
		ls=temp2;
	else
		ls=temp1;
	n=return_n(as,ap,ls);
    Butterworth(out_real,out_imag,n,q2,qbw);
}

void read_file(double *u,char *file_name,int *n)
{
	FILE *fp;
	int i;
    if ((fp=fopen(file_name,"r"))==NULL)
	{
		printf("cannot open file\n");
		return;
	}
	i=0;
	while (!feof(fp))
	{
		fscanf(fp,"%f",&u[i]);
		i=i+1;
	}
	*n=i-1;
	fclose(fp);
}

void write_file(double *u,char *file_name,int n)
{
	FILE *fp;
	int i;
    if ((fp=fopen(file_name,"w"))==NULL)
	{
		printf("cannot open file\n");
		return;
	}
    for (i=0;i<n;i++)
	{
		fprintf(fp,"%f ",u[i]);
	}
    fclose(fp);
}

void main()
{
	int i;
	double w1,w3,wsl,wsh;
	double as,ap;
	double out_real[M],out_imag[M];
	char filename[10];
    w1=2*300*pi/Fs;
	w3=2*400*pi/Fs;
    wsl=2*200*pi/Fs;
	wsh=2*500*pi/Fs;
	ap=3;
	as=18;
	digit_band_filter(out_real,out_imag,as,ap,w1,w3,wsl,wsh);
	for (i=0;i<M;i++)
	{
		double *out_ampl,*out_phase,ampl,phase;
		out_ampl=&ampl;
		out_phase=&phase;
		complex_change1(out_ampl,out_phase,out_real[i],out_imag[i]);
		out_real[i]=20*log10(*out_ampl);
	}
//	for (i=0;i<10;i++)
//	printf("\nreal=%f;imag=%f",out_real[i],out_imag[i]);
	printf("\nplease input the file name:");
	scanf("%s",filename);
	write_file(out_real,filename,M);
}