Prony算法

%打开指定文件，并对信号进行Pon分析计算function[M,Amp,Fre,damp,Pha,main_f,main_damp,x,xc,y,Amp_Response,er,all,N,dt]=x_svdprony(x_in,dt,fL,showfigure)formatlong;x_in=[0.19990.19530.18980.18390.17680.17270.16840.16650.16640.16640.1663];x=x_in';cpu=cputime;N=size(x,1);%取N/2的整数部分为初始的PP=floor(N/2);%去直流环节x_Sum=0;fori=1:Nx_Sum=x_Sum+x(i);endx_av=x_Sum/N;ifx_av10E-10fori=1:Nx(i)=x(i)-x_av;endend%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%滤波环节%fL=1;fL=2;iffL1fori=fL+1:Nx(i-fL)=0;forj=1:fLx(i-fL)=x(i-fL)+(1/fL)*x(i-j+1);endendendN=N-fL;tt=0:1:N-1;%P要求为偶数ifmod(P,2)~=0P=P-1;endP1=P;ifmod(P,2)==0%GenerateR，生成样本矩阵fori=1:P1forj=1:P1+1ss=0;fork=P1:N-1%前向预测误差ss=ss+x(k+2-j,1)*x(k+1-i,1);%后向预测误差%ss=ss+x(k+1-P1+i)*x(k+1-P1-1+j);%同时考虑双向误差%ss=ss+x(k+2-j,1)*x(k+1-i,1)+x(k+1-P1+i)*x(k+1-P1-1+j);endR(i,j)=ss;endend%divideRintoR1andR2,andgetA;R1*A=R2;fori=1:Pforj=1:PR1(i,j)=R(i,j+1);endendfori=1:PR2(i)=R(i,1);end%添加的代码，使用SVD－TLS算法%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%首先进行SVD分解[u,s,v]=svd(R1);%根据奇异值确定实际的阶数MM=0;%计算全部奇异值的均方和Sum_SVD=0;fori=1:PSum_SVD=Sum_SVD+s(i,i)^2;endcur_sum=0;i=1;while1-sqrt(cur_sum/Sum_SVD)0.000001&i=Pcur_sum=cur_sum+s(i,i)^2;M=M+1;i=i+1;end%输出预测的阶数MM;%生成Sp矩阵Sp=zeros(M+1,M+1);forj=1:Mfori=1:(P-1)-M+1Sp=Sp+s(j,j)^2*v(i:i+M,j)*v(i:i+M,j)';endend%根据公式生成最佳最小二乘解Sp_inv=inv(Sp);ifisinf(Sp_inv(1,1))==1Sp_inv=pinv(Sp);enda_SVD=Sp_inv(1+1:M+1,1)/Sp_inv(1,1);P=M;%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%GetZicof_SVD(1)=1;fori=1:Mcof_SVD(i+1)=a_SVD(i);endZ=roots(cof_SVD);%Gety,yshouldbeveryclosetoxfori=1:Py(i)=x(i);endfori=P+1:Ny(i)=0;endfori=P+1:Nforj=1:Py(i)=y(i)-a_SVD(j)*x(i-j);endend%GetBfori=1:Nforj=1:PFy(i,j)=Z(j)^(i-1);endendFz=Fy';FH=Fy'*Fy;Fn=inv(FH);B=inv(Fy'*Fy)*Fy'*y';%CalculateAmplitude,Phasor,Frequencyanddampdt=0.01;fori=1:PAmp(i)=abs(B(i));Pha(i)=atan(imag(B(i))/real(B(i)));Fre(i)=atan(imag(Z(i))/real(Z(i)))/(2*pi*dt);damp(i)=log(abs(Z(i)))/dt;end%调试用的代码ifisnan(Amp(1))==1error('幅值的求解出现错误！');end%Getxc,verifyifxcisnearlyequaltoxfori=1:Pif(real(B(i))=0.0)bth(i)=Pha(i);elsebth(i)=pi+Pha(i);endend%对Pha重新幅值fori=1:PifPha(i)0Pha(i)=Pha(i)+2*pi;endendfori=1:Nxc(i)=0.0;forj=1:Pxc(i)=xc(i)+Amp(j)*exp(damp(j)*(i-1)*dt)*cos(2*pi*Fre(j)*(i-1)*dt+bth(j));endend%ifshowfigure==1%%显示特征根的位置%figure;%plot(damp,2*pi*Fre,'r*');%endxj=xc';er=0;all=0;fori=1:Ner=er+(x(i)-xc(i))^2;all=all+x(i)^2;endSNR=-20*log(sqrt(er/all));%CalculatePronyspectrum%频谱的取值范围为0－5Hzf=0:0.01:4.99;forj=1:size(f,2)sptr(j)=0;sptr1(j)=0;sptr2(j)=0;angl(j)=0;tmpangle=0;fork=1:Psptr1(j)=sptr1(j)+Amp(k)*cos(bth(k))*(2*damp(k)/(damp(k)^2+(2*pi*(f(j)-Fre(k)))^2));sptr2(j)=sptr2(j)+Amp(k)*sin(bth(k))*(2*damp(k)/(damp(k)^2+(2*pi*(f(j)-Fre(k)))^2));endsptr(j)=sqrt(sptr1(j)^2+sptr2(j)^2);tmpangle=atan2(sptr1(j),sptr2(j));angl(j)=tmpangle*360/pi;end%对幅频响应进行归一化，并且寻找主导频率模式%寻找sptr的最大值max_sptr=0;main_f=0;forj=1:size(f,2)ifsptr(j)max_sptrmax_sptr=sptr(j);%主导频率出现在最大幅频响应位置iff(j)~=0;main_f=f(j);elsemax_sptr=0;endendendmain_f;%找出真正计算的频率值f_err=10;f_main=0;fori=1:size(Amp,2)ifabs(main_f-Fre(i))f_errf_main=Fre(i);damp_main=damp(i);f_err=abs(main_f-Fre(i));endendmain_f=f_main;main_damp=damp_main;%进行归一化forj=1:size(f,2)sptr(j)=sptr(j)/max_sptr;Amp_Response(j)=sptr(j);endfori=1:size(f,2)ifangl(i)0angl(i)=angl(i)-360;endendshowfigure=1;ifshowfigure==1%在一个图上显示时域拟和曲线和Prony频谱分布figure;subplot(2,1,1);%plot(tt,x(1:N),'b',tt,y,'r',tt,xc,'*b');plot(tt,x(1:N),'r',tt,xc,'*b');subplot(2,1,2);plot(f,sptr);%subplot(2,2,3);%plot(f,sptr);%subplot(2,2,4);%plot(f,angl);endcpu=cputime-cpu;formatshort;end