s=tf('s');
Ps=((s+5)*(s-64)*(s+28+35*j)*(s+28-35*j))/((s+1+10*j)*(s+1-10*j)*(s-1+25*j)*(s-1-25*j)*(s-3));
SYS=SS(Ps);
SD=c2d(SYS,0.1)
M=SD.a
N=SD.b
SD.c
SD.d

R=1000*eye(1);
Q=diag([1000 1 1 1 1]);
P0=Q;

for i=500:-1:1 
   K=inv(R+N'*P0*N)*N'*P0*M;
   P0=Q+M'*P0*M-M'*P0*N*K;
   K0(i,:)=K;
end;

K(1,:);
[P,L,K2]=dare(M,N,Q,R);
sim('lq1'); 
OL = ss(M,N,K2,0);

%figure(1); grid; plot(K0);
figure(1); grid; nyquist(OL,'b');
figure(2); grid; nakresli(AkcniZasah,'Akcni zasah','b');
figure(3); grid; nakresli(Vstup, 'Vstup do soustavy','b');
figure(4); grid; nakresli(Vystup, 'Vystup','b');
%figure(5); grid; nakresli_stav(Stavy,1,'x1','b');
%figure(6); grid; nakresli_stav(Stavy,2,'x2','b');
%figure(7); grid; nakresli_stav(Stavy,3,'x3','b');
%figure(8); grid; nakresli_stav(Stavy,4,'x4','b');
%figure(9); grid; nakresli_stav(Stavy,5,'x5','b');


R=10000*eye(1);
Q=diag([1000 1 1 1 1]);
[P,L,K2]=dare(M,N,Q,R);
sim('lq1'); 
OL = ss(M,N,K2,0);

%figure(1); hold; plot(K0);
figure(1); hold; nyquist(OL,'r');
figure(2); hold; nakresli(AkcniZasah,'Akcni zasah','r');
figure(3); hold; nakresli(Vstup, 'Vstup do soustavy','r');
figure(4); hold; nakresli(Vystup, 'Vystup','r');
%figure(5); hold; nakresli_stav(Stavy,1,'x1','r');
%figure(6); hold; nakresli_stav(Stavy,2,'x2','r');
%figure(7); hold; nakresli_stav(Stavy,3,'x3','r');
%figure(8); hold; nakresli_stav(Stavy,4,'x4','r');
%figure(9); hold; nakresli_stav(Stavy,5,'x5','r');