root / branches / 16299_s10 / matlab / motionModel.m @ 1812
History  View  Annotate  Download (1017 Bytes)
1 
function [x, y, theta, r, phi, state] = motionModel(v,omega,xold,yold,thetaold,dt,state) 

2  
3 
[numRobots,z] = size(v); 
4  
5 
wheelMax = [4 * pi;4 * pi]; 
6 
wheelMin = [4 * pi;4 * pi]; 
7  
8 
R = 3.5; 
9 
L = 12.75; 
10 
noise = 0.5; 
11  
12 
transform = [ R/2 R/2; R/L R/L]; 
13 
inverseTransform = inv(transform); 
14  
15 
%Transforms from v, omega into wheel speed, limits wheel speed and adds noise, then translates back. 
16 
for i=2:numRobots, 
17 

18 
wheels = inverseTransform * [v(i); omega(i)]; 
19 
wheels = min(wheels, wheelMax) + noise * randn; 
20 
wheels = max(wheels, wheelMin) + noise * randn; 
21 
q = transform * wheels; 
22 
v(i) = q(1); 
23 
omega(i) = q(2); 
24 

25 
end 
26  
27  
28 
x = zeros(numRobots,1); 
29 
y = zeros(numRobots,1); 
30 
theta = zeros(numRobots,1); 
31 
r = zeros(numRobots,1); 
32 
phi = zeros(numRobots,1); 
33  
34 
for i=1:numRobots, 
35  
36 
x(i) = xold(i) + cos(thetaold(i))*v(i)*dt; 
37 
y(i) = yold(i) + sin(thetaold(i))*v(i)*dt; 
38 
theta(i) = thetaold(i) + omega(i)*dt; 
39 

40 
r(i) = sqrt((x(i)x(1))^2 + (y(i)y(1))^2); 
41 
phi(i) = theta(1)  atan2(y(i)y(1), x(i)x(1)); 
42  
43 
end 
44  
45 
end 