时变系统多传感器信息融合kalman滤波器

前面的博客中讲得都是针对一种传感器的测量值有状态空间模型对系统状态值进行估计，本次博客讨论多传感器下对系统状态值估计的方法，即多传感器的数据融合。多传感器的状态融合又分成两种：1.集中式kalman滤波；2.分布式卡尔曼滤波。集中式kalman滤波是通过将多个观测方程集中为一个观测方程，然后进行标准kalman滤波，该方法可以得到全局的最优估计，但是缺点也是很明显，多个观测方程集中为一个观测方程，必然导致系统观测维度的增加，维度增加大大增加了标准kalman滤波的计算量，同时将多个系统集中到一个系统中，会导致该系统的容错性差，容错性对于系统的重要性可能比导航精度更加重要，所以集中式kalman滤波适用范围较小；分布式kalman滤波是将多个传感器各自组成多个kalman滤波子系统，进行标准kalman滤波，最后根据最小二乘法对状态估计进行融合，得到融合结果。由于集中式kalman滤波适用范围较小，所以本博客主要介绍分布式kalman滤波。

• 三种加权多传感器最优信息融合

在线性最小方差意义下，加权融合存在三种加权方式：1.按矩阵加权；2.按标量加权；3.按对角线加权。这三种加权方式的优缺点主要从计算量和误差的角度进行分析。计算量：矩阵加权>按对角线加权>按标量加权;误差：按标量加权>按对角线加权>矩阵加权。这里我对上述三种加权方式给出理性的一些认识，矩阵加权说明权重是一个矩阵，既然是矩阵说明在计算这个权重矩阵时考虑到因素就多，对于融合后状态值也是多个状态值组合构成的，所以该方法计算出来的估计值估计误差肯定小；按照标量加权权重是一个标量值，考虑到的因素肯定少很多，所以估计误差会差很多；按对角线加权实际上也是标量加权，与标量加权不同的是标量加权对于所有的状态值估计权重是一样的，但是按照对角线加权对于多个状态值，每个状态值都有相应的权重，这样做肯定要比标量加权的效果要好，所以估计误差小于标量加权。下面对上述三种加权方式进行推导。

1. 按矩阵加权

假设有随机向量x，基于L个传感器观测已知它的无偏估计为：,且已知上述估计误差的方差阵和协方差矩阵，寻求x的矩阵加权融合估计,其表达式如下：

                                                                                                                              (1)

其中：

                                                                                                                                   (2)

下面是求权重矩阵Ai的过程：

                                                                             (3)

                                                                                                 (4)

则有：

（5）

这里不妨令：

（6）

（7）

则有：

           （8）

要求J的最小下，矩阵A的值，但是矩阵A存在约束条件式2，采用拉格朗日乘数法，建立辅助函数可得：

                               （9）

则有对矩阵A求微分，得：

（10）

对参数微分可得：

（11）

对式10和式11进行整理可得：

对于式10，存在：

（12）

其中：

（13）

则有：

（14）

对于式11，有：

（15）

联立式14和式15，则有：

（16）

解式16矩阵可得:

（17）

所以有：

（18）

下面来计算该权重下，状态估计的协方差矩阵，根据比较协方差矩阵的迹来比较误差的大小。

（19）

根据式19有：

（20）

将式18带入式20，可得：

（21）

则有：

（22）

这说明融合后的状态估计精度高宇每个局部估计精度。

• 2.按照标量加权融合

假设有随机向量x，基于L个传感器观测已知它的无偏估计为：,且已知上述估计误差的方差阵和协方差矩阵，寻求x的矩阵加权融合估计,其表达式如下：

                                                      （22）

其中有：

                                                               （23）

其中ai为标量，下面我们来求标量表达式。

线性最小方差意义下的极小化性能指标为：

                    （24）

其中有：

             （25）

带入式21，可得：

（26）

之后拉格朗日乘数法求最优解可得：

（27）

对向量a进行微分可得：

（28）

对参数微分可得：

（29）

式25和式26联立可得：

（30）

由此可得

（31）

同理这里求融合估计协方差：

则有：

（32）

可以证明：

（33）

这说明融合后的状态估计精度高宇每个局部估计精度。

• 按对角线加权最小方差最优融合

假设有随机向量x，基于L个传感器观测已知它的无偏估计为：,且已知上述估计误差的方差阵和协方差矩阵，寻求x的矩阵加权融合估计,其表达式如下：

                             （34）

则有：

（35）

线性最小方差意义下的极小化性能指标为：

（36）

其中：

（37）

（38）

其中为协方差矩阵P11的第i个对角线上的元素。

之后拉格朗日乘数法求最优解可得：

（39）

对参数ai微分可得：

（40）

对参数微分可得：

（41）

联立式40和式41可得：

（42）

由此可得：

（43）

下面来求协方差矩阵：

（44）

则有：

（45）

可以证明：

（46）

这说明融合后的状态估计精度高宇每个局部估计精度。

仿真测试：

考虑带有色观测噪声的3传感器时变系统

其中参数有：

                   

                                     

其中

,b(t)为bernoulli白噪声，仅取0或者1，取值概率为：，，g(t)是零均值，方差为0.03的高斯白噪声，分别为方差为0.001，0.002，0.003的高斯白噪声，仿真三种传感器融合结果。

仿真结果：

 

仿真代码如下：

clear;
clc;
N=300;
t=0:0.01:0.01*N;
Lamda=0.4;
Qg=0.03;
Qe1=0.001;
Qe2=0.002;
Qe3=0.003;
RandnumBt=rand(1,N);
for i=1:N
    if RandnumBt(i)<Lamda
       RandnumBt(i)=1; 
    else
       RandnumBt(i)=0; 
    end
end
RandNum=normrnd(0,sqrt(Qg),1,N);
for i=1:N
    k=1+0.1*cos(2*pi*t(i)/200);
    W(i)=k*RandNum(i)*RandnumBt(i);
    Qw(i)=k^2*Qg*Lamda;
end
RandNumE1=normrnd(0,sqrt(Qe1),1,N);
RandNumE2=normrnd(0,sqrt(Qe2),1,N);
RandNumE3=normrnd(0,sqrt(Qe3),1,N);
x(:,1)=[0;0];

v1(1)=RandNumE1(1);
v2(1)=RandNumE2(1);
v3(1)=RandNumE3(1);
z1(:,1)=v1(1);
z2(:,1)=v2(1);
z3(:,1)=v3(1);
A1=0.001+0.0005*cos(2*pi*t(1)/100);
A2=0.002+0.0005*cos(2*pi*t(1)/100);
A3=0.003+0.0005*cos(2*pi*t(1)/100);
A=[0.1 0.25;0.5+0.1*cos(2*pi*t(1)/200) 0];
F=[2*cos(2*pi*t(1)/200);1];
for i=2:N
    H=[0.3+0.1*cos(2*pi*t(i)/200) 0];
    x(:,i)=A*x(:,i-1)+F*W(i-1);
    %%测量系统1
    v1(i)=A1*v1(i-1)+RandNumE1(i-1);
    z1(:,i)=H*x(:,i)+v1(i);
    y1(:,i-1)=z1(:,i)-A1*z1(:,i-1);
    %%测量系统2
    v2(i)=A2*v2(i-1)+RandNumE2(i-1);
    z2(:,i)=H*x(:,i)+v2(i);
    y2(:,i-1)=z2(:,i)-A2*z2(:,i-1);
    %%测量系统3
    v3(i)=A3*v3(i-1)+RandNumE3(i-1);
    z3(:,i)=H*x(:,i)+v3(i);
    y3(:,i-1)=z3(:,i)-A3*z3(:,i-1);
    %%系统参数更新
    %%系统1
    Qv1(i-1)=H*F*Qw(i-1)*F'*H'+Qe1;
    S1(i-1)=Qw(i-1)*F'*H';
    Qw1(i-1)=Qw(i-1);
    %%系统2
    Qv2(i-1)=H*F*Qw(i-1)*F'*H'+Qe2;
    S2(i-1)=Qw(i-1)*F'*H';
    Qw2(i-1)=Qw(i-1);
    %%系统3
    Qv3(i-1)=H*F*Qw(i-1)*F'*H'+Qe3;
    S3(i-1)=Qw(i-1)*F'*H';
    Qw3(i-1)=Qw(i-1);
    A1=0.001+0.0005*cos(2*pi*t(i)/100);
    A2=0.002+0.0005*cos(2*pi*t(i)/100);
    A3=0.003+0.0005*cos(2*pi*t(i)/100);
    A=[0.1 0.25;0.5+0.1*cos(2*pi*t(i)/200) 0];
    F=[2*cos(2*pi*t(i)/200);1];
end
%%卡尔曼滤波
Xk1(:,1)=[0;0];
Pkk1=[Qg,0;0,Qe1];
Xk2(:,1)=[0;0];
Pkk2=[Qg,0;0,Qe2];
Xk3(:,1)=[0;0];
Pkk3=[Qg,0;0,Qe3];
I=eye(2);
error=zeros(6,N);
error1=zeros(6,N);
P12=[Qg,0;0,Qe2];
P13=[Qg,0;0,Qe3];
P21=[Qg,0;0,Qe2];
P23=[Qg,0;0,Qe1];
P31=[Qg,0;0,Qe3];
P32=[Qg,0;0,Qe1];
e=[1;1;1];
en=[I;I;I];
m=zeros(2,2);
for i=2:N
    H=[0.3+0.1*cos(2*pi*t(i-1)/200) 0];
    Hn=[0.3+0.1*cos(2*pi*t(i)/200) 0];
    A=[0.1 0.25;0.5+0.1*cos(2*pi*t(i)/200) 0];
    A1=0.001+0.0005*cos(2*pi*t(i)/100);
    A2=0.002+0.0005*cos(2*pi*t(i)/100);
    A3=0.003+0.0005*cos(2*pi*t(i)/100);
    %%系统1滤波
    H1=Hn*A-A1*H;
    Kf1(:,i-1)=Pkk1*H1'/(H1*Pkk1*H1'+Qv1(i-1));
    X1(:,i-1)=Xk1(:,i-1)+Kf1(:,i-1)*(y1(i-1)-H1*Xk1(:,i-1));
    P1=(I-Kf1(:,i-1)*H1)*Pkk1;
    Kp1(:,i-1)=(A*Pkk1*H1'+F*S1(i-1))/(H1*Pkk1*H1'+Qv1(i-1));
    Xk1(:,i)=A*Xk1(:,i-1)+Kp1(:,i-1)*(y1(i-1)-H1*Xk1(:,i-1));
    Pkk1=A*Pkk1*A'-Kp1(:,i-1)*(A*Pkk1*H1'+F*S1(i-1))'+F*Qw(i-1)*F';
    error(1,i)=error(1,i-1)+((X1(1,i-1)-x(1,i-1))^2-error(1,i-1))/i;
    error1(1,i)=error(1,i-1)+((X1(2,i-1)-x(2,i-1))^2-error(1,i-1))/i;
    %%系统2滤波
    H2=Hn*A-A2*H;
    Kf2(:,i-1)=Pkk2*H2'/(H2*Pkk2*H2'+Qv2(i-1));
    X2(:,i-1)=Xk2(:,i-1)+Kf2(:,i-1)*(y2(i-1)-H2*Xk2(:,i-1));
    P2=(I-Kf2(:,i-1)*H2)*Pkk2;
    Kp2(:,i-1)=(A*Pkk2*H2'+F*S2(i-1))/(H2*Pkk2*H2'+Qv2(i-1));
    Xk2(:,i)=A*Xk2(:,i-1)+Kp2(:,i-1)*(y2(i-1)-H2*Xk2(:,i-1));
    Pkk2=A*Pkk2*A'-Kp2(:,i-1)*(A*Pkk2*H2'+F*S2(i-1))'+F*Qw(i-1)*F';
    error(2,i)=error(2,i-1)+((X2(1,i-1)-x(1,i-1))^2-error(2,i-1))/i;
    error1(2,i)=error(2,i-1)+((X2(2,i-1)-x(2,i-1))^2-error(2,i-1))/i;
    %%系统3滤波
    H3=Hn*A-A3*H;
    Kf3(:,i-1)=Pkk3*H3'/(H3*Pkk3*H3'+Qv3(i-1));
    X3(:,i-1)=Xk3(:,i-1)+Kf3(:,i-1)*(y3(i-1)-H3*Xk3(:,i-1));
    P3=(I-Kf3(:,i-1)*H3)*Pkk3;
    Kp3(:,i-1)=(A*Pkk3*H3'+F*S3(i-1))/(H3*Pkk3*H3'+Qv3(i-1));
    Xk3(:,i)=A*Xk3(:,i-1)+Kp3(:,i-1)*(y3(i-1)-H2*Xk3(:,i-1));
    Pkk3=A*Pkk3*A'-Kp3(:,i-1)*(A*Pkk3*H3'+F*S3(i-1))'+F*Qw(i-1)*F';
    error(3,i)=error(3,i-1)+((X3(1,i-1)-x(1,i-1))^2-error(3,i-1))/i;
    error1(3,i)=error(3,i-1)+((X3(2,i-1)-x(2,i-1))^2-error(3,i-1))/i;
    %%三种导航融合
    Phip1=(I-Kf1(:,i-1)*H1)*A;
    Phip2=(I-Kf2(:,i-1)*H2)*A;
    Phip3=(I-Kf3(:,i-1)*H3)*A;
    P12=Phip1*P12*Phip2+(I-Kf1(:,i-1)*H1)*F*Qw(i-1)*F'*(I-Kf2(:,i-1)*H2)';
    P13=Phip1*P13*Phip3+(I-Kf1(:,i-1)*H1)*F*Qw(i-1)*F'*(I-Kf3(:,i-1)*H3)';
    P21=Phip2*P21*Phip1+(I-Kf2(:,i-1)*H2)*F*Qw(i-1)*F'*(I-Kf1(:,i-1)*H1)';
    P23=Phip2*P23*Phip3+(I-Kf2(:,i-1)*H2)*F*Qw(i-1)*F'*(I-Kf3(:,i-1)*H3)';
    P31=Phip3*P31*Phip1+(I-Kf3(:,i-1)*H3)*F*Qw(i-1)*F'*(I-Kf1(:,i-1)*H1)';
    P32=Phip3*P32*Phip2+(I-Kf3(:,i-1)*H3)*F*Qw(i-1)*F'*(I-Kf2(:,i-1)*H2)';
    %Ptr=[trace(P1) trace(P12) trace(P12);trace(P21) trace(P2) trace(P23);trace(P31) trace(P32) trace(P3)];
    Ptr=[trace(P1) 0 0;0 trace(P2) 0;0 0 trace(P3)];
    %%按照标量加权线性最小方差融合
    a=e'/ Ptr/(e'/Ptr*e);
    X4(:,i-1)=a(1)*X1(:,i-1)+a(2)*X2(:,i-1)+a(3)*X3(:,i-1);
    error(4,i)=error(4,i-1)+((X4(1,i-1)-x(1,i-1))^2-error(4,i-1))/i;
    error1(4,i)=error1(4,i-1)+((X4(2,i-1)-x(2,i-1))^2-error1(4,i-1))/i;
    %%按照对角阵加权最小方差融合
    Ptr1=[P1(1,1) 0 0;0 P2(1,1) 0;0 0 P3(1,1)];
    a1=e'/ Ptr1/(e'/Ptr1*e);
    X5(1,i-1)=a1(1)*X1(1,i-1)+a1(2)*X2(1,i-1)+a1(3)*X3(1,i-1);
    Ptr1=[P1(2,2) 0 0;0 P2(2,2) 0;0 0 P3(2,2)];
    a1=e'/ Ptr1/(e'/Ptr1*e);
    X5(2,i-1)=a1(1)*X1(2,i-1)+a1(2)*X2(2,i-1)+a1(3)*X3(2,i-1);
    error(5,i)=error(5,i-1)+((X5(1,i-1)-x(1,i-1))^2-error(5,i-1))/i;
    error1(5,i)=error1(5,i-1)+((X5(2,i-1)-x(2,i-1))^2-error1(5,i-1))/i;
    %%按照矩阵加权线性最小融合
%     Pn=[P1,m,m;m,P2,m;m,m,P3];
%     An=inv(en'/Pn*en)*en'/Pn
    Pa=inv(inv(P1)+inv(P2)+inv(P3));
    a1=Pa/P1;
    a2=Pa/P2;
    a3=Pa/P3;
   % X6(:,i-1)=An(1:2,1:2)*X1(:,i-1)+An(1:2,3:4)*X2(:,i-1)+An(1:2,5:6)*X3(:,i-1);
   X6(:,i-1)=a1*X1(:,i-1)+a2*X2(:,i-1)+a3*X3(:,i-1);
    error(6,i)=error(6,i-1)+((X6(1,i-1)-x(1,i-1))^2-error(6,i-1))/i;
    error1(6,i)=error1(6,i-1)+((X6(2,i-1)-x(2,i-1))^2-error1(6,i-1))/i;
end
t=1:N-1;
figure(1);
subplot(2,1,1);
plot(t,x(1,1:299),'r',t,X1(1,:),'b');
legend('系统真实状态值1','系统估计状态值1')
title('传感器1融合结果示意图');
subplot(2,1,2);
plot(t,x(2,1:299),'r',t,X1(2,:),'b');
legend('系统真实状态值2','系统估计状态值2')
title('传感器1融合结果示意图');
figure(2);
subplot(2,1,1);
plot(t,x(1,1:299),'r',t,X2(1,:),'b');
legend('系统真实状态值1','系统估计状态值1')
title('传感器2融合结果示意图');
subplot(2,1,2);
plot(t,x(2,1:299),'r',t,X2(2,:),'b');
legend('系统真实状态值2','系统估计状态值2')
title('传感器2融合结果示意图');
figure(3);
subplot(2,1,1);
plot(t,x(1,1:299),'r',t,X3(1,:),'b');
legend('系统真实状态值1','系统估计状态值1')
title('传感器3融合结果示意图');
subplot(2,1,2);
plot(t,x(2,1:299),'r',t,X3(2,:),'b');
legend('系统真实状态值2','系统估计状态值2')
title('传感器3融合结果示意图');
figure(4);
subplot(2,1,1);
plot(t,x(1,1:299),'r',t,X4(1,:),'b');
legend('系统真实状态值1','系统估计状态值1')
title('3传感器状态值1数据融合结果示意图');
subplot(2,1,2);
plot(t,x(2,1:299),'r',t,X4(2,:),'b');
legend('系统真实状态值2','系统估计状态值2')
title('3传感器状态值2数据融合结果示意图');
figure(5);
subplot(2,2,1);
plot(t,error(1,1:299),'r',t,error(2,1:299),'b',t,error(3,1:299),'g',t,error(4,1:299),'k',t,error(5,1:299),'m',t,error(6,1:299),'y');
legend('系统1误差','系统2误差','系统3误差','系统4误差','系统5误差','系统6误差');
title('状态值1误差示意图示意图');
subplot(2,2,2);
plot(t,error1(1,1:299),'r',t,error1(2,1:299),'b',t,error1(3,1:299),'g',t,error1(4,1:299),'k',t,error1(5,1:299),'m',t,error1(6,1:299),'y');
legend('系统1误差','系统2误差','系统3误差','系统4误差','系统5误差','系统6误差');
title('状态值2误差示意图示意图');
subplot(2,2,3);
plot(t,error(1,1:299),'r',t,error(4,1:299),'k',t,error(5,1:299),'b',t,error(6,1:299),'g');
legend('系统1误差','系统4误差','系统5误差','系统6误差');
title('状态值1误差示意图示意图');
subplot(2,2,4);
plot(t,error1(1,1:299),'r',t,error1(4,1:299),'k',t,error1(5,1:299),'b',t,error1(6,1:299),'g');
legend('系统1误差','系统4误差','系统5误差','系统6误差');
title('状态值2误差示意图示意图');