waveletcontrast.txt

提供了子波域矩阵加权

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%仿真的例子是           [1,T,T^2/2]        [0]
%              x(k+1)=  | 0,1,   T| *x(k) +|0|*J(k)
%                       [ 0,0,   1]        [T]
%              y(k) =  [1 0 0]*x(k) + V
%其中T=0.1
% the dynamic equation is X(k+1)=F*X(k)+L*J(k)
% the measurement equation is Z(k)=H*X(k)+V
%J~N(0,q);V~N(0,r)


clear all;
close all;
%initilize parameters
warning off MATLAB:singularMatrix
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

XR=zeros(3,500);   
XR1=zeros(3,500);
X=zeros(3,500);
Xf=zeros(3,500);
Xu=zeros(3,500);
Xy1=zeros(3,500);
Xe2=zeros(3,500);
Xs3=zeros(3,500);
Xy=zeros(3,3,500);
Xe=zeros(3,3,500);
Xs=zeros(3,3,500);
P=zeros(3,3,500);
Pf=zeros(3,3,500);  %3乘3矩阵，500页
Pu=zeros(3,3,500);
Py=zeros(3,3,500);
Pe=zeros(3,3,500);
Ps=zeros(3,3,500);
F=zeros(3,3,500);
L=zeros(3,1,500);

W=zeros(3,500);
Z=zeros(1,500);
J=zeros(1,500);  %白噪声
V=zeros(1,500);  %白噪声
sigma=1.000;
T=0.1;           %采样周期
XR1(:,1)=[1,0.5,0]';    %第一列
XR(:,100)=[1,0.5,0.04]';
X(:,1)=[0.5,0.4,0.038]';
P(:,:,1)=[1,0,0;
          0,1,0;        %第一页
          0,0,1];
F(:,:,1)=[1,T,T^2/2;    %状态方程,三阶
          0,1,T;
          0,0,1];
L(:,:,1)=[0,0,T]';
H=[1,0,0];
Z(1)=2.0;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%--------local paramters initialize-----------------%   
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for i=2:100
    F(:,:,i)=sigma*F(:,:,i-1);    %让所有页都相等
    XR(:,i)=F(:,:,i-1)*XR1(:,i-1);%X without noise,1到100列，使用的是相同的XR1
end
for i=101:500
    F(:,:,i)=sigma*F(:,:,i-1);
    XR(:,i)=F(:,:,i-1)*XR(:,i-1);  %X without noise
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%----------add estate noise-------------------------%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

R=randn(1,600);

j=1;
for i=1:600
       if  (abs(R(i))<3)&(j<=500)  %把小于3的值赋给J,去除特异值
             J(j)=R(i);
             j=j+1;
        end;       
 end; 
q=var(J);          %J的方差
for i=2:500;
  L(:,:,i)=sigma*L(:,:,i-1); 
  W(:,i)=L(:,:,i-1)*J(i-1);
end;

Z=H*(XR+W);

%add meansure noise 
U=randn(1,600);

j=1;
for i=1:600
    if (abs(U(i))<3)&(j<=500)
           V(j)=U(i);           
     j=j+1;
    end    
end;
r=var(V);
Z=Z+V;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%---------multi-scale分解---------------------------%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

s=randn(1,500);                    
ls=length(s);
t=[0:ls-1];
[C,L1]=wavedec(s,3,'db4');          %对信号进行多尺度分解(三层)
C3=appcoef(C,L1,'db4',3); 
D3=detcoef(C,L1,3); 
D2=detcoef(C,L1,2); 
D1=detcoef(C,L1,1);  
SRC3=wrcoef('a',C,L1,'db4',3);
SRD3=wrcoef('d',C,L1,'db4',3);      %第3层
SRD2=wrcoef('d',C,L1,'db4',2);      %第2层
SRD1=wrcoef('d',C,L1,'db4',1);      %第1层
SR=waverec(C,L1,'db4'); 
figure(1);
subplot(713);plot(t,SRC3);Ylabel('SRC3');
subplot(714);plot(t,SRD3);Ylabel('SRD3');
subplot(715);plot(t,SRD2);Ylabel('SRD2');
subplot(716);plot(t,SRD1);Ylabel('SRD1');
SR=waverec(C,L1,'db4'); 
subplot(717);plot(t,SR);Ylabel('SR');           %显示多尺度分解各部分

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%---------2层kalman filtering-----------------------%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

x=[1 2 3];  %一个全局变量，用于选择不同分解层的信号

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%---------第一层kalman滤波(基于多尺度)--------------%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for i=1:3   

    switch x(i)

	case 1    %尺度1
	
		for k=2:500 

		Xy(:,:,k)=F(:,:,k-1)*SRD1(k);     %SRD1信号乘状态方程
   		Py(:,:,k)=F(:,:,k-1)*P(:,:,k-1)*F(:,:,k-1)'+L(:,:,k-1)*q*L(:,:,k-1)'; 
    		P(:,:,k)=inv(inv(Pf(:,:,k))+H'*inv(r)*H);   
    		Ky=P(:,:,k)*H'*inv(r);
    		Xy1(:,k)=Xf(:,k)+Ky*(Z(k)-H*Xf(:,k));   
      
        	end 

	case 2   %尺度2

		for k=2:500 

		Xe(:,:,k)=F(:,:,k-1)*SRD2(k);    %SRD2信号乘状态方程
   		Pe(:,:,k)=F(:,:,k-1)*P(:,:,k-1)*F(:,:,k-1)'+L(:,:,k-1)*q*L(:,:,k-1)';   
    		P(:,:,k)=inv(inv(Pf(:,:,k))+H'*inv(r)*H); 
    		Ke=P(:,:,k)*H'*inv(r);
    		Xe2(:,k)=Xf(:,k)+Ke*(Z(k)-H*Xf(:,k));   
      
        	end
	
	case 3   %尺度3

		for k=2:500 

		Xs(:,:,k)=F(:,:,k-1)*SRD3(k);   %SRD3信号乘状态方程
   		Ps(:,:,k)=F(:,:,k-1)*P(:,:,k-1)*F(:,:,k-1)'+L(:,:,k-1)*q*L(:,:,k-1)';   
    		P(:,:,k)=inv(inv(Pf(:,:,k))+H'*inv(r)*H); 
    		Ks=P(:,:,k)*H'*inv(r);
    		Xs3(:,k)=Xf(:,k)+Ks*(Z(k)-H*Xf(:,k));
      
        	end

	otherwise

		disp('Not the correct value !');
 
    end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%---------第一层修正加权融合部分--------------------%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if(det(Pu(:,:,1))>0)          %如果相关矩阵正定,则按矩阵加权

	for k=2:500 

	Xu(:,k)=F(:,:,k-1)*X(:,k-1); 
   	Pf(:,:,k)=F(:,:,k-1)*P(:,:,k-1)*F(:,:,k-1)'+L(:,:,k-1)*q*L(:,:,k-1)'; %矩阵系数  
    	P(:,:,k)=inv(inv(Pf(:,:,k))+H'*inv(r)*H);   %inv()矩阵求逆
    	Kr=P(:,:,k)*H'*inv(r);
    	Xu(:,k)=Xf(:,k)+Kr*(Z(k)-H*Xf(:,k));   
      
        end  
else                 %如果相关矩阵半正定,则按标量加权

	for k=2:500 

	Xu(:,k)=F(:,:,k-1)*X(:,k-1);
    	P(:,:,k)=inv(inv(Pf(:,:,k))+H'*inv(r)*H);   %inv()矩阵求逆
        Pf(:,:,k)=P(:,:,k-1)+L(:,:,k-1)*q*L(:,:,k-1)';    %与矩阵加权不同,按标量
    	Kr=P(:,:,k)*H'*inv(r);
    	Xu(:,k)=Xf(:,k)+Kr*(Z(k)-H*Xf(:,k));   
      
	end  

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%---------重新建立新的状态方程和观测方程------------%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

CDI=eye(3,3);
XBI=eye(3,3);
CD=zeros(3,3,500);
XB=zeros(3,3,500);
L2I=[1 0 0];
L2=zeros(3,1,500);

for i=1:500
	CD(:,:,i)=CDI;
	XB(:,:,i)=XBI;
	L2(:,:,i)=L2I;
end


CD1=zeros(1,3,500);
XB1=zeros(1,3,500);
XB2=zeros(1,3,500);
T=zeros(3,3,500);

CDD=[1 0 0;0 0 1;0 1 0];  %乘一个本仿真例子的初始值
XBB=[0 0 1;0 1 0;1 0 0];  

for i=1:500
	CD(:,:,i)=CDD*CD(:,:,i);     %尺度算子初值				  
	XB(:,:,i)=XBB*XB(:,:,i);     %小波算子初值
end

T(:,:,1)=[1 0 0;
	  0 1 0;
	  0 0 1];     %正交矩阵初值
				   				   
for i=1:500            %计算正交矩阵各分项
	CD1(:,:,i)=L2(:,:,i)'*CD(:,:,i);
	XB1(:,:,i)=L2(:,:,i)'*XB(:,:,i);                       
	XB2(:,:,i)=XB1(:,:,i)*CD(:,:,i);
end

for i=2:500
	T(:,:,i)=[CD1(:,:,i-1);
		  XB2(:,:,i-1);
		  XB1(:,:,i-1)];  %多尺度分解的正交矩阵
end

for i=2:500                     %计算新的状态方程，观测方程，误差
	F1(:,:,i-1)=T(:,:,i-1)*F(:,:,i-1)*T(:,:,i-1)';
	L2(:,:,i-1)=T(:,:,i-1)*L(:,:,i-1);
	H1=H*T(:,:,i-1);
	W1=T(:,:,i-1)*W;   
end

for i=2:500                     %将新的状态方程，观测方程，误差回代，赋给原值
	F(:,:,i-1)=F1(:,:,i-1);
	L(:,:,i-1)=L2(:,:,i-1);
	H=H1;
	W=W1;   
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%---------第二层kalman滤波--------------------------%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for k=2:500  
	
    Xf(:,k)=F(:,:,k-1)*X(:,k-1);      %the value of one step forecast
    Pf(:,:,k)=F(:,:,k-1)*P(:,:,k-1)*F(:,:,k-1)'+L(:,:,k-1)*q*L(:,:,k-1)';  
    P(:,:,k)=inv(inv(Pf(:,:,k))+H'*inv(r)*H);   %inv()矩阵求逆
    Kk=P(:,:,k)*H'*inv(r);
    X(:,k)=Xf(:,k)+Kk*(Z(k)-H*Xf(:,k));   
      
end   

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%---------第二层修正加权融合部分--------------------%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if(det(Pu(:,:,1))>0)          %如果相关矩阵正定,则按矩阵加权

	for k=2:500 

	Xu(:,k)=F(:,:,k-1)*X(:,k-1); 
   	Pf(:,:,k)=F(:,:,k-1)*P(:,:,k-1)*F(:,:,k-1)'+L(:,:,k-1)*q*L(:,:,k-1)'; %矩阵系数  
    	P(:,:,k)=inv(inv(Pf(:,:,k))+H'*inv(r)*H);   %inv()矩阵求逆
    	Kr=P(:,:,k)*H'*inv(r);
    	Xu(:,k)=Xf(:,k)+Kr*(Z(k)-H*Xf(:,k));   
      
        end  
else                 %如果相关矩阵半正定,则按标量加权

	for k=2:500 

	Xu(:,k)=F(:,:,k-1)*X(:,k-1);
    	P(:,:,k)=inv(inv(Pf(:,:,k))+H'*inv(r)*H);   %inv()矩阵求逆
        Pf(:,:,k)=P(:,:,k-1)+L(:,:,k-1)*q*L(:,:,k-1)';    %与矩阵加权不同,按标量
    	Kr=P(:,:,k)*H'*inv(r);
    	Xu(:,k)=Xf(:,k)+Kr*(Z(k)-H*Xf(:,k));   
      
	end  

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%------compute the statistics-----------------------%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

xmvar=zeros(3,1);          %均值，3维列向量
xkvar=zeros(3,1);
xk1var=zeros(3,1);
X(1,:)=X(1,:)/2;
XV=X-XR;
for i=1:3
    xvar(i)=XV(i,:)*XV(i,:)'/500;
    xfvar(i)=(Xf(i,:)-XR(i,:))*(Xf(i,:)-XR(i,:))'/500;
end
xmvar(1)=(Z-XR(1,:))*(Z-XR(1,:))'/500;

for i=1:500
    d(i)=i;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%----------------三种加权集中对比-------------------%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

figure(2);
plot(d,XR(1,:),'k',d,X(1,:),'-.g',d,X(2,:),':r',d,X(3,:),'b')
legend('理想值','标量加权','矩阵加权','修正加权');
title('三种方法位置对比图');
xlabel('t/s')
ylabel('s/m')
set(gcf,'Color',[1 1 1])

figure(3);
plot(d,XR(1,:),'k',d,Xf(1,:),'-.g',d,Xf(2,:),':r',d,Xf(3,:),'b')
legend('理想值','标量加权','矩阵加权','修正加权');
title('三种方法速度对比图');
xlabel('t/s')
ylabel('v(m/s)')
set(gcf,'Color',[1 1 1])