%  Example 8.2.  Steady-state LQG cost computation.
clear
%  Define the plant model.
A=[0 -1   0   0
   0  0   0 -1.6
   0  0   0  -1
   0 1.8  0   0];
B=[ 0   0
   -11  0
    0   0
    0  -12];
C=[1 0 0 0
   0 0 1 0];
%  Define the cost function.
Q=C'*C;
R=0.02*eye(2);
%  Generate the LQR gain.
K=lqr(A,B,Q,R)
%  Define the spectral densities.
Sw=10*eye(2);
Sv=1e-4*eye(2);
%  Generate the Kalman gain.
G=lqe(A,B,C,Sw,Sv)
%  Generate the poles.
Poles_control=eig(A-B*K)
Poles_Kalman =eig(A-G*C)
%  Generate the closed loop system model.
Acl=[ A-B*K     B*K
     zeros(4)  A-G*C];
Bcl=[B  zeros(4,2)
     B     -G     ];
Ccl=[C zeros(2,4)];
Swv=[   Sw    zeros(2)
     zeros(2)    Sv   ];
%  Generate the covariance matrix of the augmented state.
Sxe=lyap(Acl,Bcl*Swv*Bcl');
%  Generate the covariance matrix of the reference output.
Sy=Ccl*Sxe*Ccl'
%  Generate the covariance matrix of the control input.
Su=[-K K]*Sxe*[-K K]'
%  Compute the cost.
J=trace([Q+K'*R*K -K'*R*K;-K'*R*K K'*R*K]*Sxe)
