%% Demonstrace male robustnosti LQG regulatoru

% Vytvoreno pro system popsany v clanku
% Doyle, J. Guaranteed margins for LQG regulators. 
% IEEE Transactions on Automatic Control, volume 23, issue 4, pages 756- 757, Aug 1978.

%% Zadani SISO systemu 
A = [1 1;0 1]; 
Bu = [0 1]'; Bw = [1 1]'; Bv = [0 0]'; 
C = [1 0]; 
Du = 0; Dw = 0; Dv = 1;

G = ss(A,[Bu,Bw,Bv],C,[Du Dw Dv]);
set(G,'InputName',{'u','w','v'});   % vstupem je rizeni, porucha systemu a sum mereni
set(G,'OutputName','y');            % jediny vystup systemu, ktere je zaroven i mereny


%% LQ navrh
q1 = 10; Q1 = q1*[1 1; 1 1];     % penalizace chyby regulace
R1 = 1;                             % penalizace akcniho zasahu

[K,S,e] = lqr(G(:,1),Q1,R1);        % pro navrh se bere v uvahu pouze prvni vstup (u)
G_ol_lq = ss(A,Bu,K,0);             % otevrena smycka se stavovym LQ regulatorem

figure(1)
margin(G_ol_lq)

figure(2), nyquist(G_ol_lq), title('Nyquistova krivka pro LQ regulator')
%% Navrh Kalmanova filtru a vytvoreni LQG regulatoru
Q2 = 10;                          % rozptyl poruchy ovlivnujici system
R2 = 1;                             % rozptyl sumu mereni

[Kest,L,P] = kalman(G(:,1:2),Q2,R2);% vypocet Kalmanova filtru
R_lqg = lqgreg(Kest,K);             % vytvoreni LQG regulatoru
Gcl_lqg = feedback(G,R_lqg,1,1,+1); % vytvoreni uzavrene smycky (1. vstup a 1.vystup)

figure(3)
margin(-G(:,1)*R_lqg)

%%
figure(4), 
h = nyquistplot(-G(:,1)*R_lqg)
grid on
p = getoptions(h)
p.TickLabel.FontSize = 12
p.XLabel.FontSize = 13
p.YLabel.FontSize = 13
p.Title.FontSize = 14
setoptions(h,p)
axis equal
% print -depsc2 nyquist_LQG.eps
% !epstopdf nyquist_LQG.eps


%% Odezvy LQG regulatoru na stoch. poruchu: nominalni i neurcity pripad
figure(5)
dt = 0.1;
t = 0:dt:100;
w = randn(1,length(t))*sqrt(1/dt);
lsim(Gcl_lqg(:,2),w,t)

% figure(6)
% Delta = ultidyn('Delta',[1 1]);         % dynamicka neurcitost
% W = tf(1,[1 1]);                        % vahovaci filtr
% G_unc = G*(1+Delta*W);                  % system se vstupni multiplikativni neurcitosti
% Gcl_lqg_unc = feedback(G_unc,R_lqg,1,1,+1); % vytvoreni uzavrene smycky (1. vstup a 1.vystup)
% lsim(usample(Gcl_lqg_unc(:,2)),w,t);    % simulace odezvy jednoho nahodneho systemu z mnoziny pripustnych