psd.c

计算ARMA(p

#include"math.h"
void psd(double b[],double a[],int q,int p,double sigma2,double fs,double x[],double freq[],int len,int sign)
{
	int i,k;
	double ar,ai,br,bi,zr,zi,im,re,xre,xim;
	double ang,den,numr,numi,temp;
	for(k=0;k<len;k++)
	{
		ang=k*0.5/(len-1);
		freq[k]=ang*fs;
		zr=cos(-8.0*atan(1.0)*ang);
		zi=sin(-8.0*atan(1.0)*ang);
		br=0.0;
		bi=0.0;
		for(i=q;i>0;i--)
		{
			re=br;
			im=bi;
			br=(re+b[i])*zr-im*zi;
			bi=(re+b[i])*zi+im*zr;
		}
		ar=0.0;
		ai=0.0;
		for(i=p;i>0;i--)
		{
			re=ar;
			im=ai;
			ar=(re+a[i])*zr-im*zi;
			ai=(re+a[i])*zi+im*zr;
		}
		br=br+b[0];
		ar=ar+1.0;
		numr=ar*br+ai*bi;
		numi=ar*bi-ai*br;
		den=ar*ar+ai*ai;
		xre=numr/den;
		xim=numi/den;
		switch(sign)
		{
		case 0:
			{
				x[k]=xre*xre+xim*xim;
				x[k]=sigma2*x[k]/fs;
				break;
			}
		case 1:
			{
				temp=xre*xre+xim*xim;
				temp=sigma2*temp/fs;
				if(temp==0.0)temp=1.0e-20;
				x[k]=10.0*log10(temp);
			}
		}
	}
}