Revision 1820
| branches/16299_s10/matlab/motionModel.m (revision 1820) | ||
|---|---|---|
| 7 | 7 |
|
| 8 | 8 |
R = 3.5; |
| 9 | 9 |
L = 12.75; |
| 10 |
noise = 0%0.5; |
|
| 10 |
noise = 0.5; |
|
| 11 | 11 |
|
| 12 | 12 |
transform = [ R/2 R/2; -R/L R/L]; |
| 13 | 13 |
inverseTransform = inv(transform); |
| branches/16299_s10/matlab/evolveRobots.m (revision 1820) | ||
|---|---|---|
| 21 | 21 |
%%% output options |
| 22 | 22 |
makeMovie = false; |
| 23 | 23 |
showPlots = true; |
| 24 |
doFeedback = true; |
|
| 24 |
doFeedback = false; |
|
| 25 | 25 |
shape = 4; |
| 26 | 26 |
|
| 27 | 27 |
if shape == 1 |
| ... | ... | |
| 55 | 55 |
if doFeedback == false |
| 56 | 56 |
% if we aren't doing feedback just pick random speeds for everyone so |
| 57 | 57 |
% we can see some open loop motion |
| 58 |
for i=1:n |
|
| 58 |
%HACK |
|
| 59 |
V(2,:) = 2; |
|
| 60 |
for i=3:n |
|
| 59 | 61 |
X(i,1) = rand*10 - 5; |
| 60 | 62 |
Y(i,1) = rand*10 - 5; |
| 61 | 63 |
V(i,:) = rand*10; |
| ... | ... | |
| 156 | 158 |
|
| 157 | 159 |
color = hsv(n); |
| 158 | 160 |
|
| 159 |
encoderNoise = .01 * randn(2, n); |
|
| 161 |
encoderNoise = 1 * randn(2, n); |
|
| 160 | 162 |
|
| 161 | 163 |
|
| 162 | 164 |
% The things that end in s are the sensor values |
| ... | ... | |
| 190 | 192 |
xoldSensor = 0; |
| 191 | 193 |
yoldSensor = 0; |
| 192 | 194 |
thetaoldSensor = 0; |
| 195 |
noise = 0; |
|
| 193 | 196 |
|
| 194 | 197 |
% Run through each timestep |
| 195 | 198 |
for t = 0:dt:tf |
| ... | ... | |
| 202 | 205 |
%Phi |
| 203 | 206 |
|
| 204 | 207 |
% Update the sensor values using the sensor model |
| 205 |
[Xs(:,idx), Ys(:,idx), Thetas(:,idx), Phis(:,idx), sensorState, encoderNoise, xoldSensor, yoldSensor, thetaoldSensor] = ... |
|
| 206 |
sensorModel(X(:,idx), Y(:,idx), Theta(:,idx), Phi(:,idx), sensorState, n, encoderNoise, wheels, xoldSensor, yoldSensor, thetaoldSensor, dt); |
|
| 208 |
[Xs(:,idx), Ys(:,idx), Thetas(:,idx), Phis(:,idx), sensorState, encoderNoise, xoldSensor, yoldSensor, thetaoldSensor, noise] = ... |
|
| 209 |
sensorModel(X(:,idx), Y(:,idx), Theta(:,idx), Phi(:,idx), sensorState, n, encoderNoise, wheels, xoldSensor, yoldSensor, thetaoldSensor, dt, noise); |
|
| 207 | 210 |
%Phis |
| 208 | 211 |
|
| 209 | 212 |
% visualize real position |
| ... | ... | |
| 230 | 233 |
figure; |
| 231 | 234 |
hold on; |
| 232 | 235 |
title('true X vs. sensed X');
|
| 233 |
plot(1:idx-1, X(1,:),1:idx-1, Xs(1,:)) |
|
| 236 |
% plot(1:idx-1, X(1,:),1:idx-1, Xs(1,:)) |
|
| 234 | 237 |
plot(1:idx-1, X(2,:),1:idx-1, Xs(2,:)) |
| 235 | 238 |
end |
| 236 | 239 |
|
| branches/16299_s10/matlab/sensorModel.m (revision 1820) | ||
|---|---|---|
| 1 | 1 |
%TODO: pass in dt for lag model |
| 2 |
function [xSensor, ySensor, thetaSensor, phiSensor, state, encoderNoise, xoldSensed, yoldSensed, thetaoldSensed] = sensorModel(xTrue,yTrue,thetaTrue,phiTrue,state, n, encoderNoise, wheels, xoldSensed, yoldSensed, thetaoldSensed, dt) |
|
| 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; %0.1; |
|
| 5 |
encoderNoiseVar = 0.05; |
|
| 6 | 6 |
noiseMean = 0; |
| 7 | 7 |
|
| 8 | 8 |
%TODO: value in time instead of number of calls |
| ... | ... | |
| 19 | 19 |
% TODO: model encoder error? |
| 20 | 20 |
% Use the lagged values for position |
| 21 | 21 |
|
| 22 |
noise = encoderNoise .* (encoderNoiseVar * abs(randn(2, n))); |
|
| 22 |
noise = noise + encoderNoise .* (encoderNoiseVar * abs(randn(2, n))); |
|
| 23 | 23 |
|
| 24 |
|
|
| 24 | 25 |
R = 3.5; |
| 25 | 26 |
L = 12.75; |
| 26 | 27 |
|
| ... | ... | |
| 39 | 40 |
ySensor = yoldSensed; |
| 40 | 41 |
thetaSensor = thetaoldSensed; |
| 41 | 42 |
|
| 42 |
xoldSensed = xoldSensed + cos(thetaoldSensed).*v*dt; |
|
| 43 |
yoldSensed = yoldSensed + sin(thetaoldSensed).*v*dt; |
|
| 44 |
thetaoldSensed = thetaoldSensed + omega*dt; |
|
| 43 |
% xoldSensed = xoldSensed + cos(thetaoldSensed).*v*dt; |
|
| 44 |
% yoldSensed = yoldSensed + sin(thetaoldSensed).*v*dt; |
|
| 45 |
% thetaoldSensed = thetaoldSensed + omega*dt; |
|
| 45 | 46 |
|
| 47 |
xoldSensed = state.x(1,:) + cos(state.theta(1,:)).*v*dt; |
|
| 48 |
yoldSensed = state.y(1,:) + sin(state.theta(1,:)).*v*dt; |
|
| 49 |
thetaoldSensed = state.theta(1,:) + omega*dt; |
|
| 46 | 50 |
|
| 51 |
|
|
| 52 |
|
|
| 47 | 53 |
% phiSensor is the value from the BOM sensor |
| 48 | 54 |
% round past phi to the nearest pi/8 |
| 49 | 55 |
noisePhi = state.phi(1,:)' + randn(1,size(phiTrue,1))'*phiNoiseVar + noiseMean; |
Also available in: Unified diff