%
% Function to do compute sub-optimal trajectory for tracking problem.
% That is, given v_0, K_\infty, K_\infty^v and x0, together with the 
% system  compute x_k, u_k, y_k and v_k.
% 
% Input variables:
%    A   - State-space A matrix.
%    B   - State-space input matrix.
%    C   - State-space output matrix.
%    K   - Constant feedback gain.
%    Kv  - Constant feedforward gain.
%    Q   - State weighting matrix.
%    vi  - Initial affine input.
%    xi  - Initial state.
%    r   - Time varying reference signal.

%    Mu-tools must also be installed.

%
% Written by: Kjetil Havre,  6/2-1996.
%
function [x,u,y,v] = dsoptt(A,B,C,K,Kv,Q,v0,x0,r)
   k  = getiv(r);
   nk = size(k,1);
   N  = k(nk);
   i  = k(1);
   
   x = x0; xa = x;ua = []; ya = C*x; v = v0; va = v;
   for j=1:nk-1
      v   = inv((A-B*K).')*(v - C.'*Q*vunpck( xtract(r, k(j)) ));
      u   = -K*x  + Kv*v;
      x   = A*x + B*u;
      y   = C*x;
      ua = [ua;u];
      xa = [xa;x];
      ya = [ya;y];
      va = [va;v];
   end
   x = vpck(xa,k);
   y = vpck(ya,k);
   v = vpck(va,k);
   u = vpck(ua,k(1:nk-1));
end
