%
% Function to do the forward recursion of:
%    v_{k+1} = (A-BK_k)^{-T} (v_{k} - C^TQr_k) 
% from 0 with v_0 given, to N.
% 
% 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 = dintvf(A,B,C,K,v0,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 = v0; va = v;
   for j=1:nk-1
      Kd = vunpck( xtract(K, k(j)) );
      v = inv((A-B*Kd).')*(v - C.'*Q*vunpck( xtract(r, k(j)) ));
      va = [va;v];
   end
   v = vpck(va,k);
end
