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

sensorModel.m (revision 1820)
1 1 
%TODO: pass in dt for lag model
2 
function [xSensor, ySensor, thetaSensor, phiSensor, state, encoderNoise, xoldSensed, yoldSensed, thetaoldSensed] = sensorModel(xTrue,yTrue,thetaTrue,phiTrue,state, n, encoderNoise, wheels, xoldSensed, yoldSensed, thetaoldSensed, dt)
2 
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)
3 3 

  		
  




  4
  4
    
    
phiNoiseVar = 0.1;


  		
  




  5
  
    
    
encoderNoiseVar = 0; %0.1;


  		
  




  
  5
    
    
encoderNoiseVar = 0.05;


  		
  




  6
  6
    
    
noiseMean = 0;


  		
  




  7
  7
    
    

  		
  




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


  		
  




......




  19
  19
    
    
% TODO: model encoder error?


  		
  




  20
  20
    
    
% Use the lagged values for position


  		
  




  21
  21
    
    

  		
  




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


  		
  




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


  		
  




  23
  23
    
    

  		
  




  
  24
    
    

  		
  




  24
  25
    
    
R = 3.5;


  		
  




  25
  26
    
    
L = 12.75;


  		
  




  26
  27
    
    

  		
  




......




  39
  40
    
    
ySensor = yoldSensed;


  		
  




  40
  41
    
    
thetaSensor = thetaoldSensed;


  		
  




  41
  42
    
    

  		
  




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


  		
  




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


  		
  




  44
  
    
    
thetaoldSensed = thetaoldSensed + omega*dt;


  		
  




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


  		
  




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


  		
  




  
  45
    
    
% thetaoldSensed = thetaoldSensed + omega*dt;


  		
  




  45
  46
    
    

  		
  




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


  		
  




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


  		
  




  
  49
    
    
thetaoldSensed = state.theta(1,:) + omega*dt;


  		
  




  46
  50
    
    

  		
  




  
  51
    
    

  		
  




  
  52
    
    

  		
  




  47
  53
    
    
% phiSensor is the value from the BOM sensor


  		
  




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


  		
  




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


  		
  












Also available in:
	Unified diff