%
% Function to integrate 
%    dx/dt = (A-BK(t)) x + BR^{-1}B^T v
% Input variables:
%    A   - State-space A matrix.
%    B   - State-space input matrix.
%    K   - Time varying feedback gain.
%    R   - Input weighting matrix.
%    v   - input. 
%    x0  - Initial state.
%

%
% Written by: Kjetil Havre,  5/2-1996.
%
function [X,U] = lintfv(A,B,K,R,v,x0)
      
   t1 = getiv(K); t2 = getiv(v);
   nt1 = size(t1,1); nt2 = size(t2,1);      
   if nt1 < nt2
      K = intpol(K,t2);
    elseif nt1 > nt2
      v = intpol(v,t1);
   end
   
   t1 = getiv(K); t2 = getiv(v);
   nt1 = size(t1,1); nt2 = size(t2,1);      
   if nt1 < nt2
      K = intpol(K,t2);
    elseif nt1 > nt2
      v = intpol(v,t1);
   end
   
   t1 = getiv(K); t2 = getiv(v);
   nt1 = size(t1,1); nt2 = size(t2,1);      
   if norm(t1-t2) > 1e-10 
      K = intpol(K,t2);  v = intpol(v,t1);
   end

   
   t  = getiv(K);
   nt = max(size(t));
   tf = t(nt);   t0 = t(1);
   Bm = B*inv(R)*B.';
   
   nx = max(size(A)); nu = size(Bm,2);
   C = zeros(nx); D = zeros(nx,nu);
   if nargin < 8
      x0 = zeros(nx,1);
   end
   
   
   for i = 1:nt-1
      Kd = vunpck( xtract(K,t(i)) );
      Am = (A-B*Kd).'; 
      tv = [t(i) t(i+1)];
      vi  = vunpck(xtract(v,t(i))).';
      vi1 = vunpck(xtract(v,t(i+1))).';
      uv  = [vi; vi1];
      [yh,xh] = lsim(Am,Bm,C,D,uv,tv,x0);
      if i == 1
        Xh = [xh(1,:).'];
      else
        Xh = [Xh; xh(1,:).'];
      end
      x0 = xh(2,:).';
   end
   Xh = [Xh; xh(2,:).'];
   
   X = vpck(Xh,t);
   U = msub( mmult(inv(R)*B.',v), mmult(K,X) );
end

