% Vstupni hodnoty
% Soustava
close all;
clear all;
As=[0 1 0 0;
   0 0 1 0;
   0 0 0 1;
   1e-11 -24010 -2539 -54];
Bs=[0;0;0;31940];
Cs=[1 0 0 0] ;
Ds=0;
%perioda vzorkovani
Td=0.01;
%Parametry sumu
Q=[0.01 0 0 0;
   0 0.01 0 0;
   0 0 0.1 0;
   0 0 0 0.1];
R=0.05;
%
x(:,1) = [0;0;0;0]; % pocatecni stav
P=[1 0 0 0;
   0 1 0 0;
   0 0 1 0;
	0 0 0 1];
odhadx(:,1) = [0;0;0;0]; % pocatecni odhad

[Ad, Bd, Cd, Dd] = ssdata(c2d(ss(As,Bs,Cs,Ds),Td))

F=[1 1 1 1];

for i = 1:1:100
   u(i) = F*odhadx(:,i)
   x(:,i+1)=Ad*x(:,i)+Bd*u(i)+Q*randn(4,1);
   y(:,i)= Cd*x(:,i)+Dd*u(i)+R*randn(1,1);
   
   K=P*Cd'*(Cd*P*Cd'+R)^-1;
   odhadx(:,i)=odhadx(:,i)+K*(y(:,i)-Cd*odhadx(:,i)-Dd*u(i));
   P=P-P*Cd'*((Cd*P*Cd'+R)^-1)*Cd*P;
   odhadx(:,i+1)=Ad*odhadx(:,i)+Dd*u(i);
   P=Ad*P*Ad'+Q;
end
figure(1);
plot(odhadx(1,:));
hold on;
plot(x(1,:),'r');
figure(2);
plot(odhadx(2,:));
hold on;
plot(x(2,:),'r');
figure(3);
plot(odhadx(3,:));
hold on;
plot(x(3,:),'r');
figure(4);
plot(odhadx(4,:));
hold on;
plot(x(4,:),'r');
%sum=Q*randn(4,1)