clear all;
close all;
Ts=0.1;
%prevod do state_space pro 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);
%dale je to nezmeneno
[M,N,C,D]=ssdata(c2d(sys,Ts));
size1_M=size(M,1);%zjisteni poctu radku M, rovno 5 pro zadany system
size2_N=size(N,2);%zjisteni poctu sloupcu N, rovno 1 pro zadany system
Q=diag([1 1 1 1 1]); %zmeneno pro  pocet radku M=5, ale neni promenne
R=1000*eye(size2_N); %matice zesileni ve zpetne vazbe
P=Q;
n=200;
Khist=zeros(n,size1_M);
for i=n:-1:1
   K=inv((R+N'*P*N))*N'*P*M;
   P=Q+M'*P*M-M'*P*N*K;
   Khist(i,:)=K;
end;
figure(1);
plot(Khist);
x=[1 1 1 1 1]';%zmeneno pro pocet radku M=5, ale neni promenne
xhist=zeros(n+1,size1_M);%zmeneno pro promenny pocet radku M (roven 5)
xhist(1,:)=x';
for i=1:n
   u=-Khist(i,:)*x;
   x=M*x+N*u;
   xhist(i+1,:)=x';
end;
figure(2);
plot(xhist)