wavedlg_tmp.cpp

自己设计的新型FFT算法

ps_noise[0]=pn[0];
ps_noise[length/2]=pn[1];
for(i=2;i<129;i++)
ps_noise[i-1]=ps_noise[length+1-i]=pn[i];

for(i=0;i<256;i++)
frame_pn+=ps_noise[i];
//	for(i=0;i<129;i++)
//	{		
//		pmean[i]=pn[i];
//		pvar[i]=pn[i]*pn[i];
//	}

//	int im=0;
//	for(i=0;i<129;i++)
//	{
//		fsnr[i]=0;px[i]=0;
//		psnr[i]=1;
//		gain[i]=0.5;
//	}
//	double x;int nb[4000];
//	for(i=1;i<4000;i++)
//	{
//		x=13*atan(0.00076*i)+3.5*atan((i/7500)*(i/7500));
//		nb[i]=floor(x)+1;
//	}
	
	//
	//for(i=0;i<shift;i++)
	//	start++;
	pSpeech=start;

	for(k=0;k<FrameNum-1;k++)
	{
//		for(i=0;i<129;i++)
//			fsnr[i]=0.98*(gain[i]*px[i]/(pn[i]+1.0e-30))+(1-0.98)*max(psnr[i]-1,0);

	for(i=0;i<length;i++)					
		frame[i]=(double)(*pSpeech++);///nw;
//		frame[i]=(double)(*pSpeech++)/pow(2,16)/nw;
	
	for(i=0;i<shift;i++)
		start++;   
    pSpeech=start; 
//	temp1=frame_temp;
	rfft(length,frame,frame_temp,1);

	//
   
	pmix_signal[0]=(frame_temp[0]*frame_temp[0])/256.0;
	
	pmix_signal[length/2]=(frame_temp[1]*frame_temp[1])/256.0;                
	for(i=1;i<=length/2-1;i++)
	{
	//	pmix_signal[i]=(frame_temp[2*i]*frame_temp[2*i]+frame_temp[2*i+1]*frame_temp[2*i+1])/256.0;
		pmix_signal[i]=(pow(frame_temp[2*i],2)+pow(frame_temp[2*i+1],2))/256.0;
		pmix_signal[length-i]=pmix_signal[i];
	}
	for(i=0;i<=256;i++)
	{
    ps_temp[i]=pmix_signal[i];
    abs_signal[i]=pow(pmix_signal[i],0.5);
	}

	double c_n=0.995,c_s=0.999,c_beta=0.1;
	for(i=0;i<=256;i++)
	{
		if(k==0)
		{
			ps_noise[i]=c_n*ps_noise[i]+(1-c_n)*pmix_signal[i];
		    ps_signal[i]=(1-c_s)*pmix_signal[i];//-c_beta*ps_noise[i];
            ps_signal[i]=abs(ps_signal[i]);
			ps_last[i]=ps_signal[i];
		}
        else 
		{
			ps_noise[i]=ps_noise[i];
		    ps_signal[i]=c_s*ps_last[i]+(1-c_s)*pmix_signal[i]-c_beta*ps_noise[i];
			ps_signal[i]=abs(ps_signal[i]);
		    ps_last[i]=ps_signal[i];
		}
	}
    //维纳滤波
//	double aa=0.7,bb=2,h;
 //   for(i=0;i<256;i++)
 //   frame_ps+=ps_signal[i];
//	ps_final=frame_ps-0.8*frame_pn;
//	h=pow(ps_final/(ps_final+aa*frame_pn),bb);
 //   for(i=0;i<=256;i++)
//	    ps_signal[i]=ps_signal[i]*pow(h,2);


///////////////////////////////////////////////////////////////////
	int ich=18,index1=1,index2=0;//double lf2;
 //   double w=0;
	double b[22],sf[22],c[22],O[22],T[256],f[256],alfa[256],
		beita[256],a1[256],a2[256],a[256],e_ps_signal[256];//0~22
	double cc_temp[22][22];//double t1[22];
	double temp=0,uj=0,ua=0,sfm=0,gm=0,am=0,u=0,mm,temppow,tmax=0,tmin=0;

	for(i=0;i<22;i++)
	{
		b[i]=c[i]=O[i]=sf[i]=0;
	}
	for(i=0;i<256;i++)
	{
		T[i]=0;
	}


    for(i=0;i<=2;i++)
	    b[0]+=ps_signal[i];
	for(i=3;i<=5;i++)
		b[1]+=ps_signal[i];
	for(i=6;i<=9;i++)
		b[2]+=ps_signal[i];
	for(i=10;i<=12;i++)
		b[3]+=ps_signal[i];
	for(i=13;i<=15;i++)
		b[4]+=ps_signal[i];
	for(i=16;i<=19;i++)
		b[5]+=ps_signal[i];
	for(i=20;i<=24;i++)
		b[6]+=ps_signal[i];
	for(i=25;i<=28;i++)
		b[7]+=ps_signal[i];
	for(i=29;i<=34;i++)
		b[8]+=ps_signal[i];
	for(i=35;i<=40;i++)
		b[9]+=ps_signal[i];
	for(i=41;i<=46;i++)
		b[10]+=ps_signal[i];
	for(i=47;i<=54;i++)
		b[11]+=ps_signal[i];
	for(i=55;i<=63;i++)
		b[12]+=ps_signal[i];
	for(i=64;i<=73;i++)
		b[13]+=ps_signal[i];
	for(i=74;i<=85;i++)
		b[14]+=ps_signal[i];
	for(i=86;i<=100;i++)
		b[15]+=ps_signal[i];
	for(i=101;i<=117;i++)
		b[16]+=ps_signal[i];
	for(i=118;i<=140;i++)
		b[17]+=ps_signal[i];
	for(i=141;i<=169;i++)
		b[18]+=ps_signal[i];
	for(i=170;i<=204;i++)
		b[19]+=ps_signal[i];
	for(i=205;i<=245;i++)
		b[20]+=ps_signal[i];
	for(i=246;i<=255;i++)
		b[21]+=ps_signal[i];
		

	
    for(i=0;i<22;i++)
    sf[i]=15.81+7.5*((i+1)+0.474)-17.5*sqrt(1+((i+1)+0.474)*((i+1)+0.474));
    
	for(i=0;i<22;i++)
       for(j=0;j<22;j++)
        cc_temp[i][j]=b[j]*sf[i];
  
    
    for(i=0;i<22;i++)	
		for(j=0;j<22;j++)
			c[i]+=cc_temp[i][j];
        


	double temp_value=0.0;//temp_value in matlab
	for(i=0;i<22;i++)
		temp_value=temp_value+b[i];
	ua=temp_value/256.0;

	temp_value=0;
	for(i=0;i<length;i++)   
		temp_value=temp_value+log10(abs(ps_signal[i]));
	temp_value=temp_value/length;
	uj=pow(10,temp_value);
	sfm=-10*log10(uj/ua);

	u=min(sfm/(-60),1);
	for (i=0;i<=22;i++)
	{
      O[i]=u*(14.5+i)+(1-u)*5.5;
      T[i]=pow(10,log10(c[i])-O[i]/10);
	 // T[i]=(10*log10(T[i]*(b[i]/c[i]))+90.302)*fb[i];
	  c[i]=T[i];
	}

	
	for(i=0;i<=2;i++)
	    T[i]=c[0];
	for(i=3;i<=5;i++)
		T[i]=c[1];
	for(i=6;i<=9;i++)
		T[i]=c[2];
	for(i=10;i<=12;i++)
		T[i]=c[3];
	for(i=13;i<=15;i++)
		T[i]=c[4];
	for(i=16;i<=19;i++)
		T[6]=c[5];
	for(i=20;i<=24;i++)
		T[i]=c[6];
	for(i=25;i<=28;i++)
		T[i]=c[7];
	for(i=29;i<=34;i++)
		T[i]=c[8];
	for(i=35;i<=40;i++)
		T[i]=c[9];
	for(i=41;i<=46;i++)
		T[i]=c[10];
	for(i=47;i<=54;i++)
		T[i]=c[11];
	for(i=55;i<=63;i++)
		T[i]=c[12];
	for(i=64;i<=73;i++)
		T[i]=c[13];
	for(i=74;i<=85;i++)
		T[i]=c[14];
	for(i=86;i<=100;i++)
		T[i]=c[15];
      for(i=101;i<=117;i++)
		T[i]=c[16];
	for(i=118;i<=140;i++)
		T[i]=c[17];
	for(i=141;i<=169;i++)
		T[i]=c[18];
	for(i=170;i<=204;i++)
		T[i]=c[19];
       for(i=205;i<=245;i++)
		T[i]=c[20];
	for(i=246;i<=255;i++)
		T[i]=c[21];


	

	
	////计算绝对听阈////
     mm=0.0;
	for (i=0;i<length;i++)
	{
		f[i]=mm;
		mm=mm+(double)8/255;
	}
 
	f[0]=f[1];
	for(i=0;i<256;i++)
	{		
		f[i]=(3.64*pow(f[i],-0.8)-6.5*exp(-0.6*pow((f[i]-3.3),2))+0.001*pow(f[i],4));
	}									   
	
	for(i=0;i<256;i++)
	{
		T[i]=maxNum(T[i],f[i]);
//	
//		thv1[i]=pow(10,(T[i]-90.302)/10);
	}
//	for(i=0;i<256;i++)
//	{
//		if(T[i]>tmax)
//			tmax=T[i];
//		if(T[i]<tmin)
//			tmin=T[i];
//    }  
   

//////////////////////旧算法//////////////////////////
/*	for(i=0;i<256;i++)
	{
			alfa[i]=(tmax*6-T[i]*6+1*T[i]-1*tmin)/(tmax-tmin);
			beita[i]=(tmax*0.02-T[i]*0.02+0*T[i]-0*tmin)/(tmax-tmin);      
            temppow=ps_noise[i]/ps_temp[i];
            
			if(pow(temppow,2.0)<(1.0/(alfa[i]+beita[i])))
				//if(1-alfa[k]*pow(temppow,2)>0)
				frame1[i]=pow((1-alfa[i]*pow(temppow,2)),0.5)*ps_temp[i];
            else
				frame1[i]=pow((beita[i]*pow(temppow,2)),0.5)*ps_temp[i];
    }*/


	////////////////////////新算法//////////////////////////////
	for(i=0;i<256;i++)
	{
	a1[i]=pow((ps_signal[i]+ps_noise[i]),2)/(ps_signal[i])-(ps_signal[i]+ps_noise[i]);
    a2[i]=pow((ps_signal[i]+ps_noise[i]),2)/(T[i])-(ps_signal[i]+ps_noise[i]);
    if(a1[i]>a2[i])
         a[i]=a1[i];
    else a[i]=a2[i];
	}

double landa=5;      //  make up for estimate error of a_l & a_h  
double c_a=0.4;  
double above[256],under[256];          // coefficient of a_l & a_h
for(i=0;i<256;i++)
{
above[i]=(double)pow(pmix_signal[i],2);
under[i]=(double)(pmix_signal[i]+a[i]);
e_ps_signal[i]=above[i]/under[i];
//e_ps_signal[i]=(double)pow(pmix_signal[i],2)/(double)(pmix_signal[i]+a[i]);
}
/////////////////////////////////////////////////////////////////
    for(i=0;i<256;i++)
    frame1[i]=pow(e_ps_signal[i],0.5);

	frame_temp[0]=frame_temp[0]*frame1[0]/abs_signal[0];
	frame_temp[1]=frame_temp[1]*frame1[length/2]/abs_signal[length/2];
    for(i=2;i<length;i+=2)
	{
           frame_temp[i]=frame_temp[i]*frame1[i-1]/abs_signal[i-1];
	    frame_temp[i+1]=frame_temp[i+1]*frame1[i-1]/abs_signal[i-1];
	}

//反fft	
	rfft(length,frame_temp,frame,-1);
		if(k==0)
		{
			for(i=0;i<length;i++)
			frame_last[i]=frame[i];
		}
        else 
		{
			for(i=0;i<length/2;i++)
				frame[i]=(frame[i]+frame_last[128+i])/2.0;
			for(i=0;i<length;i++)
		    frame_last[i]=frame[i];
		}

    
    // double imax;
    //imax=frame[0];
     for(i=0;i<length/2;i++)
	 {if (frame[i]>imax)
      imax=frame[i];
	 }
	 for(i=0;i<length/2;i++)
		//*SpeechTmp1++=(short)(frame[i]);
		*SpeechTmp1++=(short)frame[i];//*pow(2,16)*nw);

	}
	//*SpeechTmp1=*SpeechTmp1/imax;
	pSpeech=temp;
	//*pSpeech=*SpeechTmp1;
}





int CWaveDlg::rfft(int dpts, double *yin, double *yout, int dir)
{
	 int i;
   double fact;

   if( FT_BACK == dir ) {
      fact = 2. / ((double) dpts) ;
      for( i = 0 ; i < dpts ; i++ )
         *(yout+i) = fact * *(yin+i);
      realft(yout-1, dpts/2, -1);           /* yout: unit offset in Num. Rec.*/
   } else {
      for( i = 0 ; i < dpts ; i++ )
         *(yout+i) = *(yin+i);
      realft(yout-1, dpts/2, 1);
   }

   return 1;

}

 

int CWaveDlg::rfft_mply(int dpts, double *x, double *y, double *z)
{
	int i;
   double real, imag;

   if( dpts >= 2 ) {                                /* these are not complex */
      *z     = *x     * *y     ;
      *(z+1) = *(x+1) * *(y+1) ;
   }

   for( i = 2 ; i < dpts ; i += 2 ) {                           /* other are */
      real     = *(x+i) * *(y+i)   - *(x+i+1) * *(y+i+1) ;
      imag     = *(x+i) * *(y+i+1) + *(x+i+1) * *(y+i)   ;
      *(z+i)   = real ;
      *(z+i+1) = imag ;
   }

   return 1;

}

int CWaveDlg::rfft_dvde(int dpts, double *x, double *y, double *z)
{
	int i;
   double real, imag, denom;

   if( dpts >= 2 ) {                                /* these are not complex */
      if( ( 0. == *y ) || ( 0. == *(y+1) ) )
         return EXIT_FAILURE;
      *z     = *x     / *y     ;
      *(z+1) = *(x+1) / *(y+1) ;
   }

   for( i = 2 ; i < dpts ; i += 2 ) {                          /* others are */
      denom    = *(y+i) * *(y+i) + *(y+i+1) * *(y+i+1) ;
      if( 0.0 == denom )
         return EXIT_FAILURE;                         /* avoid division by 0 */
      real     = *(x+i) * *(y+i) + *(x+i+1) * *(y+i+1) ;
      imag     = *(x+i+1) * *(y+i) - *(x+i) * *(y+i+1) ;
      *(z+i)   = real / denom;
      *(z+i+1) = imag / denom;
	   }

   return 1;

}

int CWaveDlg::rfft_corr(int dpts, double *x, double *y, double *z)
{
	int i;
   double real, imag;

   if( dpts >= 2 ) {                                /* these are not complex */
      *z     = *x     * *y     ;
      *(z+1) = *(x+1) * *(y+1) ;
   }

   for( i = 2 ; i < dpts ; i += 2 ) {                           /* other are */
      real     = *(x+i) * *(y+i) + *(x+i+1) * *(y+i+1) ;
      imag     = - *(x+i) * *(y+i+1) + *(x+i+1) * *(y+i)   ;
      *(z+i)   = real ;
      *(z+i+1) = imag ;
   }

   return 1;

}

void CWaveDlg::realft(double *data, int n, int isign)
{
	 int i,i1,i2,i3,i4,n2p3;
    double c1=0.5,c2,h1r,h1i,h2r,h2i;
    double wr,wi,wpr,wpi,wtemp,theta;

    theta=3.141592653589793/(double) n;
    if (isign == 1) {
        c2 = -0.5;
        four1(data,n,1);
    } else {
        c2=0.5;
        theta = -theta;
    }
    wtemp=sin(0.5*theta);
    wpr = -2.0*wtemp*wtemp;
    wpi=sin(theta);
    wr=1.0+wpr;
    wi=wpi;
    n2p3=2*n+3;
    for (i=2;i<=n/2;i++) {
        i4=1+(i3=n2p3-(i2=1+(i1=i+i-1)));
        h1r=c1*(data[i1]+data[i3]);
        h1i=c1*(data[i2]-data[i4]);
        h2r = -c2*(data[i2]+data[i4]);
        h2i=c2*(data[i1]-data[i3]);
        data[i1]=h1r+wr*h2r-wi*h2i;
        data[i2]=h1i+wr*h2i+wi*h2r;
        data[i3]=h1r-wr*h2r+wi*h2i;
        data[i4] = -h1i+wr*h2i+wi*h2r;
        wr=(wtemp=wr)*wpr-wi*wpi+wr;
        wi=wi*wpr+wtemp*wpi+wi;
    }
    if (isign == 1) {
        data[1] = (h1r=data[1])+data[2];
        data[2] = h1r-data[2];
    } else {
        data[1]=c1*((h1r=data[1])+data[2]);
        data[2]=c1*(h1r-data[2]);
        four1(data,n,-1);
    }

}

void CWaveDlg::four1(double *data, int nn, int isign)
{
	int n,mmax,m,j,istep,i;
    double wtemp,wr,wpr,wpi,wi,theta;
    double tempr,tempi;

    n=nn << 1;
    j=1;
    for (i=1;i<n;i+=2) {
        if (j > i) {
            SWAP(data[j],data[i]);
            SWAP(data[j+1],data[i+1]);
        }
        m=n >> 1;
        while (m >= 2 && j > m) {
            j -= m;
            m >>= 1;
        }
        j += m;
    }
    mmax=2;
    while (n > mmax) {
        istep=2*mmax;
        theta=6.28318530717959/(isign*mmax);
        wtemp=sin(0.5*theta);
        wpr = -2.0*wtemp*wtemp;
        wpi=sin(theta);
        wr=1.0;
        wi=0.0;
        for (m=1;m<mmax;m+=2) {
            for (i=m;i<=n;i+=istep) {
                j=i+mmax;
                tempr=wr*data[j]-wi*data[j+1];
                tempi=wr*data[j+1]+wi*data[j];
                data[j]=data[i]-tempr;
                data[j+1]=data[i+1]-tempi;
                data[i] += tempr;
                data[i+1] += tempi;
            }
            wr=(wtemp=wr)*wpr-wi*wpi+wr;
            wi=wi*wpr+wtemp*wpi+wi;
        }
        mmax=istep;
    }

}


double CWaveDlg::maxNum(double x, double y)
{
  double mx;
  if (x>y)
      mx=x;
  else mx=y;
  return (mx);
}

/*double CWaveDlg::maxNumOfMatrix(double *data)
{
  double imax;
  int i;
  imax=*data;
  for(i=0;i<length;i++)
  {if (*(data+i)>imax)
      imax=*(data+i);
  }
  return (imax);
}*/