%  Example 8.3.  LQG/LTR design.
clear
%  Input system model.
A=[0  1
   0.5 0];
Bu=[0 0.1]';
Bw=[0 0.1]';
Cm=[1 0];
%  Input cost function parameters.
Q=diag([1 0]);
R=16;
%  Input spectral densities.
Sw=1;
Sv=10;
%  Generate the steady-state gains.
K=lqr(A,Bu,Q,R);
G=lqe(A,Bw,Cm,Sw,Sv);
%  Evaluate the robustness of the LQR controller.
%  Generate the Nyquist plot.
set(0,'DefaultAxesFontName','times')
set(0,'DefaultAxesFontSize',16)
set(0,'DefaultTextFontName','times')
figure(1)
clf
axes('Position',[.15 .2 .75 .75]);
w=logspace(-3,1,100);
[re,im]=nyquist(A,Bu,K,0,1,w);
plot(re,im,'r-',re,-im,'r-',-1,0,'r+',re(71),im(71),...
   'r>',re(71),-im(71),'r<')
xlabel('Real axis')
ylabel('Imaginary axis')
axis([-2 2 -2 2])
axis('square')
grid
%  Generate stability margins.
[Gm,Pm,Wcg,Wcp] = margin(A,Bu,K,0);
wh=logspace(0,2);
[mag,pha]=bode(A,Bu,K,0,1,wh);
Gmp = margin(mag,pha,wh);
'GM+      Gm-       PM'
[Gmp Gm Pm]
%  Evaluate the robustness of the LQG controller.
%  Generate the state model of the loop transfer function.
Al=[ A     zeros(2)
    G*Cm A-G*Cm-Bu*K];
Bl=[    Bu
    zeros(2,1)];
Cl=[zeros(1,2) K];
%  Generate Nyquist plot.
figure(2)
clf
axes('Position',[.15 .2 .75 .75]);
w=logspace(-3,1,100);
[re,im]=nyquist(Al,Bl,Cl,0,1,w);
plot(re,im,'r-',re,-im,'r-',-1,0,'r+',re(67),im(67),...
   'r>',re(67),-im(67),'r<')
xlabel('Real axis')
ylabel('Imaginary axis')
axis([-2 2 -2 2])
axis('square')
grid
%  Generate stability margins.
[Gm,Pm,Wcg,Wcp] = margin(Al,Bl,Cl,0);
wh=logspace(0,2);
[mag,pha]=bode(Al,Bl,Cl,0,1,wh);
Gmp = margin(mag,pha,wh);
%  Generate performance parameters.
%  Form the closed loop model.
Acl=[  A-Bu*K    Bu*K
     zeros(2,2) A-G*Cm];
Bcl=[Bw zeros(2,1)
     Bw    -G     ];
%  Compute the covariance matrix of the augmented state.
Sigxe=lyap(Acl,Bcl*diag([Sw Sv])*Bcl');
%  Compute the standard deivations of the output and the control.
sigy=sqrt([1 0 0 0]*Sigxe*[1 0 0 0]');
sigu=sqrt([-K K]*Sigxe*[-K K]');
%  Compute the cost.
J=trace([Q+K'*R*K -K'*R*K;-K'*R*K K'*R*K]*Sigxe);
'J      sigy      sigu       GM+      Gm-       PM'
[J sigy sigu Gmp Gm Pm]
%  Generate the LQG/LTR controller with Swf=1e4.
Swf=1e4;
%  Compute the new Kalman gain.
G=lqe(A,Bw,Cm,Sw+Swf,Sv);
%  Evaluate the robustness of the LQG/LTR controller.
%  Generate the state model of the loop transfer function.
Al=[ A     zeros(2)
    G*Cm A-G*Cm-Bu*K];
Bl=[    Bu
    zeros(2,1)];
Cl=[zeros(1,2) K];
%  Generate Nyquist plot.
figure(3)
clf
axes('Position',[.15 .2 .75 .75]);
w=logspace(-3,1,100);
[re,im]=nyquist(Al,Bl,Cl,0,1,w);
plot(re,im,'r-',re,-im,'r-',-1,0,'r+',re(65),im(65),...
   'r^',re(65),-im(65),'r^')
xlabel('Real axis')
ylabel('Imaginary axis')
axis([-2 2 -2 2])
axis('square')
grid
%  Generate the stability margins.
[Gm,Pm,Wcg,Wcp] = margin(Al,Bl,Cl,0);
wh=logspace(0,2);
[mag,pha]=bode(Al,Bl,Cl,0,1,wh);
Gmp = margin(mag,pha,wh);
%  Generate performance parameters.
%  Form the closed loop model.
Acl=[  A-Bu*K    Bu*K
     zeros(2,2) A-G*Cm];
Bcl=[Bw zeros(2,1)
     Bw    -G     ];
%  Compute the covariance matrix of the augmented state.
Sigxe=lyap(Acl,Bcl*diag([Sw Sv])*Bcl');
%  Compute the standard deivations of the output and the control.
sigy=sqrt([1 0 0 0]*Sigxe*[1 0 0 0]');
sigu=sqrt([-K K]*Sigxe*[-K K]');
%  Compute the cost.
J=trace([Q+K'*R*K -K'*R*K;-K'*R*K K'*R*K]*Sigxe);
[J sigy sigu Gmp Gm Pm]
%  Generate the LQG/LTR controller with Swf=1e6.
Swf=1e6;
G=lqe(A,Bw,Cm,Sw+Swf,Sv);
%  Evaluate the robustness of the LQG/LTR controller.
%  Generate the state model of the loop transfer function.
Al=[ A     zeros(2)
    G*Cm A-G*Cm-Bu*K];
Bl=[    Bu
    zeros(2,1)];
Cl=[zeros(1,2) K];
%  Generate Nyquist plot.
figure(4)
clf
axes('Position',[.15 .2 .75 .75]);
w=logspace(-3,1,100);
[re,im]=nyquist(Al,Bl,Cl,0,1,w);
plot(re,im,'r-',re,-im,'r-',-1,0,'r+',re(65),im(65),...
   'r^',re(65),-im(65),'r^')
xlabel('Real axis')
ylabel('Imaginary axis')
axis([-2 2 -2 2])
axis('square')
grid
%  Generate the stability margins.
[Gm,Pm,Wcg,Wcp] = margin(Al,Bl,Cl,0);
wh=logspace(0,2);
[mag,pha]=bode(Al,Bl,Cl,0,1,wh);
Gmp = margin(mag,pha,wh);
%  Generate performance parameters.
%  Form the closed loop model.
Acl=[  A-Bu*K    Bu*K
     zeros(2,2) A-G*Cm];
Bcl=[Bw zeros(2,1)
     Bw    -G     ];
%  Compute the covariance matrix of the augmented state.
Sigxe=lyap(Acl,Bcl*diag([Sw Sv])*Bcl');
%  Compute the standard deivations of the output and the control.
sigy=sqrt([1 0 0 0]*Sigxe*[1 0 0 0]');
sigu=sqrt([-K K]*Sigxe*[-K K]');
%  Compute the cost.
J=trace([Q+K'*R*K -K'*R*K;-K'*R*K K'*R*K]*Sigxe);
[J sigy sigu Gmp Gm Pm]
