Colony

%TODO: pass in dt for lag model

function [xSensor, ySensor, thetaSensor, phiSensor, state] = sensorModel(xTrue,yTrue,thetaTrue,phiTrue,state)

noiseVar = 0.5;

noiseMean = 0;

%TODO: value in time instead of number of calls

sensorLag = 10;

% 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

xSensor = state.x(1,:)';

ySensor = state.y(1,:)';

thetaSensor = state.theta(1,:)';

% phiSensor is the value from the BOM sensor

% round past phi to the nearest pi/8

noisePhi = state.phi(1,:)' + randn()*noiseVar + 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