function hw6_3_6243_2003(x0,x1)
% function hw6_3_6243_2003(x0,x1)
%
% moving the state of system
% x1'=cos(x3)u1
% x2'=sin(x3)u1
% x3'=x4*u1
% x4'=u2
% from x0 to x1 (default: randomly generated)

if nargin<2, x1=0.5*randn(4,1); end
if nargin<1, x0=0.5*randn(4,1); end

dt=0.01; % simulation step
tol=0.01; % positioning accuracy
N=4000; % maximal number of steps
x=zeros(4,N); % to keep the state values
u=zeros(2,N); % to keep the control values
kk=zeros(1,N); % to keep strategies
t=1; % current time
x(:,t)=x0(:);
x1=x1(:);

while t<N,
% g=[g1 g2 g3 g4]
g=[cos(x(3,t)) 0 0 -sin(x(3,t));sin(x(3,t)) 0 0 cos(x(3,t));x(4,t) 0 1 0;0 1 0 0];
v=g'*(x1-x(:,t))./[(1+x(4,t)^2);1;1;1]; % normalzation
if any(abs(v)>tol),
[y,k]=max(abs(v));
du=S(k,v(k));
else
du=zeros(2,N-t);
end
for t1=1:min(size(du,2),N-t), % simulation
u(:,t+1)=du(:,t1);
x(:,t+1)=x(:,t)+dt*[cos(x(3,t)) 0;sin(x(3,t)) 0;x(4,t) 0;0 1]*u(:,t+1);
kk(t+1)=k;
t=t+1;
end
end

tt=dt*(0:N-1);
close(gcf)
for k=1:4,
subplot(5,1,k);
plot(tt,x(k,:),tt,x1(k)); grid
end
subplot(5,1,5);plot(tt,0.9*kk);grid;

function u=S(k,v)
% control needed to implement the differential flow for m steps along gk
dt=0.01;
switch k,
case 1,
m=sign(v)*(floor(abs(v)/dt)+1);
u=[repmat(sign(m),1,abs(m));zeros(1,abs(m))];
case 2,
m=sign(v)*(floor(abs(v)/dt)+1);
u=[zeros(1,abs(m));repmat(sign(m),1,abs(m))];
case 3,
v1=sign(v)*sqrt(abs(v));
u=[S(2,v1) S(1,abs(v1)) S(2,-v1) S(1,-abs(v1))];
case 4,
v1=sign(v)*sqrt(abs(v));
u=[S(3,v1) S(1,abs(v1)) S(3,-v1) S(1,-abs(v1))];
end