%  Example 6.5.  Solution of the Riccati equation for the time-varying
%    optimal feedback gain and the steady-state LQR gain.
clear
%  Define the plant model.
A=[0 1
   0 0];
B=[0 1]';
%  Define the cost function parameters.
C=[1 0];
Q=C'*C;R=0.1;
%
%  Find the steady-state LQR gain.
Kss=lqr(A,B,Q,R);
%
%  The Riccati differential equation is solved by numerical integration:
%
%            P(t-T)=P(t)-Pdot(t)*T
%
%  where Pdot is the dirivative of P given by the Riccati equation.
%  Note that the sampling time T must be short for this approach to 
%  be valid.
%
%  Enter the number of time steps, the step size, and the time vector.
n=100;
T=0.01;
tf=n*T
tvec=T:T:n*T;
%  Initialize the Riccati solution and the optimal gain.
P=zeros(2);
K(n,:)=inv(R)*B'*P;
%  Perform the iteration discussed above.
for i=n-1:-1:1,
   P=P-T*(-P*A-A'*P-Q+P*B*inv(R)*B'*P);
   K(i,:)=inv(R)*B'*P;
end
%
%  Compute the response of the closed loop system and the cost when using
%    the steady-state LQR gain.
%  Initialize the state and the cost.
xss(:,1)=[1 1]';
Costss=T*xss(:,1)'*Q*xss(:,1);
%  The system response is generated by numerical integration of the 
%  state equation.
for i=2:n,
   uss(i-1)=-Kss*xss(:,i-1);
   xss(:,i)=xss(:,i-1)+T*(A*xss(:,i-1)+B*uss(i-1));
   Costss=Costss+T*(xss(:,i)'*Q*xss(:,i)+uss(i-1)'*R*uss(i-1));
end
uss(n)=-Kss*xss(:,n);
Costss=Costss+T*uss(n)'*R*uss(n)
%
%  Compute the response of the closed loop system when using the 
%    time-varying LQR gain.
%  Initialize the state.
x(:,1)=[1 1]';
for i=2:n,
   u(i-1)=-K(i-1,:)*x(:,i-1);
   x(:,i)=x(:,i-1)+T*(A*x(:,i-1)+B*u(i-1));
end
%  Generate a final control so the dimensions match.
u(n)=-K(n,:)*x(:,n);
%  Computation of the optimal cost with the time-varying gains.
Cost = x(:,1)'*P*x(:,1)
%  Plot the results.
set(0,'DefaultAxesFontName','times')
set(0,'DefaultAxesFontSize',16)
set(0,'DefaultTextFontName','times')
figure(1)
clf
subplot(221),plot(tvec,K(:,1))
xlabel('Time (sec)')
y=ylabel('{\itK}_1')
set(y,'Units','Normalized')
xpos=-0.15;
set(y,'Position',[xpos 0.4861 0])
grid
subplot(222),
plot(tvec,K(:,2))
xlabel('Time (sec)')
y=ylabel('{\itK}_2')
set(y,'Units','Normalized')
set(y,'Position',[xpos 0.4861 0])
grid
subplot(223),
plot(tvec,x(1,:)','r-',tvec,xss(1,:)','r--')
xlabel('Time (sec)')
ylabel('Attitude angle (deg)')
grid
legend('time-varying','steady-state')
subplot(224),plot(tvec,u','r-',tvec,uss','r--')
xlabel('Time (sec)')
ylabel('Control')
grid
legend('time-varying','steady-state')
