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);

r = l*(sl + sr)/(2*(sr-sl));

G = eye(3);
if r == Inf
    % Special case: Robot is driving straight:
    d = (sl+sr)/2;
    xnew = x + [d*cos(theta); ...
                d*sin(theta); ...
                zeros(1,1,size(x,3))];
else
	dTh = repmat((sr - sl)/l,1,1,size(x,3));
	xnew = x + [r*sin(theta + dTh) - r*sin(theta); ...
               -r*cos(theta + dTh) + r*cos(theta); ...
                dTh];
            
    if (size(x,3) == 1)
        G = [1, 0, r*(cos(theta + dTh) - cos(theta)); ...
            0, 1, r*(sin(theta + dTh) - sin(theta)); ...
            0, 0, 1];
    end
end % if

end