Revision 1810
Adds noise to wheel speeds
branches/16299_s10/matlab/motionModel.m | ||
---|---|---|
7 | 7 |
|
8 | 8 |
R = 2.5; |
9 | 9 |
L = 12; |
10 |
noise = .5; |
|
10 | 11 |
|
11 | 12 |
transform = [ R/2 R/2; -R/L R/L]; |
12 | 13 |
inverseTransform = inv(transform); |
13 | 14 |
|
15 |
//Transforms from v, omega into wheel speed, limits wheel speed and adds noise, then translates back. |
|
14 | 16 |
for i=1:numRobots, |
15 | 17 |
|
16 | 18 |
wheels = inverseTransform * [v(i); omega(i)]; |
17 |
wheels = min(wheels, wheelMax);
|
|
18 |
wheels = max(wheels, wheelMin); |
|
19 |
wheels = min(wheels, wheelMax) + noise * (rand - .5);
|
|
20 |
wheels = max(wheels, wheelMin) + noise * (rand - .5);
|
|
19 | 21 |
q = transform * wheels; |
20 | 22 |
v(i) = q(1); |
21 | 23 |
omega(i) = q(2); |
Also available in: Unified diff