Revision 1818 branches/16299_s10/matlab/sensorModel.m

sensorModel.m (revision 1818)
1 1 
%TODO: pass in dt for lag model
2 
function [xSensor, ySensor, thetaSensor, phiSensor, state, encoderNoise, xoldSensor, yoldSensor, thetaoldSensor] = sensorModel(xTrue,yTrue,thetaTrue,phiTrue,state, n, encoderNoise, wheels, xoldSensor, yoldSensor, thetaoldSensor)
2 
function [xSensor, ySensor, thetaSensor, phiSensor, state, encoderNoise, xoldSensed, yoldSensed, thetaoldSensed] = sensorModel(xTrue,yTrue,thetaTrue,phiTrue,state, n, encoderNoise, wheels, xoldSensed, yoldSensed, thetaoldSensed, dt)
3 3 

  		
  




  4
  4
    
    
phiNoiseVar = 0.1;


  		
  




  5
  
    
    
encoderNoiseVar = 0.1;


  		
  




  
  5
    
    
encoderNoiseVar = 0; %0.1;


  		
  




  6
  6
    
    
noiseMean = 0;


  		
  




  7
  7
    
    

  		
  




  8
  8
    
    
%TODO: value in time instead of number of calls


  		
  




......




  18
  18
    
    

  		
  




  19
  19
    
    
% TODO: model encoder error?


  		
  




  20
  20
    
    
% Use the lagged values for position


  		
  




  21
  
    
    
noise = encoderNoise * encoderNoiseVar * abs(randn(2, n));


  		
  




  22
  21
    
    

  		
  




  
  22
    
    
noise = encoderNoise .* (encoderNoiseVar * abs(randn(2, n)));


  		
  




  
  23
    
    

  		
  




  23
  24
    
    
R = 3.5;


  		
  




  24
  25
    
    
L = 12.75;


  		
  




  25
  26
    
    

  		
  




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


  		
  




  27
  28
    
    

  		
  




  28
  
    
    
for i=2:numRobots,


  		
  




  
  29
    
    
for i=2:n,


  		
  




  29
  30
    
    
	


  		
  




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


  		
  




  31
  32
    
    
	q = transform * wheels;


  		
  




......




  34
  35
    
    
	


  		
  




  35
  36
    
    
end


  		
  




  36
  37
    
    

  		
  




  37
  
    
    
x = zeros(numRobots,1);


  		
  




  38
  
    
    
y = zeros(numRobots,1);


  		
  




  39
  
    
    
theta = zeros(numRobots,1);


  		
  




  40
  
    
    

  		
  




  41
  38
    
    
xSensor = xoldSensed;


  		
  




  42
  39
    
    
ySensor = yoldSensed;


  		
  




  43
  40
    
    
thetaSensor = thetaoldSensed;


  		
  




  44
  41
    
    

  		
  




  45
  
    
    
    xoldSensed = xoldSensed + cos(thetaoldSensed)*v(i)*dt;


  		
  




  46
  
    
    
    yoldSensed = yoldSensed + sin(thetaoldSensed)*v(i)*dt;


  		
  




  47
  
    
    
    thetaoldSensed = thetaoldSensed + omega*dt;


  		
  




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


  		
  




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


  		
  




  
  44
    
    
thetaoldSensed = thetaoldSensed + omega*dt;


  		
  




  48
  45
    
    

  		
  




  49
  46
    
    

  		
  




  50
  
    
    

  		
  




  51
  
    
    

  		
  




  52
  
    
    

  		
  




  53
  
    
    

  		
  




  54
  47
    
    
% phiSensor is the value from the BOM sensor


  		
  




  55
  48
    
    
% round past phi to the nearest pi/8


  		
  




  56
  49
    
    
noisePhi = state.phi(1,:)' + randn(1,size(phiTrue,1))'*phiNoiseVar + noiseMean;


  		
  












Also available in:
	Unified diff