%KALMAN_TRACKING
A = eye(2,2)
B(2,2) = 0;
G = eye(2,2);
C = eye(2,2);
D(2,2) = 0;
H(2,2) = 0;
R = 0.1*eye(2,2);
Q = 0.1*R;

%for discrete set sample time to -1 to model as discrete
%SYS_TEM = ss( A,[B G],C,[D H],-1)
%[kest,K,P,M,Z] = kalman(SYS_TEM,Q,R,0)
P0 = [8 0;0 4]
X0 = [181 80]'

Z=[181 80;181	80;181	80;181	80;181	80;181	80;181	80;181	80;181	80;181	80;181	80;181	80;181	80;181	80;181	80;181	80;181	80;181	80;
181	80;181	80;181	80;181	80;181	80;180	80;178	80;176	80;174	80;174	80;174	80;174	80;174	80;174	80;174	80;174	80;174	80;174	80;
174	80;174	80;174	80;174	80;174	80;174	80;174	80;174	80;174	80;174	80;174	80;174	80;174	80;174	80;173	79;172	79;170	79;167	78;
164	77;165	77;161	77;160	77;160	77;160	77;160	77;160	77;160	77;160	77;160	77;160	77;160	77;160	77;160	77;160	77;160	77;160	77;
160	77;160	77;160	77;160	77;160	77;160	77;160	77;160	77;160	77;160	77;160	77;160	77;160	77;160	77;160	77;160	76;161	77;162	78;
163	79;163	79;164	79;165	80;165	81;166	81;167	82;168	83;169	83;169	84;170	85;173	87;176	90;178	91;180	92;183	94]


Zt = Z
for k=1:1:10
    k
    if k==1
        XDashk = A*X0;
    else
        XDashk = A*Xk;
    end
    if k==1
        PDashk = A*P0*A'+ Q;
    else
        PDashk = A*Pk*A'+ Q;
    end
    Kk = PDashk*G'*(inv(G*PDashk*G'+R))
    Xk = XDashk + Kk*(Zt(k,1:2)' - XDashk)
    % no difference when using any of the below equation
    Pk = (eye(2,2)-Kk*G)*PDashk
    %Pk = (eye(2,2)-Kk)*PDashk*(eye(2,2)-Kk)' + Kk*R*Kk'
end