%  Example 4.4.  Quadratic cost computation for a system with random 
%    test conditions.  Uses both analytic results and simulation.
clear
%  t is the time vector dt is the sampling time.
dt=0.1;
t=0:dt:200;
%  Define the test conditions.  Note that the noises in the simulation
%  are discrete-time, and must, therefore, be scaled by the sampling time.
r=zeros(size(t));
Sw=diag([1 400]);
w0=sqrt(Sw(1,1)/dt)*randn(size(t));
v=sqrt(Sw(2,2)/dt)*randn(size(t));
w=[w0;v]';
x0=[0 0 0 0]';
%  Define the Plant:
A=[0 1
   0 0];
Bu=[0 1]';
Bw=[0 0
    1 0];
Cm=[1 0];
Cy=[1 0
    0 0];
Dmw=[0 1];
Dyu=[0 1]';
Dyw=zeros(2);
%  Define the Controller:
Ac=[-1.6    1
    -0.68 -0.4];
Bcm=[1.6 0.64]';
Cc=[-0.04 -0.4];
Dcm=0;
%  Generate the closed loop system:
Acl=[A+Bu*Dcm*Cm Bu*Cc
       Bcm*Cm      Ac ];
Bcl=[Bw+Bu*Dcm*Dmw
        Bcm*Dmw   ];
Ccl=[Cy+Dyu*Dcm*Cm Dyu*Cc];
Dcl=Dyw+Dyu*Dcm*Dmw;
%  Generate a discrete-time closed loop model.
[Phi,Gam]=c2d(Acl,Bcl,dt);
%  Simulate the system in discrete-time.
%  Note: The noises are discrete-time.
y=dlsim(Phi,Gam,Ccl,Dcl,w,x0);
%  Remove the transient. ktrans defines the end of the transient.
ktrans=20/dt+1;
yss=y(ktrans:length(t),:);
tss=t(1:length(t)-ktrans+1);
%  Plot the results.
set(0,'DefaultAxesFontName','times')
set(0,'DefaultAxesFontSize',16)
set(0,'DefaultTextFontName','times')
figure(1)
clf
plot(tss,yss(:,1),'r-',tss,yss(:,2),'r--')
xlabel('Time (sec)')
axis([0 180 -40 40])
legend('Error','Control')
grid
%  Compute the cost from the simulation results.
YY=[1/800     0
       0   1/200];
Jm=trace(YY*yss'*yss)/(length(t)-ktrans+1)
%  Compute the cost theoretically.
Sxx=lyap(Acl,Bcl*Sw*Bcl');
J=trace(YY*Ccl*Sxx*Ccl')
