p184.m

卡尔曼在目标跟踪中的应用本文对作匀速运动的二维空中机动目标进行研究

%例8.1p183
clc;
clear;
T=2;
num=100;
%真实轨迹
N=800/T;
x=zeros(N,1);y=zeros(N,1);
vx=zeros(N,1);vy=zeros(N,1);
x(1)=-2000;
y(1)=500;
for j=1:N
    vx(j)=10;
end
%vx=10;vy=0;
%ax=0;ay=0;
var=100;

for i=1:N-1;
    x(i+1)=x(i)+vx(i)*T;%+0.5*T^2*ax;
    y(i+1)=y(i)+vy(i)*T;%+0.5*T^2*ay;
end
nx=zeros(N,1);ny=zeros(N,1);
nx=100*randn(N,1);
ny=100*randn(N,1);
zx=x+nx;zy=y+ny;
%滤波
for m=1:num
    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;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,0;T/2,0;0, T/2;0, T/2;0,T/2];
    q=[10000 0;0 10000];
    perr=[var^2 ,var^2/T, 0, 0; var*var/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];
    %卡尔曼滤波开始
    for r=3:N;
        z=[zx(r);zy(r)];
        xk1=o*xk;
        perr1=o*perr*o';
        k=perr1*h'*inv(h*perr1*h'+q);
        xk=xk1+k*(z-h*xk1);
        perr=(eye(4)-k*h)*perr1;
        xks(r)=xk(1,1);
        yks(r)=xk(3,1);
        vkxs(r)=xk(2,1);
        vkys(r)=xk(4,1);
        xk1s(r)=xk1(1,1);
        yk1s(r)=xk1(3,1);
        perr11(r)=perr(1,1);
        perr12(r)=perr(1,2);
        perr22(r)=perr(2,2);
        rex(m,r)=xks(r);
        rey(m,r)=yks(r);
    end
end
    ex=0;ey=0;
    eqx=0;eqy=0;
    ey1=0;
    ex1=N:1;ey1=N:1;
    
    for i=1:N
        for j=1:num
            ex=ex+x(i)-rex(j,i);
            ey=ey+y(i)-rey(j,i);
        end
        ex1(i)=ex/num;
        ey1(i)=ey/num;
        ex=0;eqx=0;ey=0;eqy=0;
    end
    %绘图
    figure(1);
    plot(x,y,'k-',zx,zy,'g:',xks,yks,'r-.');
    legend('真实轨迹','观测样本','估计轨迹');
    figure(2);
    plot(ey1);
    legend('x方向上的误差');