distributed_fusion.asv

数据融合的问题

%--path(path,'E:\相关算法\建模与估计')
close all;
clc;
clear;
Bushu=300;
%-----------------------mode init-----------------------------------------
T=0.1;fai=[1 T;0 1];gama=[0.5*T^2;T];R1=4;R2=9;R3=25;
H1=[1 0];H2=[1 0];H3=[1 0];Q=4;
%-------------------------------------------------------------------------
randn('seed',14);w=sqrt(Q)*randn(1,Bushu);
randn('seed',8);v1=sqrt(R1)*randn(1,Bushu);
randn('seed',4);v2=sqrt(R2)*randn(1,Bushu);
randn('seed',5);v3=sqrt(R3)*randn(1,Bushu);
%--------------------------------------------------------------------------
x(:,Bushu)=[0 0]';
for i=2:Bushu;
    x(:,i)=fai*x(:,i-1)+gama*w(i-1);
    y(1,i)=H1*x(:,i)+v1(i);
    y(2,i)=H2*x(:,i)+v2(i);
    y(3,i)=H3*x(:,i)+v3(i);
end
%--------------------调用函数部分-------------------------------------------
%too many output arguments 暂时去掉最后一参数xxjian
[xigema1,kf1,kp1,pusaif1,pusaip1,e1,xjian1]=TS_WTKalman(fai,gama,...
H1,Q,R1,y(1,1:Bushu));%--sensor1
[xigema2,kf2,kp2,pusaif2,pusaip2,e2,xjian2]=TS_WTKalman(fai,gama,...
H2,Q,R2,y(2,1:Bushu));%--sensor2
[xigema3,kf3,kp3,pusaif3,pusaip3,e3,xjian3]=TS_WTKalman(fai,gama,...
H3,Q,R3,y(3,1:Bushu));%--sensor3
%-------------------集中式-------------------------------------------------
HJiZ=[H1;H2;H3];YJiZ=y;VJiZ=[v1;v2;v3];RJiZ=[R1 0 0;0 R2 0;0 0 R3];
[xigemaJiZ,kfJiZ,kpJiZ,pusaifJiZ,pusaipJiZ,eJiZ,xjianJiZ]=...
TS_WTKalman(fai,gama,HJiZ,Q,RJiZ,y);
%--------------------figure part-------------------------------------------
t=1:Bushu;
figure(1); 
subplot(2,2,1);plot(t,x(1,t),'b',t,xjian1(1,t),'r:');
title('稳态kalman滤波仿真通式')
subplot(2,2,2);plot(t,x(2,t),'b',t,xjian1(2,t),'r:');
figure(2);
subplot(2,2,1);plot(t,x(1,t),'b',t,xjian2(1,t),'r:');
title('稳态kalman滤波仿真通式')
subplot(2,2,2);plot(t,x(2,t),'b',t,xjian2(2,t),'r:');
figure(3); 
subplot(2,2,1);plot(t,x(1,t),'b',t,xjian3(1,t),'r:');
title('稳态kalman滤波仿真通式')
subplot(2,2,2);plot(t,x(2,t),'b',t,xjian3(2,t),'r:');
%--------------------fusion part-------------------------------------------
n=2;
P12(:,1:2)=zeros(n,n);P13(:,1:2)=zeros(n,n);P23(:,1:2)=zeros(n,n);
P21(:,1:2)=zeros(n,n);P31(:,1:2)=zeros(n,n);P32(:,1:2)=zeros(n,n);
P11(:,1:2)=zeros(n,n);P22(:,1:2)=zeros(n,n);P33(:,1:2)=zeros(n,n);
PJiZ(:,1:2)=zeros(n,n);
for i=2:Bushu;
   P11(:,n*(i-1)+1:n*(i-1)+2)=pusaif1*P11(:,n*(i-2)+1:n*(i-2)+2)*pusaif1'...
 +[eye(n)-kf1*H1]*gama*Q*gama'*[eye(n)-kf1*H1]'+kf1*R1*kf1';
   P22(:,n*(i-1)+1:n*(i-1)+2)=pusaif2*P22(:,n*(i-2)+1:n*(i-2)+2)*pusaif2'...
 +[eye(n)-kf2*H2]*gama*Q*gama'*[eye(n)-kf2*H2]'+kf2*R2*kf2';
   P33(:,n*(i-1)+1:n*(i-1)+2)=pusaif3*P33(:,n*(i-2)+1:n*(i-2)+2)*pusaif3'...
 +[eye(n)-kf3*H3]*gama*Q*gama'*[eye(n)-kf3*H3]'+kf3*R3*kf3';
   PJiZ(:,n*(i-1)+1:n*(i-1)+2)=pusaifJiZ*PJiZ(:,n*(i-2)+1:n*(i-2)+2)*...
 pusaifJiZ' +[eye(n)-kfJiZ*HJiZ]*gama*Q*gama'*[eye(n)-kfJiZ*HJiZ]'+kfJiZ*...
 RJiZ*kfJiZ';
   P12(:,n*(i-1)+1:n*(i-1)+2)=(eye(n)-kf1*H1)*[fai*P12(:,n*(i-2)+1:n*(i-2)...
  +2)*fai'+gama*Q*gama']*[eye(n)-kf2*H2]';
   P21(:,n*(i-1)+1:n*(i-1)+2)=(eye(n)-kf2*H2)*[fai*P21(:,n*(i-2)+1:n*(i-2)...
  +2)*fai'+gama*Q*gama']*[eye(n)-kf1*H1]';
   P13(:,n*(i-1)+1:n*(i-1)+2)=(eye(n)-kf1*H1)*[fai*P13(:,n*(i-2)+1:n*(i-2)...
  +2)*fai'+gama*Q*gama']*[eye(n)-kf3*H3]';
   P31(:,n*(i-1)+1:n*(i-1)+2)=(eye(n)-kf3*H3)*[fai*P31(:,n*(i-2)+1:n*(i-2)...
  +2)*fai'+gama*Q*gama']*[eye(n)-kf1*H1]';
   P23(:,n*(i-1)+1:n*(i-1)+2)=(eye(n)-kf2*H2)*[fai*P23(:,n*(i-2)+1:n*(i-2)...
  +2)*fai'+gama*Q*gama']*[eye(n)-kf3*H3]';
   P32(:,n*(i-1)+1:n*(i-1)+2)=(eye(n)-kf3*H3)*[fai*P32(:,n*(i-2)+1:n*(i-2)...
  +2)*fai'+gama*Q*gama']*[eye(n)-kf2*H2]';
end

Ps11=P11(:,Bushu-1:Bushu);Ps12=P12(:,Bushu-1:Bushu);
Ps13=P13(:,Bushu-1:Bushu);
Ps21=P21(:,Bushu-1:Bushu);Ps22=P22(:,Bushu-1:Bushu);
Ps23=P23(:,Bushu-1:Bushu);
Ps31=P31(:,Bushu-1:Bushu);Ps32=P32(:,Bushu-1:Bushu);
Ps33=P33(:,Bushu-1:Bushu);
JP11=trace(Ps11);JP22=trace(Ps22);JP33=trace(Ps33);
JJiZ=trace(PJiZ(:,Bushu-1:Bushu));
%-------------调用三种加权算法函数——————————————————————
[ABiaoL,ADiag,AMatrix,JiBiaoL,JiDiag,JiMatrix]=TS_JQuan(Ps11,Ps12,Ps13,Ps21,...
   Ps22,Ps23,Ps31,Ps32,Ps33);
%------------------------------------------------------------------------
for i=1:Bushu;
   xBiaoL(1,i)=ABiaoL(1,1)*xjian1(1,i)+ABiaoL(1,2)*xjian2(1,i)+ABiaoL(1,3)*...
    xjian3(1,i);
   xBiaoL(2,i)=ABiaoL(1,1)*xjian1(2,i)+ABiaoL(1,2)*xjian2(2,i)+ABiaoL(1,3)*...
    xjian3(2,i);
end
for i=1:Bushu;
    xDiag(1,i)=ADiag(1,1)*xjian1(1,i)+ADiag(1,2)*xjian2(1,i)+ADiag(1,3)*...
        xjian3(1,i);
    xDiag(2,i)=ADiag(2,1)*xjian1(2,i)+ADiag(2,2)*xjian2(2,i)+ADiag(2,3)*...
        xjian3(2,i);
end
for i=1:Bushu;
    xMatrix(:,i)=AMatrix(:,1:2)*xjian1(:,i)+AMatrix(:,3:4)*xjian2(:,i)+...
        AMatrix(:,5:6)*xjian3(:,i);
end
%--------------------------figure2 part-----------------------------------
figure(4);
subplot(2,2,1);plot(t,x(1,t),'b',t,xBiaoL(1,t),'r:');
title('三种加权算法仿真通式')
subplot(2,2,2);plot(t,x(2,t),'b',t,xBiaoL(2,t),'r:');
figure(5);
subplot(2,2,1);plot(t,x(1,t),'b',t,xDiag(1,t),'r:');
title('三种加权算法仿真通式')
subplot(2,2,2);plot(t,x(2,t),'b',t,xDiag(2,t),'r:');
figure(6);
subplot(2,2,1);plot(t,x(1,t),'b',t,xMatrix(1,t),'r:');
title('三种加权算法仿真通式')
subplot(2,2,2);plot(t,x(2,t),'b',t,xMatrix(2,t),'r:');
%-----------------下面开始计算误差平方--------------------------------------
ErrorPingF1(:,Bushu)=zeros(2,1);ErrorPingF2(:,Bushu)=zeros(2,1);
ErrorPingF3(:,Bushu)=zeros(2,1);ErrorBiaoL(:,Bushu)=zeros(2,1);
ErrorDiag(:,Bushu)=zeros(2,1);ErrorMatrix(:,Bushu)=zeros(2,1);
for i=2:Bushu;
    errorPingF1(1,i)=ErrorPingF1(1,i-1)+(x(1,i)-xjian1(1,i))^2;
    errorPingF1(2,i)=ErrorPingF1(2,i-1)+(x(2,i)-xjian1(2,i))^2;
    errorPingF2(1,i)=ErrorPingF2(1,i-1)+(x(1,i)-xjian2(1,i))^2;
    errorPingF2(2,i)=ErrorPingF2(2,i-1)+(x(2,i)-xjian2(2,i))^2;
    errorPingF3(1,i)=ErrorPingF3(1,i-1)+(x(1,i)-xjian3(1,i))^2;
    errorPingF3(2,i)=ErrorPingF3(2,i-1)+(x(2,i)-xjian3(2,i))^2;
    errorBiaoL(1,i)=ErrorBiaoL(1,i-1)+(x(1,i)-xBiaoL(1,i))^2;
    errorBiaoL(2,i)=ErrorBiaoL(2,i-1)+(x(2,i)-xBiaoL(2,i))^2;
    errorDiag(1,i)=ErrorDiag(1,i-1)+(x(1,i)-xDiag(1,i))^2;
    errorDiag(2,i)=ErrorDiag(2,i-1)+(x(2,i)-xDiag(2,i))^2;
    errorMatrix(1,i)=ErrorMatrix(1,i-1)+(x(1,i)-xMatrix(1,i))^2;
    errorMatrix(2,i)=ErrorMatrix(2,i-1)+(x(2,i)-xMatrix(2,i))^2;
end
%-------------------figure3 part------------------------------------------
figure(7);
subplot(2,2,1);
plot(t,ErrorPingF1(1,t),'r-',t,ErrorPingF2(1,t),'b--',t,ErrorPingF3(1,t),...
    'k-.',t,ErrorBiaoL(1,t),'r:',t,ErrorDiag(1,t),'k-',t,ErrorMatrix(1,t),...
'c:');
title('误差分析')
subplot(2,2,2);
plot(t,ErrorPingF1(2,t),'r-',t,ErrorPingF2(2,t),'b--',t,ErrorPingF3(2,t),...
    'k-.',t,ErrorBiaoL(2,t),'r:',t,ErrorDiag(2,t),'k-',t,ErrorMatrix(2,t),...
'c:')

%----------------------------------end------------------------------------