%
% Function to do the backward recursion of discrete Riccati equation:
%    S_k = A^T[S_{k+1} - S_{k+1}(B^TS_{k+1}B + R)^{-1}B^TS_{k+1}]A+C^TQ
% from N with S_N = C^TPC and P given to i.
% This recursion is done by computing:
%    1: K^v_k = (B^TS_{k+1}B + R)^{-1}
%    2: K_k = K^v_k S_{k+1} A
%    3: S_k = A^TS_{k+1}(A-BK_k) + C^TQC
% 
% Input variables:
%    A   - State-space A matrix.
%    B   - State-space input matrix.
%    C   - State-space output matrix.
%    P   - Weighting matrix.
%    Q   - State weighting matrix.
%    R   - Input weighting matrix.
%    i   - Initial time (final time for recursion).
%    N   - Final time (initial time for recursion).

%    Mu-tools must also be installed.

%
% Written by: Kjetil Havre,  6/2-1996.
%
function [S,K,Kv] = dricint(A,B,C,P,Q,R,i,N)

   S  = C.'*P*C;            Sa  = S;
   Kv = inv(B.'*S*B+R)*B.';
   K  = Kv*S*A; 
   Kva = Kv;
   Ka  = K;
   IDA = N;
   
   for j=N-1:-1:i
      Kv = inv(B.'*S*B+R)*B.';
      K  = Kv*S*A; 
      S  = A.'*S*(A-B*K)+C.'*Q*C;
      Kva = [Kv; Kva];
      Ka  = [K; Ka];
      Sa  = [S;Sa];
      IDA = [j;IDA];
   end
   S  = vpck(Sa,IDA);
   K  = vpck(Ka,IDA);
   Kv = vpck(Kva,IDA);
end
