function [xk, Pk] = KalmanTrack(x, P)

%Model
Phi = eye(4); Phi(1,3) = 1; Phi(2,4) = 1;
H =eye(2,4);
%Noise
Q = eye(4);
R = eye(2);
%% Prediction
%State prediction
xk = Phi*x;

%Observation model
zk = H*x;

%Error covariance for the predicted estimate
Pk = Phi*P*Phi'+Q;
%% Update
%optimal Kalman gain
K = Pk*H'*inv(H*Pk*H'+R)

%Residual
xk = Phi*x+K*(zk - H*Phi*x);
disp(K*(zk - H*Phi*x));

%Updating the error covariance
Pk = (eye(4) - K*H)*Pk;