root / branches / 16299_s10 / matlab / motionModel.m @ 1810
History | View | Annotate | Download (1016 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 = [100; 100]; |
6 |
wheelMin = [-100; -100]; |
7 |
|
8 |
R = 2.5; |
9 |
L = 12; |
10 |
noise = .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=1:numRobots, |
17 |
|
18 |
wheels = inverseTransform * [v(i); omega(i)]; |
19 |
wheels = min(wheels, wheelMax) + noise * (rand - .5); |
20 |
wheels = max(wheels, wheelMin) + noise * (rand - .5); |
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 |