集中式观测融合EKF算法-圆形轨迹

集中式观测融合EKF(CMF-EKF)的思想是，由N个传感器对目标进行多方位观测，这些观测数据传入融合中心进行增广，最终得到融合估计。由于集中式观测融合是把所有传感器数据进行增广，因此融合结果具有全局最优性。

本文采用以下状态模型进行仿真：

其中

，，，。

为时刻在轴上的位置，为时刻在轴上的位置，为时刻的速度，为时刻的切线与轴夹角的角度，为时刻角速度。matlab程序和仿真图如下:

clc;clear;close all;
bushu=140;
T=0.1;
X=zeros(5,bushu);
X(:,1)=[0;3;1.45;-pi/2;0.4787];%x位置，y位置，速度，角度，角速度
Q=1e-3*diag([1,1]);
W=sqrtm(Q)*randn(2,bushu);
%% 模拟轨迹
for i=2:bushu
    x=X(1,i-1);
    y=X(2,i-1);
    v=X(3,i-1);
    zeita=X(4,i-1);
    w=X(5,i-1); 
    f(:,i)=[x+(v/w)*(sin(zeita+w*T)-sin(zeita));
            y+(v/w)*(-cos(zeita+w*T)+cos(zeita));
                                              v;
                                      zeita+w*T;
                                              w];
   gama=[0.5*(cos(zeita))*T^2,0;
         0.5*(sin(zeita))*T^2,0;
         T,0;
         0,0.5*T^2;
         0,T];
       X(:,i)=f(:,i)+gama*W(:,i);
end
%% UWB位置
x1=0;y1=0;
x2=10;y2=0;
x3=0;y3=10;

%% 距离
delat_w2=1e-1;
R=delat_w2*diag([0.1,0.2,0.1]);
v=sqrtm(R)*randn(3,bushu);
for i=1:bushu
    Z(1,i)=sqrt((X(1,i)-x1)^2+(X(2,i)-y1)^2)+v(1,i);
    Z(2,i)=sqrt((X(1,i)-x2)^2+(X(2,i)-y2)^2)+v(2,i);
    Z(3,i)=sqrt((X(1,i)-x3)^2+(X(2,i)-y3)^2)+v(3,i);   
end
%% 滤波
Xekf=zeros(5,bushu);
Xekf(:,1)=[0;3;1.45;-pi/2;0.4787];
Xjian=zeros(5,bushu);
P=eye(5);
for i=2:bushu
    x=Xekf(1,i-1);
    y=Xekf(2,i-1);
    v=Xekf(3,i-1);
    zeita=Xekf(4,i-1);
    w=Xekf(5,i-1); 
    f1=[x+(v/w)*(sin(zeita+w*T)-sin(zeita));
       y+(v/w)*(-cos(zeita+w*T)+cos(zeita));
       v;
       zeita+w*T;
       w];
   fai=[ 1, 0,  (sin(zeita + T*w) - sin(zeita))/w, (v*(cos(zeita + T*w) - cos(zeita)))/w, (T*v*cos(zeita + T*w))/w - (v*(sin(zeita + T*w) - sin(zeita)))/w^2;
         0, 1, -(cos(zeita + T*w) - cos(zeita))/w, (v*(sin(zeita + T*w) - sin(zeita)))/w, (v*(cos(zeita + T*w) - cos(zeita)))/w^2 + (T*v*sin(zeita + T*w))/w;
         0, 0,                                  1,                                     0,                                                                  0;
         0, 0,                                  0,                                     1,                                                                  T;
         0, 0,                                  0,                                     0,                                                                  1];
    gama1=[0.5*(cos(zeita))*T^2,0;
         0.5*(sin(zeita))*T^2,0;
         T,0;
         0,0.5*T^2;
         0,T];
    Xjian(:,i)=f1;
    P0=fai*P*fai'+gama1*Q*gama1';
    z1=sqrt((Xjian(1,i)-x1)^2+(Xjian(2,i)-y1)^2);
    z2=sqrt((Xjian(1,i)-x2)^2+(Xjian(2,i)-y2)^2);
    z3=sqrt((Xjian(1,i)-x3)^2+(Xjian(2,i)-y3)^2);
    z(:,i)=[z1;z2;z3];
    x=Xjian(1,i);
    y=Xjian(2,i);
    H=[                   x/(x^2 + y^2)^(1/2),                   y/(x^2 + y^2)^(1/2), 0, 0, 0;
        (2*x - 6)/(2*((x - 3)^2 + y^2)^(1/2)),             y/((x - 3)^2 + y^2)^(1/2), 0, 0, 0;
                    x/((y - 3)^2 + x^2)^(1/2), (2*y - 6)/(2*((y - 3)^2 + x^2)^(1/2)), 0, 0, 0];
    K=P0*H'*pinv(H*P0*H'+R);
    Xekf(:,i)=Xjian(:,i)+K*(Z(:,i)-z(:,i));
    P=(eye(5)-K*H)*P0;
end

figure(1)
t=1:bushu;
subplot(2,3,1);plot(X(1,:),X(2,:),'-ro',Xekf(1,:),Xekf(2,:),'-b.');legend('True','CMF-EKF');title('二维平面跟踪图');
subplot(2,3,2);plot(t,X(1,:),'-ro',t,Xekf(1,:),'-b.');legend('True','CMF-EKF');title('状态1跟踪图');
subplot(2,3,3);plot(t,X(2,:),'-ro',t,Xekf(2,:),'-b.');legend('True','CMF-EKF');title('状态2跟踪图');
subplot(2,3,4);plot(t,X(3,:),'-ro',t,Xekf(3,:),'-b.');legend('True','CMF-EKF');title('状态3跟踪图');
subplot(2,3,5);plot(t,X(4,:),'-ro',t,Xekf(4,:),'-b.');legend('True','CMF-EKF');title('状态4跟踪图');
subplot(2,3,6);plot(t,X(5,:),'-ro',t,Xekf(5,:),'-b.');legend('True','CMF-EKF');title('状态5跟踪图');