Colony

%TODO: pass in dt for lag model

function [xSensor, ySensor, thetaSensor, phiSensor, state, encoderNoise, xoldSensed, yoldSensed, thetaoldSensed, noise] = sensorModel(xTrue,yTrue,thetaTrue,phiTrue,state, n, encoderNoise, wheels, xoldSensed, yoldSensed, thetaoldSensed, dt, noise)

phiNoiseVar = 0.1;

encoderNoiseVar = 0.001;

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 = noise + ((encoderNoise .* (encoderNoiseVar * abs(randn(2, n)))) .* wheels);

R = 3.5;

L = 12.75;

transform = [ R/2 R/2; -R/L R/L];

for i=2:n,

    wheels(:, i) = wheels(:, i) + noise(:, i) ;

	q = transform * wheels(:, i);

	v(i) = q(1);

	omega(i) = q(2);

	% disp(wheels)

end

xSensor = xoldSensed;

ySensor = yoldSensed;

thetaSensor = thetaoldSensed;

% xoldSensed = xoldSensed + cos(thetaoldSensed).*v*dt;

% yoldSensed = yoldSensed + sin(thetaoldSensed).*v*dt;

% thetaoldSensed = thetaoldSensed + omega*dt;

xoldSensed = state.x(1,:) + cos(state.theta(1,:)).*v*dt;

yoldSensed = state.y(1,:) + sin(state.theta(1,:)).*v*dt;

thetaoldSensed = state.theta(1,:) + 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