Colony

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

phiNoiseVar = 0.1;

encoderNoiseVar = 0.1;

noiseMean = 0;

%TODO: value in time instead of number of calls

sensorLag = 2;

% state contains historical phi values

if size(state,1) == 0

    state.phi = zeros(sensorLag,size(phiTrue,1));

    state.x = zeros(sensorLag,size(phiTrue,1));

    state.y = zeros(sensorLag,size(phiTrue,1));

    state.theta = zeros(sensorLag,size(phiTrue,1));

end

% TODO: model encoder error?

% Use the lagged values for position

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,

    wheels = wheels + noise(:, i);

	q = transform * wheels;

	v(i) = q(1);

	omega(i) = q(2);

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;

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

phiSensor = round(noisePhi*8/pi)*pi/8;

%phiSensor = round(state.phi(1,:)'*8/pi)*pi/8;

% update the state

state.x(1:end-1,:) = state.x(2:end,:);

state.y(1:end-1,:) = state.y(2:end,:);

state.theta(1:end-1,:) = state.theta(2:end,:);

state.phi(1:end-1,:) = state.phi(2:end,:);

state.x(end,:) = xTrue';

state.y(end,:) = yTrue';

state.theta(end,:) = thetaTrue';

state.phi(end,:) = phiTrue';

end