function [x, y, theta, r, phi, state] = motionModel(v,omega,xold,yold,thetaold,dt,state)

[numRobots,z] = size(v);

wheelMax = [4 * pi;4 * pi];
wheelMin = [-4 * pi;-4 * pi];

R = 3.5;
L = 12.75;
noise = 0.5;

transform = [ R/2 R/2; -R/L R/L];
inverseTransform = inv(transform);

%Transforms from v, omega into wheel speed, limits wheel speed and adds noise, then translates back.
for i=2:numRobots,
	
    wheels = inverseTransform * [v(i); omega(i)];
    wheels = min(wheels, wheelMax) + noise * randn; 
	wheels = max(wheels, wheelMin) + noise * randn;
	q = transform * wheels;
	v(i) = q(1);
	omega(i) = q(2);
	
end 


x = zeros(numRobots,1);
y = zeros(numRobots,1);
theta = zeros(numRobots,1);
r = zeros(numRobots,1);
phi = zeros(numRobots,1);

for i=1:numRobots,

    x(i) = xold(i) + cos(thetaold(i))*v(i)*dt;
    y(i) = yold(i) + sin(thetaold(i))*v(i)*dt;
    theta(i) = thetaold(i) + omega(i)*dt;
    
    r(i) = sqrt((x(i)-x(1))^2 + (y(i)-y(1))^2);
    phi(i) = theta(1) - atan2(y(i)-y(1), x(i)-x(1));

end

end