function KalmanVectorStateVectorObservation ()

varu = 25; %9; % 4
A = [1 0 1 0; 0 1 0 1; 0 0 1 0; 0 0 0 1];
Q = [0 0 0 0; 0 0 0 0; 0 0 varu 0; 0 0 0 varu];       
ms = [0; 0; 0; 0];        
Cs = 100*eye(4);
H = [1 0 0 0; 0 1 0 0];
varn = 100^2;           

figure(1);clf;hold on;

% generate initial conditions
s = ms + sqrt(Cs)*randn(4,1)

% compute initial estimator
sCorrected = ms;
MSE = Cs;

n = 1;
while n<200
    [s x] = GenerateNextStateAndObservation (s,A,Q,H,varn);
    
    % prediction
    sPredict = A*sCorrected;
    % min prediction MSE
    pMSE = A*MSE*A' + Q;
    % Kalman gain
    K = pMSE*H'/(varn*eye(2) + H*pMSE*H');
    % correction
    sCorrected = sPredict + K*(x - H*sPredict);
    % min MSE
    MSE = (eye(4) - K*H)*pMSE;
    
    % plotting
    plot(x(1),x(2),'k.');plot(s(1),s(2),'g.');
    plot(sCorrected(1),sCorrected(2),'r.');
    pause(0.1);
%     if n==100
%         s(2) = -5*s(2);
%     end;
end;


function [s, x] = GenerateNextStateAndObservation (s, A, Q, H, varn)
s = A*s + sqrt(Q)*randn(4,1);
x = H*s + sqrt(varn)*randn(2,1);