sequentialfusion.m

基于matlab的几个kalman滤波以及多传感器融合和wk算法的程序

%序贯滤波
%雷达,红外传感器分别进行观测,然后各自向系统传递数据,雷达的采样周期为1秒,红外的采样周期为0.4秒即红外每隔0.4秒和0.8秒向系统传递数据.雷达
%的观测量是[距离 速度 方位角 俯仰角]',红外的观测量为[方位角 俯仰角]',状态向量为[X Vx Ux Y Vy Uy Z Vz
%Uz]'.假设目标的初始位置在(1000,2000,2000),目标的初始速度为Vx=250m/s, Vy=100m/s
% %信号模型：X(k+1)=C*X(k)+W(k) 其中C是时间t的函数  
%观测模型：Y(k)=T*H*X(k)+V(k)
clear
clc
%理想航迹的产生
length=50;
MeasureRadar=zeros(4,10*length);
MeasureInfrared=zeros(2,10*length);
IdealTrack=zeros(3,10*length);
X=zeros(9,10*length);
for k=1:1:10*length  
    n=k/10;
    Vx=250;
    x=1000+n*Vx;
    Vy=100;
    y=1000+n*Vy;
    Vz=0;
    z=2000+n*Vz;
    IdealTrack(:,k)=[x y z]';
    MeasureRadar(:,k)=[sqrt(x^2+y^2+z^2) sqrt(Vx^2+Vy^2+Vz^2) atan(x/y) atan(z/(sqrt(x^2+y^2)))]'+[100*randn(1) 10*randn(1) randn(1)*0.5/10000 randn(1)*0.5/10000]';
    MeasureInfrared(:,k)=[atan(x/y) atan(z/(sqrt(x^2+y^2)))]'+[ randn(1)*0.1/10000 randn(1)*0.1/10000]';
    figure(1);
    plot3(x,y,z,'r');
    hold on;
end
%初始值设定
Rradar=[10000 0 0 0
    0 5000 0 0 
    0  0 0.000025 0
    0 0 0 0.000025];
Rinfra=[0.000001 0
    0 0.000001];
Ti=[0 0 1 0
    0 0 0 1];
Tr=eye(4);
for k=1:3   %取前三个数作初始值
    R=MeasureRadar(1,k);
    V=MeasureRadar(2,k);
    B=MeasureRadar(3,k);
    E=MeasureRadar(4,k);
    x(k)=R*cos(E)*sin(B);
    y(k)=R*cos(E)*cos(B);
    z(k)=R*sin(E);
end
x0=x(3);
vx0=(x(3)-x(2))*10;
ux0=(x(3)+x(1)-2*x(2))*10;
y0=y(3);
vy0=(y(3)-y(2))*10;
uy0=(y(3)+y(1)-2*y(2))*10;
z0=z(3);
vz0=(z(3)-z(2))*10;
uz0=(z(3)+z(1)-2*z(2))*10;
X(:,1)=[x(1) (x(2)-x(1))*10 ux0 y(1) (y(2)-y(1))*10  uy0 z(1) (z(2)-z(1))*10 uz0]';
X(:,2)=[x(2) (x(3)-x(2))*10 ux0 y(2) (y(3)-y(2))*10  uy0 z(2) (z(3)-z(2))*10 uz0]';
FilterState=[x0 vx0  0 y0 vy0 0 z0 vz0 0]';
X(:,3)=FilterState;
FilterP=[10000 10000 0 0 0 0  0 0 0
    10000 20000 0 0 0 0 0 0 0 
    0 0 0 0 0 0 0 0 0
    0 0 0 10000 10000 0 0 0 0
    0 0 0 10000 20000 0 0 0 0
    0 0 0 0 0 0 0 0 0
    0 0 0 0 0 0 10000 10000 0
    0 0 0 0 0 0 10000 20000 0
    0 0 0 0 0 0 0 0 0];
%滤波
t0=3;
for i=4:1:10*length
   if mod(i/10,0.3)==0
        t=(i-t0)/10;
        C=[1 t 0.5*t^2 0 0 0 0 0 0 
            0 1 t 0 0 0 0 0 0 
            0 0 1 0 0 0 0 0 0
            0 0 0 1 t 0.5*t^2 0 0 0
            0 0 0 0 1 t 0 0 0
            0 0 0 0 0 1 0 0 0
            0 0 0 0 0 0 1 t 0.5*t^2
            0 0 0 0 0 0 0 1 t
            0 0 0 0 0 0 0 0 1];
        Q=[20/3*t^3 10*t^2 0 0 0 0 0 0 0
            10*t^2 20*t 0  0 0 0 0 0 0
            0 0 0 0 0 0 0 0 0
            0 0 0 20/3*t^3 10*t^2 0 0 0 0
            0 0 0 10*t^2 20*t 0 0 0 0
            0 0 0 0 0 0 0 0 0
            0 0 0 0 0 0 10/3*t^3 5*t^2 0
            0 0 0 0 0 0 5*t^2 10*t 0
            0 0 0 0 0 0 0 0 0];
        ForcastX=C*FilterState;
        ForcastP=C*FilterP*C'+Q;
        x=ForcastX(1);
        vx=ForcastX(2);
        y=ForcastX(4);
        vy=ForcastX(5);
        z=ForcastX(7);
        vz=ForcastX(8);
        d=sqrt(x^2+y^2+z^2);
        dxy=sqrt(x^2+y^2); 
        v=sqrt(vx^2+vy^2+vz^2);
       Hk=[x/d 0 0 y/d 0 0 z/d 0 0
           0 vx/v 0 0 vy/v 0 0 vz/v 0
           y/dxy^2 0 0 -x/dxy^2 0 0 0 0 0
           -x*z/(d^2*dxy) 0 0 -y*z/(d^2*dxy) 0 0 dxy/d^2 0 0];
       H=[d v atan(x/y) atan(z/dxy) ]';
       K=ForcastP*(Ti*Hk)'*inv((Ti*Hk)*ForcastP*(Ti*Hk)'+Rinfra);
       Z=MeasureInfrared(:,i);
      X(:,i)=ForcastX+K*(Z-Ti*H);
      FilterP=(eye(9)-K*Ti*Hk)*ForcastP;
      FilterState=X(:,i);
      t0=i;
      plot3(X(1,i),X(4,i),X(7,i));
  end
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  if mod(i/10,1)==0
        t=(i-t0)/10;
        C=[1 t 0.5*t^2 0 0 0 0 0 0 
            0 1 t 0 0 0 0 0 0 
            0 0 1 0 0 0 0 0 0
            0 0 0 1 t 0.5*t^2 0 0 0
            0 0 0 0 1 t 0 0 0
            0 0 0 0 0 1 0 0 0
            0 0 0 0 0 0 1 t 0.5*t^2
            0 0 0 0 0 0 0 1 t
            0 0 0 0 0 0 0 0 t];
         Q=[2/3*t^3 1*t^2 0 0 0 0 0 0 0
            1*t^2 2*t 0  0 0 0 0 0 0
            0 0 0 0 0 0 0 0 0
            0 0 0 2/3*t^3 1*t^2 0 0 0 0
            0 0 0 1*t^2 2*t 0 0 0 0
            0 0 0 0 0 0 0 0 0
            0 0 0 0 0 0 1/3*t^3 0.5*t^2 0
            0 0 0 0 0 0 0.5*t^2 1*t 0
            0 0 0 0 0 0 0 0 0];
        ForcastX=C*FilterState;
        ForcastP=C*FilterP*C'+Q;
        x=ForcastX(1);
        vx=ForcastX(2);
        y=ForcastX(4);
        vy=ForcastX(5);
        z=ForcastX(7);
        vz=ForcastX(8);
        d=sqrt(x^2+y^2+z^2);
        dxy=sqrt(x^2+y^2); 
        v=sqrt(vx^2+vy^2+vz^2);
       Hk=[x/d 0 0 y/d 0 0 z/d 0 0
           0 vx/v 0 0 vy/v 0 0 vz/v 0
           y/dxy^2 0 0 -x/dxy^2 0 0 0 0 0
           -x*z/(d^2*dxy) 0 0 -y*z/(d^2*dxy) 0 0 dxy/d^2 0 0];
       H=[d v atan(x/y) atan(z/dxy) ]';
       K=ForcastP*(Tr*Hk)'*inv((Tr*Hk)*ForcastP*(Tr*Hk)'+Rradar);
       Z=MeasureRadar(:,i);
      X(:,i)=ForcastX+K*(Z-H);
      FilterP=(eye(9)-K*Tr*Hk)*ForcastP;
      FilterState=X(:,i);
      t0=i;
      plot3(X(1,i),X(4,i),X(7,i));
  end
end
hold off;