%  Example 6.4.  Solution of the Riccati equation for the time-varying
%    optimal feedback gains.
clear
%  Define the plant model.
A=[0  1
   0 -1];
B=[0 1]';
C=[1 0];
%  Define the cost function parameters.
h=10;q=0;r=0.1;
H=h*C'*C;
Q=q*C'*C;
R=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=1000;
T=0.01;
tvec=T:T:n*T;
%  Initialize the Riccati solution and the optimal gain.
P=H;
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.
%  Initialize the state.
x(:,1)=[1 1]';
%  The system response is generated by numerical integration of the 
%  state equation.
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);
%  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,:))
xlabel('Time (sec)')
ylabel('Motor shaft angle (rad)')
grid
subplot(224),plot(tvec,u)
xlabel('Time (sec)')
ylabel('Control voltage')
grid
%  Computation of the optimal cost.
Cost = x(:,1)'*P*x(:,1)