cnecho.cpp

回声抵消器时域算法.自适应滤波器算法的C++实现方法。

//	TITLE:  ECHO CANCELLER SIMULATION PROGRAM

#include<stdio.h>
#include<stdlib.h>
#include<iostream.h>
#include<iomanip.h>
#include<fstream.h>
#include<math.h>

#define ITHRESHOLD 16
#define IBATA1	   1024
#define CUTOFF    4
#define HANGT	  600

void start(int& s0,int y[],int sdc[],int& absy0,int& abss0);
void echoest(int a[],int y[],int s0,int& eest,int& u0,int& absu0);
void suppre(int absout, int absy,int hcntr,int u0,int& output);
void powerest(int absu0,int absy0,int& absout,
	int& absy);
void outpnorm(int un[],int iabsy,int u0);
void detect(int m[],int& abss0f,int abss0,int absy0,
	int& absyf,int h,int& hcntr);
void update(int a[],int un[],int y[],int h,int hcntr,int absy);
inline long mult(long a,long y);
inline int inverse(int absy);
void cancellecho(void);

int NTAP,M,GAIN;
char fname[40];

void main()
{
	cout<<"\n\n\t    ECHO CANCELLER SIMULATION PROGRAM\n\n";
	cout<<"  Description:\n";
	cout<<"	 1. The speech signal is simulated by white niose.\n";
	cout<<"	 2. The hybrid is simulated by a third order filter,\n";
	cout<<"	      with about 1.3ms delay,and 7dB attenuation.\n";
	cout<<"	 3. The far end talker is talking from begining to end,\n";
	cout<<"	      i.e. from sample 0 to 20000(2.5s).\n";
	cout<<"	 4. The near end talker is silent before sample 10000.\n";

	cout<<"\nEnter Number of Taps(128):";
	cin>>NTAP;
	cout<<"\nEnter M(16):";
	cin>>M;
	cout<<"\nEnter GAIN(1):";
	cin>>GAIN;
	cout<<"\nEnter output text file name:";
	cin>>fname;

	cancellecho();
}

void cancellecho()
{
	int   *a,*y,*un;
	a=new int[NTAP];
	y=new int[NTAP+M];
	un=new int[M];

	int  s0=0,sdc[2],u0=0,
	     absout=0,absy=0,m[9],abss0=0,abss0f=0,absyf=0,
	     absu0=0,absy0=0,output=0,eest=0;

	int  h=0,hcntr=0;
	long stat[15]={0,600,1200,1800,2400,3000,3600,4200,9000,
			10000,10010,10110,10710,19000,20000};

	int i=0,j=1;
	long i1=0;
	for(i=0;i<NTAP;i++) a[i]=0;
	for(i=0;i<NTAP+M;i++) y[i]=0;
	for(i=0;i<M;i++)  un[i]=0;
	for(i=0;i<9;i++)  m[i]=0;
	sdc[0]=sdc[1]=0;
	h=0;
	hcntr=HANGT;

	int echo=0,signal=0;
	float ey=0,es=0,eecho=0,enu0=0,enout=0,gain=0;

	fstream fout(fname,ios::out);
	FILE *pfyin,*pfein,*pfsin;
	pfyin=fopen("y20000.dat","rb");
	pfein=fopen("echo20k.dat","rb");
	pfsin=fopen("s20000.dat","rb");

	if(!pfyin || !pfein || !pfsin || !fout)
	{
	    cerr<<"\nFile Error.";
	    exit(0);
	}

	fout<<"\n\t\tTITLE: ECHO CANCELLER SIMULATION\n";
	fout<<"\n  Program Parameters:";
	fout<<"\n\tNTAP-->"<<NTAP<<"\n\tM-->"<<M<<"\n\tGAIN-->"<<GAIN;
	fout<<"\n\tBATA1-->2**(-10)\n\tCUTOFF-->4(-48dB)\n\tHANGT-->600(75ms)\n";

	fout<<"\n\t\tCONVERGENCE STATISTICS\n\n";
	fout<<"  Notations:\n";
	fout<<"	 Ey:    Energy of far end signal.\n";
	fout<<"	 Es:    Energy of near end signal.\n";
	fout<<"	 Eecho: Energy of echo(before cancelling).\n";
	fout<<"	 Enu0:  Energy of residual echo(after cancelling).\n";
	fout<<"	 Enout: Energy of output noise(after surpression)\n";
	fout<<"	 Sample Rate is 8kHZ,so a period of 600 samples is 75ms.\n";

	fout<<setprecision(2);

	fread(y,2,1,pfyin);
	fread(&echo,2,1,pfein);
	fread(&signal,2,1,pfsin);

	while(!feof(pfein) && !feof(pfyin) && !feof(pfsin))
	{
	   i1++;
	   sdc[0]=signal+echo;

	   start(s0,y,sdc,absy0,abss0);
	   echoest(a,y,s0,eest,u0,absu0);
	   powerest(absu0,absy0,absout,absy);
	   suppre(absout,absy,hcntr,u0,output);
	   outpnorm(un,absy,u0);
	   detect(m,abss0f,abss0,absy0,absyf,h,hcntr);
	   if(absy>CUTOFF) update(a,un,y,h,hcntr,absy);

	   h++;
	   if(h>M-1) h=0;

	   if(i1>=stat[j])
	   {
	      fout<<"\n  From Sample "<<stat[j-1]
			<<" To Sample "<<stat[j];
	      fout<<"\n\tEy="<<ey;
	      fout<<"   \tEs="<<es;
	      fout<<"\n\tEecho="<<eecho;
	      fout<<"\n\tEnu0= "<<enu0;
	      gain=10*log10(eecho/enu0);
	      fout<<"\t------->Eecho/Enu0= "<<gain<<"dB";
	      fout<<"\n\tEnout="<<enout;
	      if(enout+1!=1)
	      {
		 gain=10*log10(eecho/enout);
		 fout<<"\t------->Eecho/Enout="<<gain<<"dB\n";
	      }
	      else
		 fout<<"\t\t------->Eecho/Enout=Infinity\n";
	      ey=es=enu0=eecho=enout=0;
	      j++;
	   }


	   ey+=1.0*y[0]*y[0];
	   es+=1.0*signal*signal;
	   enu0+=1.0*(u0-signal)*(u0-signal);
	   eecho+=1.0*echo*echo;
	   enout+=1.0*(output-signal)*(output-signal);


	   fread(y,2,1,pfyin);
	   fread(&echo,2 ,1,pfein);
	   fread(&signal,2,1,pfsin);
	}

	fclose(pfyin);
	fclose(pfein);
	fclose(pfsin);
	fout.close();

	delete a;delete y;delete un;


}



// (1) Cycle Start Routine
// A-law or mu-law conversion is skipped.
void start(int& s0,int y[],int sdc[],int& absy0,int& abss0)
{
//	int i1;
//	i1=sdc[0]-sdc[1];
//	i1=(i1-i1/8)/2;
//	s0=s0-s0/8+i1;
// High pass filtering is omitted.

	if(y[0]>=0) absy0=y[0];
	else        absy0=-y[0];

	s0=sdc[0]; //***********************************************
	abss0=s0+s0;
	if(s0<0) abss0=-abss0;
	sdc[1]=sdc[0];
}




// (2) Echo Estimation
void echoest(int a[],int y[],int s0,int& eest,int& u0,int& absu0)
{
	long r=0;
	int k;
	for(k=0;k<NTAP;k++)
		r+=mult(a[k],y[k])/32768L;
	eest=(int)r;
	u0=s0-eest;
	absu0=u0;
	if(u0<0) absu0=-u0;
}


// (3) Residual Error Suppression.

void suppre(int absout, int absy,int hcntr,int u0,int& output)
{
	if(absout<=absy/ITHRESHOLD && hcntr==0) output=0;
	else output=u0;
}

// (4) Linear to mu-law Conversion.
// skipped.

// (5) Sigal and Output Power Estimation.
void powerest(int absu0,int absy0,int& absout,int& absy)
{
	long r;
	r=absout;
	absout=(int)((127*r+absu0)/128);
	r=absy;
	absy=(int)((absy0+127*r+CUTOFF)/128);
	if(absy<CUTOFF) absy=CUTOFF;
}

// (6) Output Normalization.
void outpnorm(int un[],int absy,int u0)
{
	int i;
	for(i=M-1;i>0;i--) un[i]=un[i-1];

	if(u0>absy) un[0]=32767;
	else if(u0<-absy) un[0]=-32767;
	else un[0]=inverse(absy)*u0;

}



// (7) Near-End Speech Detection.
//

void detect(int m[],int& abss0f,int abss0,int absy0,
	int& absyf,int h,int& hcntr)
{
	int i;
	abss0f=abss0f+(-abss0f+abss0)/32;
	absyf=absyf+(-absyf+absy0)/32;
	if(h==0)
	{
	   for(i=8;i>0;i--) m[i]=m[i-1];
	   m[0]=absyf;
	}
	else if(m[0]<absyf)
	   m[0]=absyf;
	for(i=0;i<9;i++)
	   if(abss0f<m[i]) {if(hcntr>0) hcntr--;return;}
	hcntr=HANGT;

}



// (8) Coefficient Updation.
//
void update(int a[],int un[],int y[],int h,int hcntr,int absy)
{
	int i,j;
	long r;
	if(hcntr==0)
	{
	   for(i=h;i<h+NTAP-M;i+=M)
	      {
		 r=0;
		 for(j=0;j<M;j++) r+=mult(un[j],y[i+j]);
		 a[i]+=(int)(r/absy/IBATA1)*GAIN;
	      }
	}
	for(i=NTAP+M-1;i>0;i--) y[i]=y[i-1];

}





// floating point multiplication.
// a is of the form Q.15,and y Q.0.
inline long mult(long a,long y)
{
	return a*y;
}

inline int inverse(int absy)
{
	return (int)(32768L/absy) ;
}