%卡爾曼濾波器的應用，運動軌跡估計
clc;
clear;
T=4;%雷達掃描周期
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;
vx=10;vy=0;
ax=0;ay=0;
var=100;
for i=1:N-1
    x(i+1)=x(i)+vx*T+0.5*ax*T^2;
    y(i+1)=y(i)+vy*T+0.5*ay*T^2;
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;
%濾波50次
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];
    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];
    %Kalman 濾波開始
    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);
        ykls(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方向平均誤差');