%
% Function to do compute optimal trajectory for tracking problem.
% That is, given v_k, K_k, K_k^v and x0, together with the system  
% compute x_k, u_k and y_k.
% 
% Input variables:
%    A   - State-space A matrix.
%    B   - State-space input matrix.
%    C   - State-space output matrix.
%    K   - Time varying feedback gain.
%    Kv  - Feedforward gain.
%    v   - Affine input.
%    x0  - Initial state.

%    Mu-tools must also be installed.

%
% Written by: Kjetil Havre,  6/2-1996.
%
function [x,u,y] = doptt(A,B,C,K,Kv,v,x0)
   rcidv = indvcmp(K,v);
   if rcidv ~= 1
      disp('Error: time is different in v and K .')
      return;
   end

   rcidv = indvcmp(K,Kv);
   if rcidv ~= 1
      disp('Error: time is different in Kv and K .')
      return;
   end

   k  = getiv(K);
   nk = size(k,1);
   N  = k(nk);
   i  = k(1);
   
   x = x0; xa = x;ua = []; ya = C*x;
   for j=1:nk-1
      Kd  = vunpck( xtract(K,  k(j)) );
      Kvd = vunpck( xtract(Kv, k(j)) );
      vd  = vunpck( xtract(v, k(j)) );
      u   = -Kd*x + Kvd*vd;
      x   = A*x + B*u;
      y   = C*x;
      ua = [ua;u];
      xa = [xa;x];
      ya = [ya;y];
   end
   x = vpck(xa,k);
   y = vpck(ya,k);
   u = vpck(ua,k(1:nk-1));
end
