%
% Function to do the backward recursion of:
%    v_k = (A-BK_k)^T v_{k+1} + C^TQr_k 
% from N with v_N = C^TPr_N and P given, to i.
% 
% Input variables:
%    A   - State-space A matrix.
%    B   - State-space input matrix.
%    C   - State-space output matrix.
%    K   - Time varying feedback gain.
%    P   - Weighting matrix.
%    Q   - State weighting matrix.
%    r   - Time varying reference.

%    Mu-tools must also be installed.

%
% Written by: Kjetil Havre,  6/2-1996.
%
function v = dintv(A,B,C,K,P,Q,r)
   rcidv = indvcmp(K,r);
   
   if rcidv ~= 1
      disp('Error: time is different in r and K .')
      return;
   end

   k  = getiv(K);
   nk = size(k,1);
   N  = k(nk);
   i  = k(1);
   
   v = C.'*P*vunpck( xtract(r,N) ); va = v;
   for j=nk-1:-1:1
      Kd = vunpck( xtract(K, k(j)) );
      v = (A-B*Kd).'*v + C.'*Q*vunpck( xtract(r, k(j)) );
      va = [v;va];
   end
   v = vpck(va,k);
end
