Revision 1822
don't even know
branches/16299_s10/matlab/visualizeRobots.m | ||
---|---|---|
38 | 38 |
if idx < 3 %s o we don't see giant arrows |
39 | 39 |
clf |
40 | 40 |
end |
41 |
%mov(:,idx) = getframe(f,winsize);
|
|
41 |
mov(:,idx) = getframe(f,winsize); |
|
42 | 42 |
pause(.001); |
43 | 43 |
|
44 | 44 |
end |
branches/16299_s10/matlab/motionModel.m | ||
---|---|---|
7 | 7 |
|
8 | 8 |
R = 3.5; |
9 | 9 |
L = 12.75; |
10 |
noise = 0.5;
|
|
10 |
noise = 0.1;
|
|
11 | 11 |
|
12 | 12 |
transform = [ R/2 R/2; -R/L R/L]; |
13 | 13 |
inverseTransform = inv(transform); |
... | ... | |
36 | 36 |
y = yold + sin(thetaold).*v*dt; |
37 | 37 |
theta = thetaold + omega*dt; |
38 | 38 |
|
39 |
disp(x) |
|
40 | 39 |
|
41 | 40 |
phi = theta - atan2(y-y(1), x-x(1)); |
42 | 41 |
|
branches/16299_s10/matlab/evolveRobots.m | ||
---|---|---|
1 | 1 |
% n robots |
2 | 2 |
% Robot #1 is the queen |
3 |
n = 13;
|
|
3 |
n = 15;
|
|
4 | 4 |
%for movie output use: |
5 |
dt = 0.041709;
|
|
5 |
dt = 0.041709; |
|
6 | 6 |
%else: |
7 |
%dt = .01;
|
|
7 |
%dt = .1; |
|
8 | 8 |
tf = 20 %40; |
9 | 9 |
|
10 | 10 |
|
... | ... | |
19 | 19 |
numsteps = ceil(tf / dt) + 1; |
20 | 20 |
|
21 | 21 |
%%% output options |
22 |
makeMovie = false;
|
|
23 |
showPlots = true;
|
|
22 |
makeMovie = true;
|
|
23 |
showPlots = false;
|
|
24 | 24 |
doFeedback = true; |
25 | 25 |
shape = 1; |
26 | 26 |
|
27 | 27 |
if shape == 1 |
28 | 28 |
n = 65; |
29 |
elseif shape < 4
|
|
29 |
elseif shape == 2
|
|
30 | 30 |
n = 65; |
31 |
elseif shape == 3 |
|
32 |
n = 65; |
|
31 | 33 |
elseif shape == 4 |
32 | 34 |
n = 65; |
33 | 35 |
elseif shape == 5 |
... | ... | |
56 | 58 |
% if we aren't doing feedback just pick random speeds for everyone so |
57 | 59 |
% we can see some open loop motion |
58 | 60 |
%HACK |
59 |
V(2,:) = 2; |
|
60 |
for i=3:n
|
|
61 |
X(i,1) = rand*10 - 5; |
|
62 |
Y(i,1) = rand*10 - 5;
|
|
63 |
V(i,:) = rand*10;
|
|
64 |
W(i,:) = rand*2*pi;
|
|
61 |
%V(2,:) = 2;
|
|
62 |
for i=2:n
|
|
63 |
%X(i,1) = rand*10 - 5;
|
|
64 |
Y(i,1) = -2.5;
|
|
65 |
V(i,:) = 6;
|
|
66 |
W(i,:) = 2.5;
|
|
65 | 67 |
end |
66 | 68 |
else |
67 | 69 |
|
... | ... | |
158 | 160 |
|
159 | 161 |
color = hsv(n); |
160 | 162 |
|
161 |
encoderNoise = 3 * randn(2, n);
|
|
163 |
encoderNoise = .01 * randn(2, n);
|
|
162 | 164 |
|
163 | 165 |
|
164 | 166 |
% The things that end in s are the sensor values |
... | ... | |
232 | 234 |
if showPlots |
233 | 235 |
figure; |
234 | 236 |
hold on; |
235 |
title('true X vs. sensed X'); |
|
237 |
%title('true X vs. sensed X');
|
|
236 | 238 |
% plot(1:idx-1, X(1,:),1:idx-1, Xs(1,:)) |
237 |
plot(1:idx-1, X(2,:),1:idx-1, Xs(2,:)) |
|
239 |
%plot(1:idx-1, X(2,:) - Xs(2,:),1:idx-1, ) |
|
240 |
%legend('true x', 'sensed x') |
|
241 |
|
|
242 |
title('Error'); |
|
243 |
% plot(1:idx-1, X(1,:),1:idx-1, Xs(1,:)) |
|
244 |
for i=2:n |
|
245 |
plot(0:dt:(tf+dt), abs(X(i,:)) .* abs(Y(i,:)) - abs(Xs(i,:)) .* abs(Ys(i,:))) |
|
246 |
end |
|
247 |
%legend('true x', 'sensed x') |
|
238 | 248 |
end |
239 | 249 |
|
240 | 250 |
if makeMovie |
241 | 251 |
disp('making movie...'); |
242 |
movie2avi(mov,'movie.avi'); |
|
252 |
movie2avi(mov,'movie.avi', 'fps', 23.976);
|
|
243 | 253 |
disp('movie.avi created!'); |
244 | 254 |
end |
branches/16299_s10/matlab/sensorModel.m | ||
---|---|---|
2 | 2 |
function [xSensor, ySensor, thetaSensor, phiSensor, state, encoderNoise, xoldSensed, yoldSensed, thetaoldSensed, noise] = sensorModel(xTrue,yTrue,thetaTrue,phiTrue,state, n, encoderNoise, wheels, xoldSensed, yoldSensed, thetaoldSensed, dt, noise) |
3 | 3 |
|
4 | 4 |
phiNoiseVar = 0.1; |
5 |
encoderNoiseVar = 0.05;
|
|
5 |
encoderNoiseVar = 0.001;
|
|
6 | 6 |
noiseMean = 0; |
7 | 7 |
|
8 | 8 |
%TODO: value in time instead of number of calls |
... | ... | |
20 | 20 |
% Use the lagged values for position |
21 | 21 |
|
22 | 22 |
|
23 |
noise = noise + (encoderNoise .* (encoderNoiseVar * abs(randn(2, n))) ./ wheels);
|
|
23 |
noise = noise + ((encoderNoise .* (encoderNoiseVar * abs(randn(2, n)))) .* wheels);
|
|
24 | 24 |
|
25 | 25 |
|
26 | 26 |
R = 3.5; |
... | ... | |
41 | 41 |
|
42 | 42 |
end |
43 | 43 |
|
44 |
disp(wheels) |
|
45 | 44 |
|
46 | 45 |
xSensor = xoldSensed; |
47 | 46 |
ySensor = yoldSensed; |
Also available in: Unified diff