function [xnew, xnewdot] = robot(x, u)

B = 0.5;


xnew = x
u

xnew(1) = x(1) + u(1)*cos(x(3)+u(2));
xnew(2) = x(2) + u(1)*sin(x(3)+u(2));
xnew(3) = x(3) + u(1)/B*sin(u(2));

xnewdot = [ 1 0 -u(1)*sin(x(3)+u(2))
            0 1  u(1)*cos(x(3)+u(2))
            0 0  1                   ];
