root / branches / 16299_s10 / matlab / motionModel.m @ 1818
History | View | Annotate | Download (866 Bytes)
1 | 1817 | cbrownel | function [x, y, theta, phi, state, wheels] = motionModel(v,omega,xold,yold,thetaold,dt,state, n) |
---|---|---|---|
2 | 1708 | bneuman | |
3 | 1713 | bneuman | [numRobots,z] = size(v); |
4 | 1708 | bneuman | |
5 | 1812 | cbrownel | wheelMax = [4 * pi;4 * pi]; |
6 | wheelMin = [-4 * pi;-4 * pi]; |
||
7 | 1795 | cbrownel | |
8 | 1812 | cbrownel | R = 3.5; |
9 | L = 12.75; |
||
10 | 1818 | bneuman | noise = 0%0.5; |
11 | 1795 | cbrownel | |
12 | transform = [ R/2 R/2; -R/L R/L]; |
||
13 | inverseTransform = inv(transform); |
||
14 | |||
15 | 1812 | cbrownel | %Transforms from v, omega into wheel speed, limits wheel speed and adds noise, then translates back. |
16 | 1818 | bneuman | for i=2:numRobots |
17 | |||
18 | wheels = inverseTransform * [v(i); omega(i)] + noise * randn(2,1); |
||
19 | 1817 | cbrownel | wheels = min(wheels, wheelMax); |
20 | wheels = max(wheels, wheelMin); |
||
21 | 1797 | bneuman | q = transform * wheels; |
22 | 1795 | cbrownel | v(i) = q(1); |
23 | omega(i) = q(2); |
||
24 | |||
25 | end |
||
26 | |||
27 | |||
28 | 1712 | bbland | x = zeros(numRobots,1); |
29 | y = zeros(numRobots,1); |
||
30 | theta = zeros(numRobots,1); |
||
31 | phi = zeros(numRobots,1); |
||
32 | |||
33 | |||
34 | 1818 | bneuman | x = xold + cos(thetaold).*v*dt; |
35 | y = yold + sin(thetaold).*v*dt; |
||
36 | theta = thetaold + omega*dt; |
||
37 | 1712 | bbland | |
38 | 1818 | bneuman | phi = theta - atan2(y-y(1), x-x(1)); |
39 | 1712 | bbland | |
40 | |||
41 | 1817 | cbrownel | |
42 | 1712 | bbland | end |