Assignment code for Georgia Tech CS 3630, 2014
Update Differential.m
+8
-7
+8
-7
Assignment_3/code/matlab/Differential.m
+8
-7
Assignment_3/code/matlab/Differential.m
···
227
227
uR=u(2);
228
228
229
229
% Measure constants
230
-
r = 0; %% <------ IMPLEMENT
231
-
L = 0; %% <------ IMPLEMENT
230
+
r = 1.375; %% <------ IMPLEMENT
231
+
L = 5.75; %% <------ IMPLEMENT
232
232
233
233
% calculate speed
234
-
vx = 0; %% <------ IMPLEMENT
235
-
omega = 0; %% <------ IMPLEMENT
234
+
vx = (r / 2) * (uR + uL); %% <------ IMPLEMENT
235
+
omega = (r / L) * (uR - uL); %% <------ IMPLEMENT
236
236
237
237
% update new state using Euler integration, not expmap
238
238
xp = veh.x; % previous state
239
-
veh.x(1) = 0; %% <------ IMPLEMENT
240
-
veh.x(2) = 0; %% <------ IMPLEMENT
241
-
veh.x(3) = 0; %% <------ IMPLEMENT
239
+
R = (vx / omega);
240
+
veh.x(1) = xp(1) + R * sin(omega * veh.dt); %% <------ IMPLEMENT
241
+
veh.x(2) = xp(2) + R - R * cos(omega * veh.dt); %% <------ IMPLEMENT
242
+
veh.x(3) = xp(3) + omega * veh.dt; %% <------ IMPLEMENT
242
243
243
244
odo = [vx*veh.dt omega*veh.dt];
244
245
veh.odometry = odo;