yulewalk.cpp

DSP 实现基于BURG算法的功率谱估计

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

struct complex
{
  double real,imag;
};
complex set(double r,double i);
complex operator +(complex c1,complex c2);
complex operator *(complex c1,complex c2);
complex operator -(complex c1,complex c2);
complex operator *(complex c1,double c2);
//**********************************************************************************/
complex operator *(complex c1,double c2)
{
 complex temp;
 temp.real=c1.real*c2;
 temp.imag=c1.imag*c2;
 return temp;
}

complex operator *(double c2,complex c1)
{
  complex temp;
  temp.real=c1.real*c2;
  temp.imag=c1.imag*c2;
  return temp;
}

//*********************************************************************************/
complex set(double r,double i)
{
  complex temp;
  temp.real=r;
  temp.imag=i;
  return temp;
}

//********************************************************************************/
complex operator +(complex c1,complex c2)
{
   complex temp;
   temp.real=c1.real+c2.real;
   temp.imag=c1.imag+c2.imag;
   return temp;
}

//*******************************************************************************/
complex operator -(complex c1,complex c2)
{
   complex temp;
   temp.real=c1.real-c2.real;
   temp.imag=c1.imag-c2.imag;
   return temp;
}

//******************************************************************************/
complex operator *(complex c1,complex c2)
{
     complex temp;
     temp.real=c1.real*c2.real-c1.imag*c2.imag;
     temp.imag=c1.imag*c2.real+c1.real*c2.imag;
     return temp;
}

//******************************************************************************/
complex operator / (complex a,double b)
{
  complex divi;
  divi.real=a.real/b;
  divi.imag=a.imag/b;
  return(divi);
}

//******************************************************************************/
complex conjg(complex x)
{
  complex temp;
  temp.real=x.real;
  temp.imag=-x.imag;
  return temp;
}

//*****************************************************************************/
double mod(complex x)
{
  double temp;
  temp=(x.real)*(x.real)+(x.imag)*(x.imag);
  return temp;
}

//*****************************************************************************/
double czz_abs(complex c)
{
   double temp;
        temp=sqrt(mod(c));
   return temp;
}

//*****************************************************************************/
float czz_max(float c1,float c2)
{
   float temp;
   if(c1<=c2)
     c1=c2;
   temp=c1;
   return temp;
} 

//输入参数
//m-所求的系数a(i)的维数
//to-与矩阵元素t(0)相对应的实标量
//t-Toeplitz矩阵最左边一列的元素t(1),……,t(m)
//输出参数：p-右边向量的第一个元素，即输入白噪声激励的方差
//          a-自回归系数，即a(0),a(1),……
//      istat-整数值，当为0时，正常；当为1时，即p=0（奇异矩阵）
void levinson(int m,double to,complex *t,double *p,complex *a,int *istat,double *p_last,int *p_val_ip)
{
  int k,kj,j,khalf;
  complex save,temp;
  double p1;
  * p=to;                               //to就是R(0)，就是输入白噪声激励的方差
  * istat=0;                            //表明正常
  if(m>0)
  {
    k=0;
    do
    {
       k=k+1;
       save=t[k];
       if(k==1);
       else
       {
          for(j=1;j<=k-1;j++)
            save=save+a[j]*t[k-j];     //计算D
       }
	   *p_last=*p;
       p1=1./(-(*p));                  
       temp=save*p1;                   //计算-r
       *p=(*p)*(1.-mod(temp));         //计算下一个方差值
       if((*p)>0);
       else
       {
          *istat=1;                   //方差值为负,不正常,结束循环
		  *p=*p_last;
		  *p_val_ip=k-1;
          break;
       }
       a[k]=temp;
       if(k==1)
		   ;
       else
       {
           khalf=k/2;
         for(j=1;j<=khalf;j++)
         {
           kj=k-j;
           save=a[j];
           a[j]=save+temp*conjg(a[kj]);
           if(j==kj)
			   ;
           else
              a[kj]=a[kj]+temp*conjg(save);
         }
       }
     }while(k<m);
   }
}    

/*************************************************************************************************************************/
//n------Number of data samples in array x and y (integer)
//lag----Number of correlation lags to compute(lags from 0 to Lag are computed and stored in r0 and r(1) to r(LAG)) (integer)
//mode---Set to 0 for unbiased correlation estimates;otherwise biased correlation estimates are computed(integer)
//x------Array of complex data sample x(1) through x(N)
//y------Array of complex data samples y(1) through y(N)
//Output parameter:
//r0-----complex correlation estimate for lag 0
//r------Array of complex correlation estimates for lags 1 to LAG 

void correlation(int n,int lag,int mode,complex *x,complex *y,complex r0,complex *r)
{
  int k,nk,j;
  static complex sum={0.,0};
  for(k=0;k<=lag;k++)
  {
    nk=n-k;
    sum.real=0.;
    sum.imag=0.;
    for(j=1;j<=nk;j++)
        sum=sum+x[j+k]*conjg(y[j]);
    if(k==0)
        r[0]=r0=sum/(float)n;
    else if (mode==0)
        r[k]=sum/(float)(n-k);
    else
        r[k]=sum/(float)n;
   }
} 


/***************************************************************************************************************************/
//Input parameters:
//    n------Number of data samples
//    ip-----Order of autoregressive to be fitted
//    l------Set this integer to 0 for unbiassed lags,or l for biassed
//    x------Array of complex data values,x[1] to x[n]
//Output parameters;
//    p------Driving nios variance
//    a------Array of complex autoregressive coeffcients,a[l] to a[ip]
//    istat--Status indicator,0 for normal exit,1 for levinsion ill-conditioned 


void yulewalk(int n,int ip,int l,complex *x,double *p,complex *a,int *istat,double *p_last,int *p_val_ip)
{
  double rzero=0.;
  complex *r=new complex[ip+5];
  complex r0={0.,0.};
  correlation(n,ip,l,x,x,r0,r);
  rzero=r[0].real;
  levinson(ip,rzero,r,p,a,istat,p_last,p_val_ip);
  delete r;
}
#define order    40                         //尝试的阶数
#define mode     1                          //选择求相关的模式，0 为无偏，1为有偏
#define num_dat  64                         //实际有效数据的个数
void main()
{   
	static int n=num_dat,ip=order,l=mode;
	int valid_ip=order;
	double noise_var=0.;
	double noise_var_last=0;
	double * p;
	double * p_last;
	int * p_val_ip;
	complex a[order+1];                            
	int status=0;
	int * istat;
	complex in_data[num_dat+1];
	int i;
	p= & noise_var;
	p_last=& noise_var_last;
	p_val_ip=&valid_ip;
	istat= & status;
    ifstream f1("D:\\homework\\test_dat.txt",ios::binary);
	if(! f1)
	{
		cout<<"cannot open this file for input";
	}
	ofstream f2("D:\\homework\\test_yule_output.txt");
	if(! f2)
	{
		cout<<"cannot open this file for output";
	}
	ofstream f3("D:\\homework\\test_yule_output_next.txt");
	if(! f3)
	{
		cout<<"cannot open this file for output";
	}
	if(num_dat>order)
	{
	  for(i=0;i<=ip;i++)
	  {

            a[i].real=0.;
			a[i].imag=0.;
	  }
      for(i=0;i<num_dat+1;i++)
	  {
		f1>>in_data[i].real>>in_data[i].imag;
	  }
	  yulewalk(n,ip,l,in_data,p,a,istat,p_last,p_val_ip);
   	  f2<<"状态为："<<status<<"\n"<<"方差为："<<setprecision(10)<<noise_var<<"\n"<<"此时的有效阶数是："<<valid_ip<<"\n";
 	  f3<<setprecision(10)<<noise_var<<"\n";
	  for(i=1;i<=valid_ip;i++)
	  {

       f2<<setprecision(10)<<a[i].real<<"     "<<a[i].imag<<"\n";
	   f3<<setprecision(10)<<a[i].real<<"     "<<a[i].imag<<"\n";
	  }
      f1.close();
	  f2.close();
	}
	else
		f2<<"阶数不能大于有效数据的个数"<<"\n"<<"请重新选择阶数后，再运行程序！";
}