delT = 1;
F = [ 1 delT
      0   1  ];
H = [ 1 0 ];
Q = [  1  1
       1  1 ];
R = [ 1 ];
z = [0.0 2.5 1 4 2.5 5.5 ];

x = zeros(2,6);
P = zeros(2,2,6);

x(:,1) = [ 0
           10];
P(:,:,1) = [ 10  0
              0 10 ];

for i=2:6
   [xpred, Ppred] = predict(x(:,i-1), P(:,:,i-1), F, Q);
   [nu, S] = innovation(xpred, Ppred, z(i), H, R);
   [x(:,i), P(:,:,i)] = innovation_update(xpred, Ppred, nu, S, H);
end

plot(z(2:6),'b');
hold on;
plot(x(1,2:6),'r');
axis([0 5 0 6]);
