Statistics
| Revision:

## root / branches / 16299_s10 / matlab / motionModel.m @ 1812

 1 ```function [x, y, theta, r, phi, state] = motionModel(v,omega,xold,yold,thetaold,dt,state) ``` ```[numRobots,z] = size(v); ``` ```wheelMax = [4 * pi;4 * pi]; ``` ```wheelMin = [-4 * pi;-4 * pi]; ``` ```R = 3.5; ``` ```L = 12.75; ``` ```noise = 0.5; ``` ```transform = [ R/2 R/2; -R/L R/L]; ``` ```inverseTransform = inv(transform); ``` ```%Transforms from v, omega into wheel speed, limits wheel speed and adds noise, then translates back. ``` ```for i=2:numRobots, ``` ``` ``` ``` wheels = inverseTransform * [v(i); omega(i)]; ``` ``` wheels = min(wheels, wheelMax) + noise * randn; ``` ``` wheels = max(wheels, wheelMin) + noise * randn; ``` ``` q = transform * wheels; ``` ``` v(i) = q(1); ``` ``` omega(i) = q(2); ``` ``` ``` ```end ``` ```x = zeros(numRobots,1); ``` ```y = zeros(numRobots,1); ``` ```theta = zeros(numRobots,1); ``` ```r = zeros(numRobots,1); ``` ```phi = zeros(numRobots,1); ``` ```for i=1:numRobots, ``` ``` x(i) = xold(i) + cos(thetaold(i))*v(i)*dt; ``` ``` y(i) = yold(i) + sin(thetaold(i))*v(i)*dt; ``` ``` theta(i) = thetaold(i) + omega(i)*dt; ``` ``` ``` ``` r(i) = sqrt((x(i)-x(1))^2 + (y(i)-y(1))^2); ``` ``` phi(i) = theta(1) - atan2(y(i)-y(1), x(i)-x(1)); ``` ```end ``` `end`