%%%% LQ regulator  %%%%
close all;
A = [[0 1 0 0];[0 0 1 0];[0 0 0 1];[-10^(-11) -24010 -2539 -54]];
B = [0; 0; 0; 31940];
C = [1 0 0 0 ];
D = 0;
Ts = 0.001;
sys=ss(A,B,C,D);
dsys=c2d(sys,Ts,'zoh');
[M,N,C,D]=ssdata(dsys);
% Q a R am byt pozitivne definitni
%Q = eye(4); % vaha stavu
%R = eye(1); % vaha rizeni
Q=[[50000 0 0 0];[0 120 0 0];[0 0 100 0];[0 0 0 1000]] %volime - cim vetsi, tim vice zakazujeme velke rizeni
R=10000 %volime - cim vetsi, tim vice zakazujeme velke rizeni
n = 5000 %volime
P = Q;
ns = size(A,1);
Khist=zeros(n,ns);
for i = n:-1:1
   K = inv(R  + N'*P*N)*N'*P*M; %matice stavove zpetne vazby
   P = Q + M'*P*M - M'*P*N*K; %prozatim k nicemu
   Khist(i,:) = K;
end
P
figure(1);
plot(Khist);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%55

x=[5 1 2 3]'
xhist = zeros(n,ns);
for i = 1:1:n
   x = M*x - N*((Khist(i,:))*x);
   xhist(i,:) = x';
end

figure(2);
plot(xhist);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%55

x=[5 1 2 3]'
xhist = zeros(n,ns);
for i = 1:1:n
   x = M*x - N*((Khist(1,:))*x);
   xhist(i,:) = x';
end

figure(3);
plot(xhist);
legend('x1','x2','x3','x4');