clc;
clear all;
close all;
T=2;
A=[1 T 0 0;0 1 0 0;0 0 1 T;0 0 0 1];
G=[T 0;1 0;0 T;0 1];
H=[1 0 0 0;0 0 1 0];
vx=-15;
vy=-15;
t=400;
alldot=t/T;
i=1:t/T;
rx=2000+vx*i*T;
ry=10000+vy*i*T;
q=1;
Q=[q 0;0 q];
r=10;
R=[r,0; 0,r];
XX=zeros(4,200);
XX(:,1)=[1970;-15;9970;-15];
for j=1:199
XX(:,j+1)=A*XX(:,j)+G*[q*randn(1,1);q*randn(1,1)];
end
for k=1:200
srx(k)=XX(1,k);
sry(k)=XX(3,k); 
end
grx=srx+randn(1,200)*200;
gry=sry+randn(1,200)*200;
Z=[grx;gry];
PP=eye(4);
for k=1:200
    lrx(k)=XX(1,k);
    lry(k)=XX(3,k);   
    X=A*XX(:,k);
    P=A*PP*A'+G*Q*G';
    K=P*H'*inv(H*P*H'+R);
    wk(:,k)=K(:,1);
    XX(:,k)=X+K*[Z(:,k)-H*X];
    PP=[eye(4)-K*H]*P;
end
    figure;plot(rx,ry,'r-',lrx,lry,'k-',grx,gry,'g');
    figure;plot(wk(1,:));
    figure;plot(wk(2,:));
    figure;plot(srx-XX(1,:));hold on; plot(srx-grx,'g');