多级式多传感器信息融合中的状态估计（Matlab代码实现）

🍒🍒🍒欢迎关注🌈🌈🌈

📝个人主页：我爱Matlab

👍点赞➕评论➕收藏 == 养成习惯（一键三连）🌻🌻🌻

🍌希望大家多多支持🍓~一起加油 🤗

💬语录：将来的我一定会感谢现在奋斗的自己！

🍁🥬🕒摘要🕒🥬🍁

信息融合作为一门新兴交叉学科，其研究目的是如何通过综合多信息源的信息获得比依靠任何单个信息源更加准确和更加确定的估计与推理。目标跟踪是信息融合的底层关键技术，是实现高层次融合的前提,并在国防、民用多个领域有广泛的应用，也是本文的研究主题。

本文主要针对多传感器多时滞（包括状态之后和观测滞后）系统，基于Kalman滤波和现代时间序列分析方法，利用集中式融合估计、分布式融合估计（按矩阵加权、按对角阵加权、按标量加权）、 协方差交叉融合等方法实现对状态的融合估计。

✨🔎⚡运行结果⚡🔎✨

💂♨️👨‍🎓Matlab代码👨‍🎓♨️💂

%-----------------------------------------%
%多传感器多重观测滞后CI融合
%-----------------------------------------%
clc;clear all;
Bushu=300;
T=1.5;
Qw=2;
Rv1=1;Rv2=0.54;Rv3=2;
% N=200;
% for j=1:N
randn('seed',1);w=sqrt(Qw)*randn(1,Bushu+10);
randn('seed',1);v1=sqrt(Rv1)*randn(1,Bushu+10);
randn('seed',1);v2=sqrt(Rv2)*randn(1,Bushu+10);
randn('seed',5);v3=sqrt(Rv3)*randn(1,Bushu+10);
%--------------给出系统模型---------------%
fai=[1 T;0 1];gama=[0.5*T^2;T];H01=[1 0];H12=[0.4 0.8];H03=[1.5 0];H13=[0.6 1];
% fai=[1 1.2;0.3 1];gama=[1;3];H01=[1 0];H12=[0.4 1];H03=[1.5 0];H13=[0.6 1];
x(:,2)=[0;0];x(:,1)=[0;0];
y1(2)=H01*x(:,2)+v1(2);y2(2)=H12*x(:,1)+v2(2);y3(2)=H03*x(:,2)+H13*x(:,1)+v3(2);
for i=3:Bushu+10
x(:,i)=fai*x(:,i-1)+gama*w(i-1);
y1(i)=H01*x(:,i)+v1(i);y2(i)=H12*x(:,i-1)+v2(i);y3(:,i)=H03*x(:,i)+H13*x(:,i-1)+v3(i);
end
%------------------局部Kalman滤波器---------------%
%---------------------传感器1----------------%
PP1(:,:,1)=0.1*eye(2);P1(:,:,1)=0.1*eye(2);x1jian(:,1)=zeros(2,1);
for i=1:Bushu+5
PP1(:,:,i+1)=fai* P1(:,:,i)*fai'+gama*Qw*gama';%预报误差方差阵
Qeps1(i+1)=H01*PP1(:,:,i+1)*H01'+Rv1;
k1(:,i+1)=(PP1(:,:,i+1)*H01')*inv(Qeps1(i+1));
P1(:,:,i+1)=PP1(:,:,i+1)-k1(:,i+1)*Qeps1(i+1)*k1(:,i+1)';%滤波误差方差阵
x1jianp(:,i+1)=fai*x1jian(:,i);%预报
eps1(i+1)=y1(i+1)-H01*x1jianp(:,i+1);
x1jian(:,i+1)=x1jianp(:,i+1)+k1(:,i+1)*eps1(i+1);%滤波
end
% t=1:Bushu;
% figure
% subplot(2,2,1);plot(t,x(1,t),'b',t,x1jian(1,t),'r:');
% subplot(2,2,2);plot(t,x(2,t),'b',t,x1jian(2,t),'r:');

%-----------------传感器2----------------%
PP2(:,:,1)=0.1*eye(2);P2(:,:,1)=0.1*eye(2);x2jian(:,1)=zeros(2,1);
for i=1:Bushu+5
PP2(:,:,i+1)=fai* P2(:,:,i)*fai'+gama*Qw*gama';%预报误差方差阵
Qeps2(i+1)=H12*P2(:,:,i)*H12'+Rv2;
k2(:,i+1)=(fai*P2(:,:,i)*H12')*inv(Qeps2(i+1));
P2(:,:,i+1)=PP2(:,:,i+1)-k2(:,i+1)*Qeps2(i+1)*k2(:,i+1)';%滤波误差方差阵
x2jianp(:,i+1)=fai*x2jian(:,i);%预报
eps2(i+1)=y2(i+1)-H12*x2jian(:,i);
x2jian(:,i+1)=x2jianp(:,i+1)+k2(:,i+1)*eps2(i+1);%滤波
end
% t=1:Bushu;
% figure
% subplot(2,2,1);plot(t,x(1,t),'b',t,x2jian(1,t),'r:');
% subplot(2,2,2);plot(t,x(2,t),'b',t,x2jian(2,t),'r:');

%-----------------传感器3----------------%
PP3(:,:,1)=0.1*eye(2);P3(:,:,1)=0.1*eye(2);x3jian(:,1)=zeros(2,1);
for i=1:Bushu+5
PP3(:,:,i+1)=fai* P3(:,:,i)*fai'+gama*Qw*gama';%预报误差方差阵
Qeps3(i+1)=H03*PP3(:,:,i+1)*H03'+H13*P3(:,:,i)*H13'+H03*fai*P3(:,:,i)*H13'+H13*P3(:,:,i)*fai'*H03'+Rv3;
k3(:,i+1)=(PP3(:,:,i+1)*H03'+fai*P3(:,:,i)*H13')*inv(Qeps3(i+1));
P3(:,:,i+1)=PP3(:,:,i+1)-k3(:,i+1)*Qeps3(i+1)*k3(:,i+1)';%滤波误差方差阵
x3jianp(:,i+1)=fai*x3jian(:,i);%预报
eps3(i+1)=y3(i+1)-H03*x3jianp(:,i+1)-H13*x3jian(:,i);
x3jian(:,i+1)=x3jianp(:,i+1)+k3(:,i+1)*eps3(i+1);%滤波
end
% t=1:Bushu;
% figure
% subplot(2,2,1);plot(t,x(1,t),'b',t,x3jian(1,t),'r:');
% subplot(2,2,2);plot(t,x(2,t),'b',t,x3jian(2,t),'r:');

%-----------------迹----------------%
for i=1:Bushu+5
a(i)=trace(P1(:,:,i));
b(i)=trace(P2(:,:,i));
c(i)=trace(P3(:,:,i));
end

%-----------------滤波误差互协方差阵----------------%
P12(:,:,1)=eye(2);P13(:,:,1)=eye(2);P23(:,:,1)=eye(2);
for i=1:Bushu
PP12(:,:,i+1)=fai* P12(:,:,i)*fai'+gama*Qw*gama';%预报误差互协方差阵
PP13(:,:,i+1)=fai* P13(:,:,i)*fai'+gama*Qw*gama';
PP23(:,:,i+1)=fai* P23(:,:,i)*fai'+gama*Qw*gama';
%---滤波误差互协方差阵----%
P12(:,:,i+1)=[eye(2)-k1(:,i+1)*H01]*PP12(:,:,i+1)-[eye(2)-k1(:,i+1)*H01]*fai*P12(:,:,i)*H12'*k2(:,i+1)';P21(:,:,i+1)=P12(:,:,i+1)';
P13(:,:,i+1)=[eye(2)-k1(:,i+1)*H01]*PP13(:,:,i+1)*[eye(2)-k3(:,i+1)*H03]'-[eye(2)-k1(:,i+1)*H01]*fai*P13(:,:,i)*H13'*k3(:,i+1)';P31(:,:,i+1)=P13(:,:,i+1)';
P23(:,:,i+1)=PP23(:,:,i+1)*[eye(2)-k3(:,i+1)*H03]'+k1(:,i+1)*H12*P23(:,:,i)*H13'*k3(:,i+1)'-fai*P23(:,:,i)*H13'*k3(:,i+1)'-k2(:,i+1)*H12*P23(:,:,i)*fai'*[eye(2)-k3(:,i+1)*H03];P32(:,:,i+1)=P23(:,:,i+1)';
end

%-----------------按矩阵加权---------------%
for i=1:Bushu
Psigma(:,:,i)=[P1(:,:,i),P12(:,:,i),P13(:,:,i);
P12(:,:,i)',P2(:,:,i),P23(:,:,i);
P13(:,:,i)',P23(:,:,i)',P3(:,:,i)];
end
e=[eye(2),eye(2),eye(2)]';
for i=1:Bushu
A(:,:,i)=inv(Psigma(:,:,i))*e*inv(e'*inv(Psigma(:,:,i))*e);
Pf(:,:,i)=inv(e'*inv(Psigma(:,:,i))*e);%误差方差阵
xfjian(:,i)=A(1:2,:,i)'*x1jian(:,i)+A(3:4,:,i)'*x2jian(:,i)+A(5:6,:,i)'*x3jian(:,i);
end
% t=1:Bushu;
% figure
% subplot(2,2,1);plot(t,x(1,t),'b',t,xfjian(1,t),'r:');
% subplot(2,2,2);plot(t,x(2,t),'b',t,xfjian(2,t),'r:');
%-----------------exf---------------%
exf(300)=[0];
exf(1)=(xfjian(1,1)-x(1,1))*(xfjian(1,1)-x(1,1));
for i=2:Bushu
exf(i)=(xfjian(1,i)-x(1,i))*(xfjian(1,i)-x(1,i))+exf(i-1);
end
% t=1:Bushu;
% figure
% subplot(2,2,1);plot(t,exf(1,t),'b');
% subplot(2,2,2);plot(t,exf(2,t),'b');

%-----------------BCI----------------%
f=f_bcifun(w);
Aeq=[1 1 1];Beq=[1]; LB=[0 0 0]';UB=[1 1 1]';A=[];B=[];
w0=[0.4;0.4;0.2];
w=fmincon(@f_bcifun,w0,A,B,Aeq,Beq,LB,UB)
PBci=inv(w(1)*inv(P1(:,:,Bushu))+w(2)*inv(P2(:,:,Bushu))+w(3)*inv(P3(:,:,Bushu)));%CI融合估值误差方差阵
JBci=trace(PBci);%CI融合估值误差方差阵的迹
for i=1:Bushu
xBci(:,i)=PBci*(w(1)*inv(P1(:,:,Bushu))*x1jian(:,i)+w(2)*inv(P2(:,:,Bushu))*x2jian(:,i)+w(3)*inv(P3(:,:,Bushu))*x3jian(:,i));
end
% %-----------------BCI----------------%
% % PBci_=PBci*(w(1)*w(1)*inv(p11(:,:,Bushu))*p11(:,:,Bushu)*inv(p11(:,:,Bushu))+w(1)*w(2)*inv(p11(:,:,Bushu))*p12(:,:,Bushu)*inv(p22(:,:,Bushu))+...
% % w(1)*w(3)*inv(p11(:,:,Bushu))*p13(:,:,Bushu)*inv(p33(:,:,Bushu))+...
% % w(2)*w(1)*inv(p22(:,:,Bushu))*p12(:,:,Bushu)'*inv(p11(:,:,Bushu))+w(2)*w(2)*inv(p22(:,:,Bushu))*p22(:,:,Bushu)*inv(p22(:,:,Bushu))+...
% % w(2)*w(3)*inv(p22(:,:,Bushu))*p23(:,:,Bushu)*inv(p33(:,:,Bushu))+...
% % w(3)*w(1)*inv(p33(:,:,Bushu))*p13(:,:,Bushu)'*inv(p11(:,:,Bushu))+w(3)*w(2)*inv(p33(:,:,Bushu))*p23(:,:,Bushu)'*inv(p22(:,:,Bushu))+...
% % w(3)*w(3)*inv(p33(:,:,Bushu))*p33(:,:,Bushu)*inv(p33(:,:,Bushu)))*PBci;
% % JBci_=trace(PBci_);
% P1_ni=inv(P1(:,:,Bushu));P2_ni=inv(P2(:,:,Bushu));P3_ni=inv(P3(:,:,Bushu));
% PBci_ni=inv(PBci);%PBci_ni_=inv(PBci_);
% theta=0:pi/100:2*pi;
%-----------------SCI----------------%
deta=0.0001;pp1=P1(:,:,Bushu);pp2=P2(:,:,Bushu);pp3=P3(:,:,Bushu);
% [w12,Pci12]=y12_0618(pp1,pp2,deta)%P1和P2形成pci1
% [w123,Pci123,trPci123]=y123_0618(Pci12,pp3,deta)%P3和pci1融合成psci123
[w13,Pci13]=y13_0618(deta,pp1,pp3)%pp1和pp3形成pci1
[w132,Pci132,trPci132]=y132_0618(deta,Pci13,pp2)%pp2和pci1融合成psci132
for i=1:Bushu
% xci12(:,i)=Pci12*(w12*inv(P1(:,:,Bushu))*x1jian(:,i)+(1-w12)*inv(P2(:,:,Bushu))*x2jian(:,i));
% xci123(:,i)=Pci123*(w123*inv(Pci12)*xci12(:,i)+(1-w123)*inv(P3(:,:,Bushu))*x3jian(:,i));%xci123
xci13(:,i)=Pci13*(w13*inv(pp1)*x1jian(:,i)+(1-w13)*inv(pp3)*x3jian(:,i));
xci132(:,i)=Pci132*(w132*inv(Pci13)*xci13(:,i)+(1-w132)*inv(pp2)*x2jian(:,i));%xci132
end
t=1:Bushu;
figure
plot(t,x(1,t),'b',t,xci132(1,t),'r:');
figure
plot(t,x(2,t),'b',t,xci132(2,t),'r:');axis([0,Bushu,-45,20]);
%-----------------exci---------------%
exci(300)=[0];
exci(1)=(xci132(1,1)-x(1,1))*(xci132(1,1)-x(1,1));
for i=2:Bushu
exci(i)=(xci132(1,i)-x(1,i))*(xci132(1,i)-x(1,i))+exci(i-1);
end
t=1:Bushu;
figure
plot(t,exf(t),'b',t,exci(t),'r');axis([0,300,0,180]);

Pci132_=Pci132*(w132*w13*w132*w13*inv(pp1)*pp1*inv(pp1)+w132*w13*w132*(1-w13)*inv(pp1)*P13(:,:,Bushu)*inv(pp3)+...
w132*w13*(1-w132)*inv(pp1)*P12(:,:,Bushu)*inv(pp2)+w132*(1-w13)*w132*w13*inv(pp3)*P31(:,:,Bushu)*inv(pp1)+w132*(1-w13)*w132*(1-w13)*inv(pp3)+...
w132*(1-w13)*(1-w132)*inv(pp3)*P32(:,:,Bushu)*inv(pp2)+(1-w132)*w132*w13*inv(pp2)*P21(:,:,Bushu)*inv(pp1)+(1-w132)*w132*(1-w13)*inv(pp2)*P23(:,:,Bushu)*inv(pp3)+...
(1-w132)*(1-w132)*inv(pp2)*pp2*inv(pp2))*Pci132;
%-----------------椭圆半径----------------%
P1_ni=inv(P1(:,:,Bushu));P2_ni=inv(P2(:,:,Bushu));P3_ni=inv(P3(:,:,Bushu));
% Pci123_ni=inv(Pci123);
Pci132_ni=inv(Pci132);Pci132__ni=inv(Pci132_);
% PBci_ni=inv(PBci);%PBci_ni_=inv(PBci_);
theta=0:pi/100:2*pi;
r1=1./sqrt(P1_ni(1,1)*cos(theta).^2+(P1_ni(1,2)+P1_ni(2,1))*cos(theta).*sin(theta)+P1_ni(2,2)*sin(theta).^2);
r2=1./sqrt(P2_ni(1,1)*cos(theta).^2+(P2_ni(1,2)+P2_ni(2,1))*cos(theta).*sin(theta)+P2_ni(2,2)*sin(theta).^2);
r3=1./sqrt(P3_ni(1,1)*cos(theta).^2+(P3_ni(1,2)+P3_ni(2,1))*cos(theta).*sin(theta)+P3_ni(2,2)*sin(theta).^2);
% rBci=1./sqrt(PBci_ni(1,1)*cos(theta).^2+(PBci_ni(1,2)+PBci_ni(2,1))*cos(theta).*sin(theta)+PBci_ni(2,2)*sin(theta).^2);
% rBci_=1./sqrt(PBci_ni_(1,1)*cos(theta).^2+(PBci_ni_(1,2)+PBci_ni_(2,1))*cos(theta).*sin(theta)+PBci_ni_(2,2)*sin(theta).^2);
% rci123=1./sqrt(Pci123_ni(1,1)*cos(theta).^2+(Pci123_ni(1,2)+Pci123_ni(2,1))*cos(theta).*sin(theta)+Pci123_ni(2,2)*sin(theta).^2);
rci132=1./sqrt(Pci132_ni(1,1)*cos(theta).^2+(Pci132_ni(1,2)+Pci132_ni(2,1))*cos(theta).*sin(theta)+Pci132_ni(2,2)*sin(theta).^2);
rci132_=1./sqrt(Pci132__ni(1,1)*cos(theta).^2+(Pci132__ni(1,2)+Pci132__ni(2,1))*cos(theta).*sin(theta)+Pci132__ni(2,2)*sin(theta).^2);
%-----------------MSE曲线----------------%
% for i=1:Bushu
% ErroP1(j,i)=(x1jian(:,i)-x(:,i))'*(x1jian(:,i)-x(:,i));
% ErroP2(j,i)=(x2jian(:,i)-x(:,i))'*(x2jian(:,i)-x(:,i));
% ErroP3(j,i)=(x3jian(:,i)-x(:,i))'*(x3jian(:,i)-x(:,i));
% ErroPc(j,i)=(xcjian(:,i)-x(:,i))'*(xcjian(:,i)-x(:,i));
% ErroPm(j,i)=(xmjian(:,i)-x(:,i))'*(xmjian(:,i)-x(:,i));
% ErroPd(j,i)=(xdjian(:,i)-x(:,i))'*(xdjian(:,i)-x(:,i));
% ErroPs(j,i)=(xsjian(:,i)-x(:,i))'*(xsjian(:,i)-x(:,i));
% ErroPBci(j,i)=(xBci(:,i)-x(:,i))'*(xBci(:,i)-x(:,i));
% end
% % end
% MSE1=sum(ErroP1)/N;
% MSE2=sum(ErroP2)/N;
% MSE3=sum(ErroP3)/N;
% MSEc=sum(ErroPc)/N;
% MSEm=sum(ErroPm)/N;
% MSEd=sum(ErroPd)/N;
% MSEs=sum(ErroPs)/N;
% MSEci=sum(ErroPBci)/N;
%--------------作图----------------%
t=1:Bushu;
figure
hold on;
polar(theta,r1,'b');
polar(theta,r2,'k');
polar(theta,r3,'r');
polar(theta,rci132,'k');
polar(theta,rci132_,'k-.');axis([-2.3,2.3,-2.3,2.3]);
% polar(theta,rci123,'b-.');
% polar(theta,rBci,'b-.');
%polar(theta,rBci_,'b-.');% figure
% t=50:50:Bushu;
% plot(t,MSE1(t),'r-h',t,MSE2(t),'r-s',t,MSE3(t),'r-x',t,MSEc(t),'k-^',t,MSEm(t),'g-o',t,MSEd(t),'g-d',t,MSEs(t),'g-*',t,MSEci(t),'r-v');
% legend('MSE1','MSE2','MSE3','MSEc','MSEm','MSEd','MSEs','MSEci');
% hold on
% t=1:Bushu;
% line([50,Bushu],[a(Bushu),a(Bushu)]);line([50,Bushu],[b(Bushu),b(Bushu)]);line([50,Bushu],[c(Bushu),c(Bushu)]);
% line([50,Bushu],[jiPm(Bushu),jiPm(Bushu)]);line([50,Bushu],[jiPd(Bushu),jiPd(Bushu)]);line([50,Bushu],[jiPs(Bushu),jiPs(Bushu)]);
% line([50,Bushu],[JBci,JBci]);line([50,Bushu],[JBci_,JBci_]);