clear all;
close all;
Ts=0.1;
% zadany system
num=poly([-5 64 -complex(28,35) -complex(28,-35)]);
denum=poly([-complex(1,10) -complex(1,-10) -complex(-1,25) -complex(-1,-25) 3]);
sys=tf(num,denum);

[M,N,C,D]=ssdata(c2d(sys,Ts));

Ns = size(M, 1);
Nu = size(N, 2);

% vahove matice
Q = diag([1, 1, 1, 1, 1]);
R=1e+5;

P = Q;

x = 70;
Kh = zeros(Ns,x);
for t = x : -1 : 1
   K=inv(R+N'*P*N)*N'*P*M;
   P=M'*P*M+Q-M'*P*N*K;
   Kh(:,t) = K';
end

figure(1);
plot(Kh');
hold on;

[P,L,Kdare] = dare(M,N,Q,R);
Kdare

figure(1);
plot(0,Kdare);

K_x = Kh(:,1)'
VLCisla_MNK = eig(M-N*K_x)
sim('lq1'); 

%otevrena smycka
OL = ss(M,N,K_x,0);

figure(2); grid; nyquist(OL,'b');
figure(3); grid; nakresli(AkcniZasah,'Akcni zasah','b');
figure(4); grid; nakresli(Vstup, 'Vstup do soustavy','b');
figure(5); grid; nakresli(Vystup, 'Vystup','b');