%  Example 6.11.  Stochastic regulator simulation and cost function
%    computation.
clear
%  Define the plant model.
A=[0   1
   0 -0.1];
Bu=[0 0.001]';
Bw=[0 0.001]';
%  Define the cost function parameters.
Q=[180 0
    0  0];
R=1;
Sw=5000;
%  Find the steady-state LQR gain and Riccati solution. 
[K,P]=lqr(A,Bu,Q,R);
K
%  Compute the cost.
J=trace(P*Bw*Sw*Bw')
%  Generate the state correlation matrix for the closed loop system.
Sigmax=lyap(A-Bu*K,Bw*Sw*Bw')
%  Find the standard deviation of theta.
sigmaTheta=sqrt(Sigmax(1,1))
%  Simulate the system for a check on the standare deviation.
%  Define the time step and time vector.
dt=0.1;
t=0:dt:100;
%  Generate the random input.
w=sqrt(Sw/dt)*randn(size(t));
%  Simulate the system.
y=lsim(A-Bu*K,Bw,[1 0],0,w,t);
%  Plot the results.
set(0,'DefaultAxesFontName','times')
set(0,'DefaultAxesFontSize',16)
set(0,'DefaultTextFontName','times')
figure(1)
clf
plot(t,y)
xlabel('Time')
ylabel('Theta')
grid
