root / branches / 16299_s10 / matlab / motionModel.m @ 1821
History | View | Annotate | Download (939 Bytes)
1 |
function [x, y, theta, phi, state, wheels] = motionModel(v,omega,xold,yold,thetaold,dt,state, n) |
---|---|
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 |
wheels = zeros(2, numRobots); |
15 |
|
16 |
%Transforms from v, omega into wheel speed, limits wheel speed and adds noise, then translates back. |
17 |
for i=2:numRobots |
18 |
|
19 |
wheels(:, i) = inverseTransform * [v(i); omega(i)] + noise * randn(2,1); |
20 |
wheels(:, i) = min(wheels(:, i), wheelMax); |
21 |
wheels(:, i) = max(wheels(:, i), wheelMin); |
22 |
q = transform * wheels(:, i); |
23 |
v(i) = q(1); |
24 |
omega(i) = q(2); |
25 |
|
26 |
end |
27 |
|
28 |
|
29 |
x = zeros(numRobots,1); |
30 |
y = zeros(numRobots,1); |
31 |
theta = zeros(numRobots,1); |
32 |
phi = zeros(numRobots,1); |
33 |
|
34 |
|
35 |
x = xold + cos(thetaold).*v*dt; |
36 |
y = yold + sin(thetaold).*v*dt; |
37 |
theta = thetaold + omega*dt; |
38 |
|
39 |
disp(x) |
40 |
|
41 |
phi = theta - atan2(y-y(1), x-x(1)); |
42 |
|
43 |
|
44 |
|
45 |
end |