%
% Function to integrate auxillart variable v 
%    -dv/dt = (A-BK(t)) v + C^TQr
% Input variables:
%    A   - State-space A matrix.
%    B   - State-space input matrix.
%    C   - State space output matrix.
%    K   - Feedback gain.
%    Q   - State weighting matrix.
%    P   - Final weighting matrix.
%    r   - input. 
%

%
% Written by: Kjetil Havre,  5/2-1996.
%
function V = auxvint(A,B,C,K,Q,P,r)
      
%   K = intpol(K,t2); r = intpol(r,t1);
   t1 = getiv(K); t2 = getiv(r); 
   nt1 = size(t1,1); nt2 = size(t2,1);      
   if nt1 < nt2
      K = intpol(K,t2);
   elseif nt1 > nt2
      r = intpol(r,t1);
   end
   
   t1 = getiv(K); t2 = getiv(r);
   nt1 = size(t1,1); nt2 = size(t2,1);      
   if nt1 < nt2
      K= intpol(K,t2);
   elseif nt1 > nt2
      r = intpol(r,t1);
   end
   
   t1 = getiv(K); t2 = getiv(r);
   nt1 = size(t1,1); nt2 = size(t2,1);      
   if norm(t1-t2) > 1e-10 
      K = intpol(K,t2); r = intpol(r,t1);
   end
   
   t  = getiv(K);
   nt = max(size(t));
   tf = t(nt);   t0 = t(1);
   Bm = C.'*Q; D = zeros( size(C,1), size(B,2) );
   
   x0 = C.'*P*vunpck(xtract(r,tf));
   for i = 1:nt-1
      Kd = vunpck( xtract(K,t(i)) );
      Am = (A-B*Kd).'; 
      tv = [t(i) t(i+1)];
      ri  = vunpck(xtract(r,t(i))).';
      ri1 = vunpck(xtract(r,t(i+1))).';
      uv  = [ri; ri1];
      [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,:)];
   
   Xhu = flipud(Xh);
   tu = flipud(tf-t);

   D = [];
   for i = 1:nt
      D = [D;  Xhu(i,:).'];
   end
   V = vpck(D,tu);
end

