x_svdprony.m

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)
    format long;
    x = x_in';
    cpu=cputime;
	N=size(x,1);
	%取N/2的整数部分为初始的P
	P=floor(N/2);
    
	%去直流环节
	x_Sum = 0;
	for i=1:N
	    x_Sum = x_Sum + x(i);
	end 
	x_av = x_Sum / N;
    if x_av > 10E-10
    	for i=1:N
	        x(i) = x(i) - x_av;
    	end
    end
	%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

	%滤波环节
	%fL=1;
	if fL>1
	    for i=fL+1:N
	        x(i-fL)=0;
	        for j=1:fL
	            x(i-fL) = x(i-fL)+(1/fL)*x(i-j+1);
	        end
	    end
	end
	N=N-fL;
	tt=0:1:N-1;
	
	%P要求为偶数
	if mod(P, 2) ~= 0
	   P = P - 1;
	end
	
	P1=P;
	if mod(P, 2) == 0
	   % Generate R，生成样本矩阵
	   for i=1:P1
	          for j=1:P1+1
	              ss=0;
	              for k=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);              
	              end   
	              R(i,j)=ss;
	          end
	   end

   	   % divide R into R1 and R2, and get A; R1*A=R2;
	   for i=1:P
	      for j=1:P
	         R1(i,j)=R(i,j+1);
	      end
	   end
	   for i=1:P
     	      R2(i)=R(i,1);
	   end
	
	   %添加的代码，使用SVD－TLS算法%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	   %首先进行SVD分解
	   [u,s,v]=svd(R1);
	   %根据奇异值确定实际的阶数M
	   M=0;
	   %计算全部奇异值的均方和
	   Sum_SVD=0;
	   for i=1:P
	      Sum_SVD = Sum_SVD+s(i,i)^2;
	   end
	   cur_sum = 0;
	   i=1;
	   while 1 - sqrt(cur_sum/Sum_SVD) > 0.000001 & i<=P
	      cur_sum = cur_sum + s(i,i)^2;
	      M = M + 1;
	      i = i + 1;
	   end
	   %输出预测的阶数M
	   M;

	   %生成Sp矩阵
	   Sp=zeros(M+1, M+1);
	   for j=1:M 
	      for i=1:(P-1)-M+1
	         Sp = Sp+s(j,j)^2*v(i:i+M,j)*v(i:i+M,j)';
	      end
	   end
	
	   %根据公式生成最佳最小二乘解
       Sp_inv=inv(Sp);
       if isinf(Sp_inv(1,1)) == 1
           Sp_inv = pinv(Sp);
       end
       
	   a_SVD = Sp_inv(1+1:M+1, 1)/Sp_inv(1,1);
	   P = M;
	   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	   % Get Zi
	   cof_SVD(1)=1;
	   for i=1:M
	      cof_SVD(i+1)=a_SVD(i);
	   end
	   Z=roots(cof_SVD);
	   
       % Get y,y should be very close to x
	   for i=1:P
	      y(i)=x(i);
	   end
	
	   for i=P+1:N
	      y(i)=0;
	   end
	   for i=P+1:N
	      for j=1:P
	         y(i)=y(i)-a_SVD(j)*x(i-j);
	      end
	   end
	    
	   % Get B
	   for i=1:N
	      for j=1:P
	         Fy(i,j)=Z(j)^(i-1);
	       end
	   end
	   Fz=Fy';
	   FH=Fy'*Fy;
	   Fn=inv(FH);
	   B = inv(Fy'*Fy)*Fy'*y';
	
	   % Calculate Amplitude, Phasor, Frequency and damp
	   for i=1:P
	      Amp(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

       %调试用的代码
       if isnan(Amp(1)) == 1
           error('幅值的求解出现错误！');
       end
       
	   % Get xc,verify if xc is nearly equal to x
	   for i=1:P
	         if(real(B(i))>=0.0)
	             bth(i)=Pha(i);
	         else
	             bth(i)=pi+Pha(i);
	         end
	   end
       %对Pha重新幅值
       for i=1:P
           if Pha(i) < 0
               Pha(i) = Pha(i) + 2*pi;
           end
       end
       
	   for i=1:N
	         xc(i)=0.0;
	         for j=1:P
	             xc(i)=xc(i)+Amp(j)*exp(damp(j)*(i-1)*dt)*cos(2*pi*Fre(j)*(i-1)*dt+bth(j));
	         end
	   end

%       if showfigure == 1
%    	  %显示特征根的位置
%	      figure;
%	      plot(damp, 2*pi*Fre, 'r*');
%       end
       
	   xj=xc';
	   er=0;
	   all = 0;
	   for i=1:N          
	          er=er+(x(i)-xc(i))^2;
	          all = all+x(i)^2;
	   end
	   SNR=-20*log(sqrt(er/all));
	   
	   % Calculate Prony spectrum
	   %频谱的取值范围为0－5Hz
	   f=0:0.01:4.99;
	   for j=1:size(f,2)
	       sptr(j)=0;
	       sptr1(j)=0;
	       sptr2(j)=0;
	       angl(j)=0;
	       tmpangle = 0;
	       for k=1:P
	          sptr1(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));
	       end
	       sptr(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;
       for j=1:size(f,2)
           if sptr(j) > max_sptr
               max_sptr = sptr(j);
               %主导频率出现在最大幅频响应位置
               if f(j) ~= 0;
                   main_f = f(j);
               else
                   max_sptr = 0;
               end
           end
       end
       main_f;
       %找出真正计算的频率值
       f_err = 10;
       f_main = 0;
       for i=1:size(Amp, 2)
           if abs(main_f - Fre(i)) < f_err
               f_main = Fre(i);
               damp_main = damp(i);
               f_err = abs(main_f - Fre(i));
           end
       end
       main_f = f_main;
       main_damp = damp_main;
       %进行归一化
       for j=1:size(f,2)
           sptr(j)=sptr(j)/max_sptr;
           Amp_Response(j) = sptr(j);
       end
       
	   for i=1:size(f,2)
	       if angl(i) > 0
	           angl(i)=angl(i)-360;
	       end
	   end
	   
       if showfigure == 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);
      end
      
	  cpu=cputime-cpu;
      format short;
	end