%  Example 8.1.  Time-varying LQG control.
clear
%  Input the plant model.
A=[0   1
   0 -0.1];
Bu=[0 0.001]';
Bw=[0 0.001]';
Cm=[1 0];
%  Input cost function parameters.
Q=[180 0
    0  0];
R=1; Ri=1/R;
%  Input spectral densities.
Sw=5000;
Sv=1; Svi=1/Sv;
%  Use a sampling time of one second.
dt=0.1;
t=0:dt:100-dt;
%  Initial condition for control Ricatti equation.
P=zeros(2);
K=zeros(1000,2);
K(1000,:)=Ri*Bu'*P;
%  Solve control Ricatti equation.
for n=999:-1:1,
  P=P-dt*(-P*A-A'*P-Q+P*Bu*Ri*Bu'*P);
  K(n,:)=Ri*Bu'*P;
end
%  Plot the control gains
set(0,'DefaultAxesFontName','times')
set(0,'DefaultAxesFontSize',16)
set(0,'DefaultTextFontName','times')
figure(1)
clf
subplot(221), plot(t,K)
grid
xlabel('Time (sec)')
ylabel('Feedback gains')
%  Initial condition for Kalman Ricatti equation.
Se=diag([0 0]);
G=zeros(2,1000);
G(:,1)=Se*Cm'*Svi;
%  Solve Kalman Ricatti equation.
for n=2:1000,
  Se=Se+dt*(Se*A'+A*Se+Bw*Sw*Bw'-Se*Cm'*Svi*Cm*Se);
  G(:,n)=Se*Cm'*Svi;
  end
%  Plot the Kalman gains
subplot(222), plot(t,G)
grid
xlabel('Time (sec)')
ylabel('Kalman gains')
%  Generate inputs for simulating the closed loop system.
w=sqrt(Sw/dt)*randn(1000,1);
v=sqrt(Sv/dt)*randn(1000,1);
%  Initialize the augmented state consisting of x and e.
xa=zeros(4,1000);
u=zeros(1,1000);
xa(:,1)=[0 0 0 0]';
u(1)=-K(1,:)*(xa(1:2,1)-xa(3:4,1));
%  Simulation loop.
for n=1:999,
	Acl=[A-Bu*K(n,:)  Bu*K(n,:)
	      zeros(2)   A-G(:,n)*Cm];
	Bcl=[Bw  [0 0]'
	     Bw -G(:,n)];
	xa(:,n+1)=xa(:,n)+dt*(Acl*xa(:,n)+Bcl*[w(n);v(n)]);
	u(n+1)=-K(n+1,:)*(xa(1:2,n+1)-xa(3:4,n+1));
	end
%  Plot theta (x1) and x-hat = theta (x1) - theta-hat (x3).
subplot(223),plot(t,xa(1,:),'r-',t,xa(1,:)-xa(3,:),'r--')
grid
xlabel('Time (sec)')
ylabel('Angle error (deg)')
legend('Actual','Estimated')
%  Plot the control input.
subplot(224),plot(t,u)
grid
xlabel('Time (sec)')
ylabel('Control (N-m)')
%
%  Evaluate the performance of the controller.
%
%  Allocate space for the standard deviations of theta, the error, and 
%    the control.
sigtheta=zeros(1,1000);
sige=zeros(1,1000);
sigu=zeros(1,1000);
%  Initialize the correlation matrix of the closed loop state (plant state 
%    and error).
Sxe=zeros(4);
%  Initialize the integral in the cost function.
Ji=0;
%  Perform the integral to generate the cost and solve for the correlation 
%    matrix of the closed loop state.
for n=1:999,
	%  Form the closed loop state model.
	Acl=[A-Bu*K(n,:)  Bu*K(n,:)
	      zeros(2)   A-G(:,n)*Cm];
	Bcl=[Bw  [0 0]'
	     Bw -G(:,n)];
	%  Update the correlation matrix.
	Sxe=Sxe+dt*(Acl*Sxe+Sxe*Acl'+Bcl*[Sw 0;0 Sv]*Bcl');
	%  Compute and save the standard deviations.
	sigtheta(n+1)=sqrt(Sxe(1,1));
	sige(n+1)=sqrt(Sxe(3,3));
	sigu(n+1)=sqrt([-K(n,:) K(n,:)]*Sxe*[-K(n,:)';K(n,:)']);
	%  Perform the integral to generate the cost.
	Qt=[Q+K(n,:)'*R*K(n,:) -K(n,:)'*R*K(n,:)
	    -K(n,:)'*R*K(n,:)   K(n,:)'*R*K(n,:)];
	Ji=Ji+dt*trace(Sxe*Qt);
end
%  Compute the final cost.
J=Ji/2
%
%  Evaluate the performance of the steady-state controller.
%
%  Generate steady-state gains.
Kss=lqr(A,Bu,Q,R);
Gss=lqe(A,Bw,Cm,Sw,Sv);
%  Form closed loop system (steady-state).
Aclss=[A-Bu*Kss  Bu*Kss
	   zeros(2) A-Gss*Cm];
Bclss=[Bw [0 0]'
	   Bw  -Gss ];
%  Allocate space for the standard deviations of theta, the error, and 
%    the control.
sigthetass=zeros(1,1000);
sigess=zeros(1,1000);
siguss=zeros(1,1000);
%  Generate the matrix in the cost function integral.
Qtss=[Q+Kss'*R*Kss  -Kss'*R*Kss
	  -Kss'*R*Kss   Kss'*R*Kss];
%  Initialize the correlation matrix of the closed loop state (plant state 
%    and error).
Sxess=zeros(4);
%  Initialize the integral in the cost function.
Jiss=0;
%  Perform the integral to generate the cost and solve for the correlation 
%    matrix of the closed loop state.
for n=1:999,
	%  Update the correlation matrix.
	Sxess=Sxess+dt*(Aclss*Sxess+Sxess*Aclss'+Bclss*[Sw 0;0 Sv]*Bclss');
	%  Compute and save the standard deviations.
	sigthetass(n+1)=sqrt(Sxess(1,1));
	sigess(n+1)=sqrt(Sxess(3,3));
	siguss(n+1)=sqrt([-Kss Kss]*Sxess*[-Kss';Kss']);
	%  Perform the integral to generate the cost.
	Jiss=Jiss+dt*trace(Sxess*Qtss);
	end
%  Compute the final cost.
Jss=Jiss/2
%  Plot the standard deviation of theta, the estimation error,
%  and the control input for both the time-varying and the 
%  steady-state controllers.
figure(2)
clf
subplot(221),plot(t,sigtheta,'r-',t,sigthetass,'r-.')
grid
xlabel('Time (sec)')
ylabel('Angle error (st. dev.)')
legend('Time-varying','Steady-state')
subplot(222),plot(t,sige,'r-',t,sigess,'r-.')
grid
xlabel('Time (sec)')
ylabel('Estimation error (st. dev.)')
legend('Time-varying','Steady-state')
subplot(223),plot(t,sigu,'r-',t,siguss,'r-.')
grid
xlabel('Time (sec)')
ylabel('Control (st. dev.)')
legend('Time-varying','Steady-state')
