kinematics.m

function y = kynematics(u)
init;
y = [
    1/2*(u(1)+u(2))*cos(u(3))
    1/2*(u(1)+u(2))*sin(u(3))
    (u(2)-u(1))/robot.w];

---