Colony

     R = 3.5;
     L = 12.75;
     noise = 0.5;
     noise = 0%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);
     for i=2:numRobots
         wheels = inverseTransform * [v(i); omega(i)] + noise * randn(2,1);
         wheels = min(wheels, wheelMax);
     	wheels = max(wheels, wheelMin);
     	q = transform * wheels;
-...
     phi = zeros(numRobots,1);
         x = xold + cos(thetaold)*v*dt;
         y = yold(i) + sin(thetaold)*v*dt;
         theta = thetaold + omega*dt;
     x = xold + cos(thetaold).*v*dt;
     y = yold + sin(thetaold).*v*dt;
     theta = thetaold + omega*dt;
         phi = theta - atan2(y(-y(1), x-x(1));
     phi = theta - atan2(y-y(1), x-x(1));

      dt = 0.041709;
     %else:
     %dt = .01;
     tf = 40;
     tf = 5.5 %40;
     %shape numbers:
-...
     %%% output options
     makeMovie = false;
     showPlots = false;
     showPlots = true;
     doFeedback = true;
     shape = 1;
     if shape == 1
        n = 100;
        n = 10%100;
     elseif shape < 4
             n = 9;
     elseif shape == 4
-...
     mov = moviein(numsteps+1,f,winsize);
     mov(:,1) = getframe(f,winsize);
     % init
     xoldSensor = 0;
     yoldSensor = 0;
     thetaoldSensor = 0;
     % Run through each timestep
     for t = 0:dt:tf
-...
         % Update the sensor values using the sensor model
         [Xs(:,idx), Ys(:,idx), Thetas(:,idx), Phis(:,idx), sensorState, encoderNoise, xoldSensor, yoldSensor, thetaoldSensor] = ...
             sensorModel(X(:,idx), Y(:,idx), Theta(:,idx), Phi(:,idx), sensorState, n, encoderNoise, wheels, xoldSensor, yoldSensor, thetaoldSensor);
             sensorModel(X(:,idx), Y(:,idx), Theta(:,idx), Phi(:,idx), sensorState, n, encoderNoise, wheels, xoldSensor, yoldSensor, thetaoldSensor, dt);
         %Phis
         % visualize real position

     %TODO: pass in dt for lag model
     function [xSensor, ySensor, thetaSensor, phiSensor, state, encoderNoise, xoldSensor, yoldSensor, thetaoldSensor] = sensorModel(xTrue,yTrue,thetaTrue,phiTrue,state, n, encoderNoise, wheels, xoldSensor, yoldSensor, thetaoldSensor)
     function [xSensor, ySensor, thetaSensor, phiSensor, state, encoderNoise, xoldSensed, yoldSensed, thetaoldSensed] = sensorModel(xTrue,yTrue,thetaTrue,phiTrue,state, n, encoderNoise, wheels, xoldSensed, yoldSensed, thetaoldSensed, dt)
     phiNoiseVar = 0.1;
     encoderNoiseVar = 0.1;
     encoderNoiseVar = 0; %0.1;
     noiseMean = 0;
     %TODO: value in time instead of number of calls
-...
     % TODO: model encoder error?
     % Use the lagged values for position
     noise = encoderNoise * encoderNoiseVar * abs(randn(2, n));
     noise = encoderNoise .* (encoderNoiseVar * abs(randn(2, n)));
     R = 3.5;
     L = 12.75;
     transform = [ R/2 R/2; -R/L R/L];
     for i=2:numRobots,
     for i=2:n,
         wheels = wheels + noise(:, i);
     	q = transform * wheels;
-...
     end
     x = zeros(numRobots,1);
     y = zeros(numRobots,1);
     theta = zeros(numRobots,1);
     xSensor = xoldSensed;
     ySensor = yoldSensed;
     thetaSensor = thetaoldSensed;
         xoldSensed = xoldSensed + cos(thetaoldSensed)*v(i)*dt;
         yoldSensed = yoldSensed + sin(thetaoldSensed)*v(i)*dt;
         thetaoldSensed = thetaoldSensed + omega*dt;
     xoldSensed = xoldSensed + cos(thetaoldSensed).*v*dt;
     yoldSensed = yoldSensed + sin(thetaoldSensed).*v*dt;
     thetaoldSensed = thetaoldSensed + omega*dt;
     % phiSensor is the value from the BOM sensor
     % round past phi to the nearest pi/8
     noisePhi = state.phi(1,:)' + randn(1,size(phiTrue,1))'*phiNoiseVar + noiseMean;

Revision 1818