clear;
close all;

A=[0 1 0 0;0 0 1 0;0 0 0 1;1e-11 -24010 -2539 -54];
B=[0;0;0;31940];
C=[1 0 0 0];
D=0;

[M,N,C,D]=ssdata(c2d(ss(A,B,C,D),0.01,'zoh'));

Q=diag([0.01 0.01 0.1 0.1]);
R=0.05;
x=[0;0;0;0];
xp=x;
Ns=4;
P=Q;

for i=1:2000,
   v=chol(Q)*randn(Ns,1);
   e=chol(R)*randn(1,1);
   u=1;
   y=C*x+D*u;
   
   %datovy tok
   yp=C*xp+D*u;
   eps=y-yp;
   
   L=P*C'*inv(C*P*C'+R);
   P=P-L*C*P;
   
   %ulozeni historie
   xp=xp+L*eps;
   xphist(:,i)=xp;
   yphist(i)=yp;
   xhist(:,i)=x;
   yhist(i)=y;
   Lhist(:,i)=L;
   
   %casovy tok
   x=M*x+N*u+v;
   xp=M*xp+N*u;
   
      
   Pc=M*P*M'+Q;
end;
 plot(Lhist');