xe=u(7);ye=u(8);the=u(9); vr=1;wr=1; delta1=0.05;delta2=0.05; k1=6;k2=6; s1=xe; s2=the+atan(vr*ye); Q=vr/(1+vr^2*ye^2); u4=(wr+Q*vr*sin(the)+k2*s2/(abs(s2)+delta2))/(1+Q*xe); w=u4; u3=ye*w+vr*cos(the)+k1*s1/(abs(s1)+delta1);