kalman机动.m

卡尔曼机动

%**************主程序*********************%
clear;
T=2;
num=50;
%真实轨迹
N1=400/T; N2=600/T; N3=610/T;N4=660/T;N5=900/T;
x=zeros(N5,1);
y=zeros(N5,1);
vx=zeros(N5,1);
vy=zeros(N5,1);
x(1)=2000;
y(1)=10000;
vx(1)=0;
vy(1)=-15;
ax=0;
ay=0;
var=100;
for i=1:N5-1
    if(i>N1-1&i<=N2-1)
        ax=0.075;
        ay=0.075;
        vx(i+1)=vx(i)+0.075*T;
        vy(i+1)=vy(i)+0.075*T;
    elseif(i>N2-1&i<=N3-1)
        ax=0;
        ay=0;
        vx(i+1)=vx(i);
        vy(i+1)=vy(i);
    elseif(i>N3-1&i<=N4-1)
        ax=-0.3;
        ay=-0.3;
        vx(i+1)=vx(i)-0.3*T;
        vy(i+1)=vy(i)-0.3*T;
    else
        ax=0;
        ay=0;
        vx(i+1)=vx(i);
        vy(i+1)=vy(i);
    end
    x(i+1)=x(i)+vx(i)*T+0.5*ax*T^2;%真实轨迹
    y(i+1)=y(i)+vy(i)*T+0.5*ay*T^2;
end
rex=num:N5;
rey=num:N5;
for m=1:num
    %噪声
    nx=randn(N5,1)*100;
    ny=randn(N5,1)*100;
    zx=x+nx;
    zy=y+ny;
 %kalman滤波初始化
 rex(m,1)=2000;
 rey(m,1)=10000;
 ki=0;
 low=1;high=0;
 u=0;ua=0;
 e=0.8;
 z=2:1;
 xks(1)=zx(1);
 yks(1)=zy(1);
 xks(2)=zx(2);
 yks(2)=zy(2);
 o=4:4;g=4:2;c=2:4;q=2:2;xk=4:1;perr=4:4;
 o=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];
 g=[(T^2)/2,0;T,0;0,(T^2)/2;0,T];
 h=[1,0,0,0;0,0,1,0];
 q=[10000 0;0 10000];
 perr=[var^2,var^2/T,0,0;
       var^2/T,2*var^2/(T^2),0,0;
       0,0,var^2,var^2/T;
       0,0,var^2/T,2*var^2/(T^2)];
   vx=(zx(2)-zx(1))/2;
   vy=(zy(2)-zy(1))/2;
   xk=[zx(1);vx;zy(1);vy];
   %kalman滤波开始
 for n=3:N5;
   if(u<=40)  %非机动模型、赋初值
     if(low==0)
          [o,h,g,q,perr,xk]=lmode_initial(T,n,zx,zy,vkxs,vkys,perr2);
          z=2:2;
          high=0;
          low=1;
          ua=0;
     end
     z=[zx(n);zy(n)];
     xkl=o*xk;
     perr1=o*perr*o';
     k=perr1*h'*inv(h*perr1*h'+q);
     xk=xkl+k*(z-h*xkl);
     perr=(eye(4)-k*h)*perr1;
     xks(n)=xk(1,1);
     yks(n)=xk(3,1);
     vkxs(n)=xk(2,1);
     vkys(n)=xk(4,1);
     xkls(n)=xkl(1,1);
     ykls(n)=xkl(3,1);
     perr11(n)=perr(1,1);
     perr12(n)=perr(1,2);
     perr22(n)=perr(2,2);
    if(n>=20)
        v=z-h*xkl;
        w=h*perr*h'+q;
        p=v'*inv(w)*v;
        u=e*u+p;
        s(n-19)=u;
    end
elseif(u>40)  %启动机动检测
    if(high==0)
        [o,g,h,q,perr,xk]=hmode_initial(T,n,e,zx,zy,xkls,ykls,vkxs,vkys,perr11,perr12,perr22);
        high=1;
        low=0;
        for i=n-5:n-1
            z=[zx(i);zy(i)];
            xkl=o*xk;
            perr1=o*perr*o';
            k=perr1*h'*inv(h*perr1*h'+q);
            xk=xkl+k*(z-h*xkl);
            perr=(eye(6)-k*h)*perr1;
            xks(n)=xk(1,1);
            yks(n)=xk(3,1);
            vkxs(n)=xk(2,1);
            vkys(n)=xk(4,1);
            xkls(n)=xkl(1,1);
            ykls(n)=xkl(3,1);
        end
    end
    z=[zx(n);zy(n)];
    xkl=o*xk;
    perr1=o*perr*o';
    k=perr1*h'*inv(h*perr1*h'+q);
    xk=xkl+k*(z-h*xkl);
    perr=(eye(6)-k*h)*perr1;
    xks(n)=xk(1,1);
    yks(n)=xk(3,1);
    vkxs(n)=xk(2,1);
    vkys(n)=xk(4,1);
    xkls(n)=xkl(1,1);
    ykls(n)=xkl(3,1);
    ag=[xk(5,1);xk(6,1)];
    perr2=perr;
    ki=ki+1;
    pm=[perr(5,5) perr(5,6);perr(6,5) perr(6,6)];
    pa=ag'*inv(pm)*ag;
    sa(n)=pa;
    if(ki>5) %退出机动判断
        ul=sa(n-4)+sa(n-3)+sa(n-2)+sa(n-1)+sa(n);
        sb(n)=ul;
        if(ul<20)
            u=0;
        end
    end
end
     rex(m,n)=xks(n); %将滤波值存入数组
     rey(m,n)=yks(n);
 end
end
%计算滤波误差的均值以及标准差
ex=0;ey=0;
eqx=0;eqy=0;
ey1=0;
ex1=N5:1;ey1=N5:1;
qx=N5:1;qy=N5:1;
for i=1:N5
    for j=1:num
        ex=ex+x(i)-rex(j,i);
        ey=ey+y(i)-rey(j,i);
        eqx=eqx+(x(i)-rex(j,i))^2;
        eqy=eqy+(y(i)-rey(j,i))^2;
    end
    ex1(i)=ex/num;
    ey1(i)=ey/num;
    qx(i)=(eqx/num-(ex1(i)^2))^0.5;
    qy(i)=(eqy/num-(ey1(i)^2))^0.5;
    ex=0;ey=0;eqx=0;eqy=0;
end
%绘图
figure(1);
plot(x,y,'k-',zx,zy,'b:',xks,yks,'r-.');
legend('真实轨迹','观测样本','滤波数据');
figure(2);
plot(zx,zy,'b:');
legend('观测样本');
figure(3);
plot(xks,yks);
legend('滤波数据');
figure(4);
%subplot(2,1,1);
plot(ex1);
legend('x方向滤波误差均值');
%subplot(2,1,2);
figure(5);
plot(qx);
legend('x方向滤波误差标准差');

%********非机动模型初始化*******%
function[o,h,g,q,perr,xk]=lmode_initial(T,n,zx,zy,vkxs,vkys,perr2)  
o=4:4;
g=4:2;
h=2:4;
q=2:2;
xk=4:1;
perr=4:4;
o=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];
h=[1 0 0 0;0 0 1 0];
g=[(T^2)/2,0;T,0;0,(T^2)/2;0,T];
xk=[zx(n-1);vkxs(n-1);zy(n-1);vkys(n-1)];
perr=perr2(1:4,1:4);

%*********机动模型初始化***********%
function[o,g,h,q,perr,xk]=hmode_initial(T,n,e,zx,zy,xkls,ykls,vkxs,vkys,perr11,perr12,perr22)
o=6:6;
g=6:2;
h=2:6;
q=2:2;
xk=6:1;
perr=6:6;
o=[1 T 0 0 0.5*T^2 0;0 1 0 0 T 0;0 0 1 T 0 0.5*T^2;0 0 0 1 0 T;0 0 0 0 1 0;0 0 0 0 0 1];
h=[1 0 0 0 0 0;0 0 1 0 0 0];
g=[T^2/4,0;T/2,0;0,T^2/4;0,T/2;1,0;0,1];
q=[10000 0;0 10000];
jsx=0.5*(zx(n-1/(1-e))-xkls(n-1/(1-e)));
jsy=0.5*(zy(n-1/(1-e))-ykls(n-1/(1-e)));
vx=vkxs(n-1/(1-e)-1)+2*jsx;
vy=vkys(n-1/(1-e)-1)+2*jsy;
xk=[zx(n-5);vx;zy(n-5);vy;jsx;jsy];
p111=10000;p112=10000;p115=5000;
p155=0.25*(10000+perr11(n-6)+4*perr12(n-6)+4*perr22(n-6));
p122=10000+perr11(n-6)+perr22(n-6)+2*perr12(n-6);
p125=5000+0.5*perr11(n-6)+perr22(n-6)+1.5*perr12(n-6);
perr=[p111 p112 0 0 p115 0;0 p122 0 0 p125 0;0 0 100 0 0 0;0 0 0 100 0 0;0 0 0 0 p155 0;0 0 0 0 0 100];