% Simulation of a Tracking Problem
% Constant Velocity Vc
% Linear Kalman Filter
% Author: Yu Guannan 22003205

clc
clear
close all

%-------------alterable initial values--------------------------------------
Xk=[-1000;1000;1;0];                               % initial  state vector
P=[0.1 0 0 0; 0 0.1 0 0;0 0 0.1 0;0 0 0 0.1];   % initila  covarian matrix
%-------------------------------------------------------------------------


T=240;                      % totle time

% State Model
% ------Begin-----
O=[1 0 1 0;0 1 0 1;0 0 1 0;0 0 0 1];                %
Xt=[0;10;1;0];     %
%Vc=[1;0;0;0]; 
W_std=0.01;                 %
Q=[W_std^4 0 0 0;0 W_std^4 0 0;0 0 W_std^4 0;0 0 0 W_std^4];    %
W=W_std*randn(4,T);         %
for i=2:T
    Xt(:,i)=O*Xt(:,i-1);%+Vc
end
X=Xt+W;
% ------End-------

% Measure Model
% ------Begin-----
H=[1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1];                %
V_std=0.1;                  %
R=[V_std^4 0 0 0;0 V_std^4 0 0;0 0 V_std^4 0;0 0 0 V_std^4];    %
V=V_std*randn(4,T);         %
Z=H*X+V;                    %
% ------End-------

% Kalman Filtering
% ------Begin-----
%Xk=[-10000;10000;1;1];                  % initial  state vector
%P=00.1*[0.1 0 0 0; 0 0.1 0 0;0 0 0.1 0;0 0 0 0.1];           % initila  covarian matrix
for i=1:T
    Xk1=O*Xk;%+Vc
    P1=O*P*O'+Q;
    K=P1*H'*inv(H*P1*H'+R);
    Xkf(:,i)=Xk1+K*(Z(:,i)-H*Xk1);
    Xk=Xkf(:,i);
    P=(eye(4)-K*H)*P1;
end
% ------End-------

figure();
plot(Xt(1,:),Xt(2,:),'r-',Z(1,:),Z(2,:),'g-',Xkf(1,:),Xkf(2,:),'k-');
legend('ideal location','position measurement','Location estimates');
xlabel('x(m)');
ylabel('y(m)');


figure();
plot([1:T],Xkf(1,:)-Xt(1,:),'g-',[1:T],Xkf(2,:)-Xt(2,:),'k-');
legend('X error','Y error');
xlabel('Time(s)');
ylabel('error(m)');