function [ xnew G] = motion_diff( x, u )
%MOTION_DIFF Compute differential drive motion model

l = 0.1; % distance between both wheels

theta = x(3);
sl = u(1);
sr = u(2);

% YOUR CODE STARTS HERE %
syms x1 x2 x3

if (sl == sr)
    xnew=x+sl*[cos(theta);sin(theta);0];
    G=jacobian([x1;x2;x3]+sl*[cos(x3);sin(x3);0],[x1;x2;x3]); 
else
    r=(sl+sr)/(sr-sl)*l/2;
    dtheta=(sr-sl)/l;
    xnew=x+[r*(sin(theta+dtheta)-sin(theta));r*(-cos(theta+dtheta)+cos(theta));dtheta];
    G=jacobian([x1;x2;x3]+[r*(sin(x3+dtheta)-sin(x3));r*(-cos(x3+dtheta)+cos(x3));dtheta],[x1;x2;x3]);
end
x3=theta;
G=eval(G);
% YOUR CODE ENDS HERE %     

end

