%
%  Function SOPTC simualtes continouos time-varying state feedback 
%  controller. Model is:   
%     \dot{x} =  Ax + Bu;
%         u   = -K(t)x(t)
%  Input variables:
%     A    - State-space A matrix.
%     B    - Sate-space input matrix.
%     K(t) - Time varying state feedback control law. 
%  Output variables:
%     x(t) - State trajectory.
%     u(t) - Control trajectory.
%
function [x,u] = soptc(A,B,K,x0)
   t = getiv(K);
   
   nt = size(t,1);
   
   xd = x0; ud = -vunpck(xtract(K,t(1)))*xd; xh = x0;
   for i = 2:nt;
      xh =  expm( (A-B*vunpck(xtract(K,t(i))))*(t(i)-t(i-1)) )*xh;
      xd = [xd; xh];
      ud = [ud; -vunpck(xtract(K,t(i)))*xh];
   end
   u = vpck(ud,t);
   x = vpck(xd,t);
end
