wavelet.cpp

信号消燥

// wavelet.cpp: implementation of the Cwavelet class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "study.h"
#include "wavelet.h"
#include "math.h"


#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

Cwavelet::Cwavelet()
{

}

Cwavelet::~Cwavelet()
{

}

void Cwavelet::wlagrang(int N)
{
    //此函数用于计算"Lagrange a trous"滤波器wlagrang_P;
	//结果返回N阶滤波器wlagrang_P以及按由小到大顺序排列的根real(实部)和imag(虚部)；
	//编写时间:2006.09.14

    int lon;
    lon=2*N-1;
    vector sup,a(N);
    sup.Cut(-N+1,N,2*N-1);
    a.Const(0);
    vector nok,const1(2*N-1),const2(2*N-1);
      for(int i=N+1;i<=2*N;i++)
	  {
	     nok=sup;
         nok.Del(i);
         const1.Const(0.5);
         const2.Const(double(i-N));
         const1-=nok;
         const2-=nok;
         a[i-N]=prod(const1)/prod(const2);
	  }
	nok.Destroy();  //提前销毁中间变量;
	const1.Destroy();
	const2.Destroy();

    wlagrang_P.Set(lon);
    wlagrang_P.Zero();
      for(i=1;i<=N;i++)
         wlagrang_P[2*i-1]=a[i];

    vector wlagrang_P_invers=wlagrang_P;
    vector wlagrang_P_1=wlagrang_P;
    wlagrang_P_invers.Invers();
    wlagrang_P_invers.Add(1.0);
    wlagrang_P.Link(wlagrang_P_invers,wlagrang_P_1);
	wlagrang_P_invers.Destroy();
    wlagrang_P_1.Destroy();
    int n=wlagrang_P.Size()-1;
    vector real_temp(n),imag_temp(n);
    RootQR(wlagrang_P,real_temp,imag_temp);

    vector real_temp_1=real_temp;
    real_temp_1+=1.0;
    int real_temp_Size=real_temp.Size();
    vector abs(real_temp_Size);

      for(i=1;i<=real_temp_Size;i++)
         abs[i]=sqrt(real_temp_1[i]*real_temp_1[i]+imag_temp[i]*imag_temp[i]);
	real_temp_1.Destroy();
      int k;
      for(i=1;i<=lon+1;i++)
	  {
	     abs.Min(k);
         real_temp.Del(k);
         imag_temp.Del(k);
         abs.Del(k);
	  }
    real_temp_Size=real_temp.Size();
      for(i=1;i<=real_temp_Size;i++)
         abs[i]=sqrt(real_temp[i]*real_temp[i]+imag_temp[i]*imag_temp[i]);

    real.Set(lon-1);
    imag.Set(lon-1);
	real=real_temp;
    imag=imag_temp;

}

double Cwavelet::prod(vector X)
{ 
	//计算向量内部元素的乘积;
	//编写时间:2006.09.14
	double prod_out=1.0;
      for(int i=1;i<=X.Size();i++)
         prod_out*=X[i];

    return prod_out;
}

void Cwavelet::dbaux(int N)
{
	//此函数用于计算daubechies尺度滤波器系数db_w;
	//N为尺度滤波器的阶数,N=1,2,3...;
	//注意:当N的取值过大时,可能出现不稳定的情况,一般取10左右;
	//编写时间:2006.09.14
    wlagrang(N);
    int real_Size=real.Size();
	vector abs(real_Size);
	vector real_temp=real;
	vector imag_temp=imag;
	  for(int i=1;i<=real_Size;i++)
         abs[i]=sqrt(real_temp[i]*real_temp[i]+imag_temp[i]*imag_temp[i]);
      for(i=real_Size;i>=1;i--)
	  {  
		  if(abs[i]>=1.0)
		  {real_temp.Del(i);
		   imag_temp.Del(i);
		  }
	  }
	  for(i=1;i<=N;i++)
	  {	  real_temp.Add(-1.0);
	      imag_temp.Add(0.0);
	  }
	  vector ww;
	 //Poly(real_temp,imag_temp,ww);   //为自己所编写的求系数函数,作为Cwavelet的成员函数；
      ww.PolyCoef(real_temp,imag_temp);//此函数与上边函数功能相同，为数值库自带函数，精度与上函数相当；
	  double sum_;
	  sum_=sum(ww);
	  db_w.Mult(ww,1.0/sum_);
}

int Cwavelet::HessenbergQR(matrix a,vector & real_,vector & imag_)
{ //此函数用带原点位移的双重步QR方法计算实上H矩阵(赫申伯格矩阵)的全部特征值；
	//编写时间:2006.09.18
	int i, j, k, l, it(0), stRank;
    double b,c,w,g,xy,p,q,r,x,s,e,f,z,y;
    double eps=1.0e-15;
	int jt=80;
	stRank = a.LineSize();		// 矩阵阶数

    while(stRank!=0)
    { 
		l=stRank-1;
        while((l>0)&&(fabs(a(l+1,l))>eps*(fabs(a(l,l))+fabs(a(l+1,l+1))))) l=l-1;
        if(l==stRank-1)
        { 
			real_[stRank]=a(stRank,stRank);
            imag_[stRank]=0.0;
            stRank=stRank-1;
			it=0;
        }
        else if(l==stRank-2)
        { 
			b=-(a(stRank,stRank)+a(stRank-1,stRank-1));
            c=a(stRank,stRank)*a(stRank-1,stRank-1)-a(stRank,stRank-1)*a(stRank-1,stRank);
            w=b*b-4.0*c;
            y=sqrt(fabs(w));
            if(w>0.0)
            { 
				xy=1.0;
                if(b<0.0) xy=-1.0;
				real_[stRank]=(-b-xy*y)/2.0;
                imag_[stRank]=0.0;
			    real_[stRank-1]=c/real_[stRank];
                imag_[stRank-1]=0.0;
				
            }
            else
            {
				real_[stRank]=-b/2.0;
				imag_[stRank]=y/2.0;
				real_[stRank-1]=real_[stRank];
				imag_[stRank-1]=-imag_[stRank];
            }
            stRank=stRank-2; 
			it=0;
        }
        else
        { 
			if(it>=jt)
            { //printf("fail\n");
				return(-1);		//出错！
            }
            it=it+1;
            for(j=l+2; j<stRank; j++)	a(j+1,j-1)=0.0;
            for(j=l+3; j<stRank; j++)    a(j+1,j-2)=0.0;
            for(k=l; k<stRank-1; k++)
            { 
				if(k!=l)
                {
					p=a(k+1,k);
					q=a(k+2,k);
                    r=0.0;
                    if(k!=stRank-2) r=a(k+3,k);
                }
                else
                {
					x=a(stRank,stRank)+a(stRank-1,stRank-1);
                    y=a(stRank-1,stRank-1)*a(stRank,stRank)-a(stRank-1,stRank)*a(stRank,stRank-1);
                    p=a(l+1,l+1)*(a(l+1,l+1)-x)+a(l+1,l+2)*a(l+2,l+1)+y;
                    q=a(l+2,l+1)*(a(l+1,l+1)+a(l+2,l+2)-x);
                    r=a(l+2,l+1)*a(l+3,l+2);
                }
                if((fabs(p)+fabs(q)+fabs(r))!=0.0)
                { 
					xy=1.0;
                    if(p<0.0) xy=-1.0;
                    s=xy*sqrt(p*p+q*q+r*r);
                    if(k!=l) a(k+1,k)=-s;
                    e=-q/s;
					f=-r/s; 
					x=-p/s;
                    y=-x-f*r/(p+s);
                    g=e*r/(p+s);
                    z=-x-e*q/(p+s);
                    for(j=k; j<stRank; j++)
                    {	
                        p=x*a(k+1,j+1)+e*a(k+2,j+1);
                        q=e*a(k+1,j+1)+y*a(k+2,j+1);
                        r=f*a(k+1,j+1)+g*a(k+2,j+1);
                        if(k!=stRank-2)
                        { 
                            p=p+f*a(k+3,j+1);
                            q=q+g*a(k+3,j+1);
                            r=r+z*a(k+3,j+1);
							a(k+3,j+1)=r;
                        }
                        a(k+2,j+1)=q; 
						a(k+1,j+1)=p;
                    }
                    j=k+3;
                    if(j>=stRank-1) j=stRank-1;
                    for(i=l; i<=j; i++)
                    {	
                        p=x*a(i+1,k+1)+e*a(i+1,k+2);
                        q=e*a(i+1,k+1)+y*a(i+1,k+2);
                        r=f*a(i+1,k+1)+g*a(i+1,k+2);
                        if(k!=stRank-2)
                        { 
                            p=p+f*a(i+1,k+3);
                            q=q+g*a(i+1,k+3);
                            r=r+z*a(i+1,k+3); 
							a(i+1,k+3)=r;
                        }
                        a(i+1,k+2)=q; 
						a(i+1,k+1)=p;
                    }
                }
            }
        }
    }
    return(1);
}

int Cwavelet::RootQR(vector aa, vector  & real_1, vector & imag_1)
{
    //求多项式的根,精度与matlab相当;
	//编写时间:2006.09.18
	int n=aa.Size()-1;
	matrix q(n,n);
	for(int i=1;i<=n;i++)
		q(1,i)=-aa[i+1]/aa[1];
	for(i=1;i<=n-1;i++)
		q(i+1,i)=1.0;
   if(HessenbergQR(q, real_1,  imag_1))
   {return 1;}
   else
   {return -1;
   }
}

void Cwavelet::Poly(vector real11, vector imag11, vector &yy)
{
	//已知多项式的所有根(包括复根),求多项式系数;
	//编写时间:2006.09.19
   int n=real11.Size(),i,j,k;
   yy.Set(n+1);
   complex<double> *e=new complex<double>[n];
   for (i=0;i<=n-1;i++) 
   {
	e[i]=complex<double>(real11[i+1],imag11[i+1]);
   }

   complex<double> *c=new complex<double>[n+1];
   c[0]=complex<double>(1.0,0.0);
   for (i=1;i<=n;i++) 
   {
	   c[i]=complex<double>(0.0,0.0);
   }
   complex<double> *c_temp=new complex<double>[n];
   for (j=1;j<=n;j++)
   {
	   
	   for (i=1;i<=j;i++)
	   {
		   c_temp[i-1]=e[j-1]*c[i-1];
	   }

	   for (k=1;k<=j;k++)
	   {
		   
          c[k]=c[k]-c_temp[k-1];
	   }
	   
   }
      for (i=0;i<=n;i++)
	   {
		   yy[i+1]=c[i].real();
	   }
   delete[] c_temp;  
   delete[] c;
   delete[] e;
   
}

double Cwavelet::sum(vector xx)
{
	//此函数用于计算向量的和；
	//编写时间：2006.09.19
	int n=xx.Size();
    double summ;
    summ=0.0;
    for (int i=1;i<=n;i++)
	{summ+=xx[i];
	}
    return summ;

}

vector Cwavelet::qmf(vector qmf, int p=0)
{
	//镜像二次滤波器(Quadrature Mirror Filters,简称QMF);
	//当p为偶数时,函数改变向量qmf中偶数位置的元素符号;
	//当p为奇数时,函数改变向量qmf中奇数位置的元素符号;
	//默认情况下P=0;
	//编写时间:2006.09.19
	qmf_y=qmf;
	qmf_y.Invers();
	int first=2-p%2;
	for (int i=first;i<=qmf_y.Size();i+=2)
	{qmf_y[i]=-qmf_y[i];
	}
     return qmf_y;
}

void Cwavelet::orthfilt(vector orth)
{
	//正交小波滤波器组
    //Hi_R;  重构的高通滤波器;
    //Lo_R;  重构的低通滤波器;
    //Hi_D;  分解的高通滤波器;
    //Lo_D;  分解的低通滤波器;
	//编写时间:2006.09.19
 orth.Mult(orth,1.0/sum(orth));

 Lo_R.Mult(orth,sqrt(2));
 Hi_R = qmf(Lo_R,0);
 Hi_D=Hi_R;
 Hi_D.Invers();
 Lo_D=Lo_R;
 Lo_D.Invers();
}

void Cwavelet::dyaddown(vector X, vector & Y, int evenodd=0)
{ 
	//二元取样；
	//从向量X中每隔一个元素抽取一个元素组成向量Y；
	//如果evenodd为偶数，进行偶取样，否则进行奇取样；
	//编写时间:2006.09.20
	int n=X.Size(),i;
	if (evenodd%2==0)
	{ 
		if (n==1) 
		{
			AfxMessageBox("只有一个元素，无法返回偶数序列！");
		}
		else
		{
			Y.Set(n/2);
	       for (i=1;i<=n/2;i++)
		   {
		   Y[i]=X[2*i];
		   }
		}
	}
	else
	{
		if (n%2==0)
		{
			Y.Set(n/2);
			for (i=1;i<=n/2;i++)
			{
		       Y[i]=X[2*i-1];
			}
		}
		else
		{
			Y.Set(n/2+1);
			for (i=1;i<=n/2+1;i++)
			{
		       Y[i]=X[2*i-1];
			}
		}
	}
}

void Cwavelet::dyadup(vector X, vector & Y, int evenodd,int arg)
{
	//二元插值；
	//从向量X中每隔一个元素填充一个0元素，组成向量Y；
    //如果evenodd为偶数，进行偶插值，否则进行奇插值；
	/*举例:
	 X=[1 2 3];
     DYADUP(X,Y,0,0) ==> [1 0 2 0 3];
	 DYADUP(X,Y,0,1) ==> [1 0 2 0 3 0];
	 DYADUP(X,Y,1,0) ==> [0 1 0 2 0 3 0];
	 DYADUP(X,Y,1,1) ==> [0 1 0 2 0 3];
	*/
	//编写时间:2006.09.20
	int n=X.Size(),i;
	if (evenodd%2==0)
	{
		if (arg==0)
		{
			Y.Set(2*n-1);
		}
		else
		{
			Y.Set(2*n);
		}
		
		for (i=1;i<=n;i++)
		{
			Y[2*i-1]=X[i];
		}
	}
	else
	{
		if (arg==0)
		{
			Y.Set(2*n+1);
		}
		else
		{
			Y.Set(2*n);
		}
		
        for (i=1;i<=n;i++)
		{
			Y[2*i]=X[i];
		}
	}
	
}

void Cwavelet::wextend(vector X, vector & Y, CString mode, int l, char loc)
{
	//扩展向量，将向量X扩展以后存入Y中；
	//扩展模式mode取值如下:
	//"zpd"表示零扩展；"sym"表示对称扩展；"ppd"表示周期扩展(1)；
	//"per"表示周期扩展(2),如果信号长度为奇数，wextend添加一个与右边末尾值相等的额外采样；
	//l表示需要扩展的长度；
	//loc表示扩展的方向，其中'b'表示双向扩展，'l'表示向左扩展，'r'表示向右扩展；
	//编写时间:2006.09.20
	int n=X.Size(),i;
	switch(loc)
	{
	case 'b':		
		     if ( mode=="zpd")
		     {
				 Y.Set(n+2*l);
				 for (i=1;i<=n;i++)
				 {
					 Y[l+i]=X[i];
				 }
		     }
			 else if (mode=="sym")
			 {
				 	int a=l/n,mod=l%n;
				 if (a%2==0)
				 {
					 if (mod!=0)
					 {
			         Y.GetLeft(X,mod);
					 Y.Invers();
					 }
                     Y.Link(X);
					 for (i=2;i<=2*a+1;i++)
					 {
						 X.Invers();
						 Y.Link(X);
					 }
					 if (mod!=0)
					 {
						 vector X_temp;
					 X_temp.GetRight(X,mod);
					 X_temp.Invers();
					 Y.Link(X_temp);
					 }
				 }
				 else if (a%2==1)
				 {
					 if (mod!=0)
					 {
                        Y.GetRight(X,mod); 
					 }
					
					 for (i=1;i<=2*a+1;i++)
					 {
						 X.Invers();
						 Y.Link(X);
					 }
					  if (mod!=0)
					  {
					 X.Invers();
					 vector X_temp1;
					 X_temp1.GetLeft(X,mod);
					 Y.Link(X_temp1);
					  }

				 }
			 }
			 else if (mode=="ppd")
			 {
				 int a=l/n,mod=l%n;
                 if (mod!=0)
				 {
					 Y.GetRight(X,mod);
				 }
				 
				 for (i=1;i<=2*a+1;i++)
				 {
					 Y.Link(X);
				 }
				 if (mod!=0)
				 {
			     vector X_temp2;
				 X_temp2.GetLeft(X,mod);
			     Y.Link(X_temp2);
				 }
				 
			 }
			 else if (mode=="per")
			 {
				if (n%2==1)
				{
					X.Add(X[n]);
					int nn=X.Size();
                    int a=l/nn,mod=l%nn;
					if (mod!=0)
					{
							Y.GetRight(X,mod);
					}
				
				    for (i=1;i<=2*a+1;i++)
					{
					   Y.Link(X);