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

 ‍个人主页：研学社的博客 

欢迎来到本博客❤️❤️

博主优势：博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

⛳️座右铭：行百里者，半于九十。

本文目录如下：

目录

1 概述

2 运行结果

3 参考文献

4 Matlab代码实现

---

1 概述

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

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

2 运行结果

部分代码：

%------------------局部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))*x3ji

3 参考文献

部分理论来源于网络，如有侵权请联系删除。

[1]乔向东. 信息融合系统中目标跟踪技术研究[D].西安电子科技大学,2003.

4 Matlab代码实现