%function kalman
clear all;
close all;

% Defining system
A = [0 1 0 0; 0 0 1 0; 0 0 0 1;-10e-11 -24010 -2539 -54];
B = [0 0 0 31940]';
Cc = [1 0 0 0];
Dc = 0;
Ts = 0.01;
S = ss(A,B,Cc,Dc);
Pd = c2d(S,Ts);
[M,N,C,D]=ssdata(Pd);

% Depth of prediction
Tp = 100;
Nm = size(M,1);

Q=diag([0.01 0.01 0.1 0.1]);
R=0.05;

x = zeros(4,1);
xp = zeros(4,1);

P = Q;

for i=1:1:Tp
   v = 0.5*chol(Q)*randn(Nm,1)+0.1*sin(i/5);
%   v = chol(Q)*randn(Nm,1);
   e = 0.5*chol(R)*randn(1,1)+0.1*sin(i);
%   e = chol(R)*randn(1,1);
   u = 1;
   y = C*x+D*u+e;
%data krok
   yp = C*xp+D*u;
   eps = y - yp;
   L = P*C'*inv(C*P*C'+R);
   P = P - L*C*P;
   xp = xp + L*eps;
    
   Y(i,:) = y;
   Yp(i,:) = yp;
   X(i,:) = x';
   Xp(i,:) = xp';
   Lh(i,:) = L';
   
   x = M*x+N*u+v;
  
% cas krok
   xp = M*xp + N*u;
   P = M*P*M' + Q;   
end;

figure(1)
plot(Lh);
figure(2)
plot(1:Tp,Y,'b',1:Tp,Yp,'r');
figure(3)
plot(1:Tp,X,'b',1:Tp,Xp,'r');