Revision 1818
some bugfixes, stuff still isn't working though
| motionModel.m | ||
|---|---|---|
| 7 | 7 |
|
| 8 | 8 |
R = 3.5; |
| 9 | 9 |
L = 12.75; |
| 10 |
noise = 0.5; |
|
| 10 |
noise = 0%0.5;
|
|
| 11 | 11 |
|
| 12 | 12 |
transform = [ R/2 R/2; -R/L R/L]; |
| 13 | 13 |
inverseTransform = inv(transform); |
| 14 | 14 |
|
| 15 | 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)] + noise * randn(2, n);
|
|
| 16 |
for i=2:numRobots |
|
| 17 |
|
|
| 18 |
wheels = inverseTransform * [v(i); omega(i)] + noise * randn(2,1);
|
|
| 19 | 19 |
wheels = min(wheels, wheelMax); |
| 20 | 20 |
wheels = max(wheels, wheelMin); |
| 21 | 21 |
q = transform * wheels; |
| ... | ... | |
| 31 | 31 |
phi = zeros(numRobots,1); |
| 32 | 32 |
|
| 33 | 33 |
|
| 34 |
|
|
| 35 |
x = xold + cos(thetaold)*v*dt; |
|
| 36 |
y = yold(i) + sin(thetaold)*v*dt; |
|
| 37 |
theta = thetaold + omega*dt; |
|
| 34 |
x = xold + cos(thetaold).*v*dt; |
|
| 35 |
y = yold + sin(thetaold).*v*dt; |
|
| 36 |
theta = thetaold + omega*dt; |
|
| 38 | 37 |
|
| 39 |
phi = theta - atan2(y(-y(1), x-x(1));
|
|
| 38 |
phi = theta - atan2(y-y(1), x-x(1));
|
|
| 40 | 39 |
|
| 41 | 40 |
|
| 42 | 41 |
|
Also available in: Unified diff