% LQ regulator - cviceni 14. 11. 2002

% zadany system:

A = [0 1 0 0;0 0 1 0;0 0 0 1;-1e-11 -24010 -2539 -54];
B = [0;0;0;31490];
C = [1 0 0 0];
D = 0;

sys = ss(A,B,C,D);

% diskretizace se zadanou Ts

Ts = 0.01; % ze zadani

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

% zvolime vahove matice (musi byt pozitivne definitni a symetricke, napr. diagonalni)
% jelikoz zadani nerika nic o vahach jednotlivych stavu, zvolime vahy stejne (=1)
% ev. vahy doladime na zaklade vysledku

Q = diag([1000,2,3,4]); % vahy jednotlivych stavovych promennych
                        % (jak moc do nich investujeme energii pri tlaceni do ciloveho stavu)
R = 1000*eye(1);

% optimalizujeme podle ztratove funkce V (Riccatiho rovnice)

n = 2000; % zvolime dobu (pocet iteraci) - zkusmo, kdyztak se prodlouzi
Nsize = size(A,1); % rad systemu

Khist = zeros(Nsize,n); % vynulujeme historii matice K
P = Q; % pocatecni podminka Riccatiho rovnice

for i = n:-1:1 % cas pozpatku
   K = inv(R + N'*P*N) * N'*P*M; 
   P = Q + M'*P*M - M'*P*N* K;   % vaha stavu v i-tem kroku
   Khist(:,i) = K'; % ulozime mezivysledek
end;

figure(1);
plot(Khist'); % vykreslime prubeh iteracniho procesu

% ted je v K clen, ktery by se pouzil pro mirne suboptimalni stavovy regulator (LTI)
% casove promenny clen => bude se menit podle historie => bude skutecne optimalni

% v grafu je v leve casti ustaleny stav (cas pozpatku) - tj. K pro LTI regulator




% napocitame rizeni vypoctenym regulatorem:

x = [1;50;40;0]; % lib. nenulova pocatecni podminka (jinak nula od nuly pojde) - postouchneme
Xhist = zeros(Nsize,n); % vynulujeme vystup
Yhist = zeros(1,n);

for j = 1:1:n % cas popredu
   u = -Khist(:,1)' * x; % optimalni rizeni s konst. K (j misto 1 =>prom.)
   y = C*x + D*u; % napocteme vystup regulovane soustavy
   Xhist(:,j) = x; % a vsechno ukladame do historie
   Yhist(:,j) = y;
   x = M*x + N*u; % prepocitame stav
end;

figure(2);
plot(Xhist'); % historie stavu
hold on;
plot(Yhist); % prubeh regulace