%
% Function to integrate ricatti-equation:
%    -dS/dt = A^T S + SA - SBR^{-1}B^TS + Q
% from tf to t0 numerically backward in time, 
% with boundary condition S(tf) = Sf.
% Input variables:
%    A   - State-space A matrix.
%    B   - State-space input matrix.
%    Q   - State weighting matrix.
%    R   - Input weighting matrix.
%    t0  - Initial time (final time for integration).
%    tf  - Final time (initial time for integration).
%    Sf  - Final value for Riccati-equation.
%    tol - tolerance for integration, default 1e-6.
%
% Note this function make use of the following functions:
%    ricf - to evaluate -dS/dt = A^T S + SA - SBR^{-1}B^TS + Q
%           in vector form.
%    vtri - to vectroize the solution matrix to the Riccati equation.
%    rtri - to restore the solution vector to the Riccati equation
%           in the symmetric matrix S.
%    Mu-tools must also be installed.

%
% Written by: Kjetil Havre,  4/2-1996.
%
function S = ricint(A,B,Q,R,t0,tf,Sf,tol)
   global Ah Qh Rh
   Ah = A; Qh = Q; 
   Rh = B*inv(R)*B.'; 
   
   if nargin < 8
      tol = 1e-6;
   end
   nx = size(A,1);
   no = sum(1:nx);
   if nargin < 7
      Sfv = zeros(no,1);       
   else
      Sfv = vtri(Sf);
   end
      
   [t,Sv] = ode45('ricf',t0,tf,Sfv,tol,0);
   Svu = flipud(Sv);
   tu = flipud(tf-t);

   nt = size(tu,1);
   D = [];
   for i = 1:nt
      M = rtri(Svu(i,:),nx);
      D = [D;  M + M.'-diag(diag(M))];
   end
   S = vpck(D,tu);
end
