Colony

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

[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)] + noise * randn(2, n);

    wheels = min(wheels, wheelMax); 

	wheels = max(wheels, wheelMin);

	q = transform * wheels;

	v(i) = q(1);

	omega(i) = q(2);

end 

x = zeros(numRobots,1);

y = zeros(numRobots,1);

theta = zeros(numRobots,1);

phi = zeros(numRobots,1);

    x = xold + cos(thetaold)*v*dt;

    y = yold(i) + sin(thetaold)*v*dt;

    theta = thetaold + omega*dt;

    phi = theta - atan2(y(-y(1), x-x(1));

end