close all;
% regulator
AA=[0 1 0 0 ;
   0 0 1 0 ;
   0 0 0 1;
   (-10^-11) -24010 -2539 -54]
BB = [0;0;0;31940]
CC = [1 0 0 0;
   0 1 0 0;
   0 0 1 0;
   0 0 0 1]
DD = [0;0;0;0]

%Casovy horizont
N=500;

R = 1000;

Q= [1000 0 0 0;
   0 1 0 0 ;
   0 0 1 0;
   0 0 0 1];
sys = ss(AA,BB,CC,DD) 
pole(sys)
dsys = c2d(sys, 0.01, 'zoh')
[a,b,c,d]=ssdata(dsys)

P=Q;
vlcis(:,1) = eig(a-b*K);
for i = 2 : N,
%   j=N-i;
   Pom = a'*P*a-a'*P*b*((R+b'*P*b)^-1)*b'*P*a+Q;
   P = Pom;
   K=((R+b'*P*b)^-1)*b'*P*a;
   vlcis(:,i)=eig(a-b*K); %vlastni cisla uzavrene smycky
end
%w=1:1:N;
%plot(w,abs(vlcis(1,:)),'m');
%figure(2);
%plot(w,abs(vlcis(2,:)),'r');
%figure(3);
%plot(w,abs(vlcis(3,:)),'g');
%figure(4);
%plot(w,abs(vlcis(4,:)),'b');

%K=((R+b'*P*b)^-1)*b'*P*a
%eig(a-b*K)

% Dalsi experimenty
Q=Q*1000;
P=Q;
vlcis2(:,1) = eig(a-b*K);
for i = 2 : N,
   %   j=N-i;
   if i == 100 Q=Q/1000;end
   Pom = a'*P*a-a'*P*b*((R+b'*P*b)^-1)*b'*P*a+Q;
   P = Pom;
   K=((R+b'*P*b)^-1)*b'*P*a;
   vlcis2(:,i)=eig(a-b*K); %vlastni cisla uzavrene smycky
end
w=1:1:N;
figure(1);
hold on;
plot(w,abs(vlcis(1,:)),'b');
plot(w,abs(vlcis2(1,:)),'r');
%hold on;

figure(2);
hold on;
plot(w,abs(vlcis(2,:)),'b');
plot(w,abs(vlcis2(2,:)),'r');

figure(3);
hold on;
plot(w,abs(vlcis(3,:)),'b');
plot(w,abs(vlcis2(3,:)),'r');

figure(4);
hold on;
plot(w,abs(vlcis(4,:)),'b');
plot(w,abs(vlcis2(4,:)),'r');

K=((R+b'*P*b)^-1)*b'*P*a
eig(a-b*K)