Assignment code for Georgia Tech CS 3630, 2014
Merge branch 'master' of https://github.com/james7132/CS-3630-Huynh-Liu
Conflicts:
Assignment 3/code/matlab/RangeSensor.m
Assignment 3/code/matlab/log-1423005664.txt
Assignment_3/code/matlab/log-1423005664.txt
Assignment_3/code/matlab/log-boxworld.txt
+18
-15
+6
-6
Assignment 3/code/matlab/RangeSensor.m
+6
-6
Assignment 3/code/matlab/RangeSensor.m
···
277
277
% % A lower score means it is less likely to be selected for the next generation...
278
278
% % The weight is never zero.
279
279
% end
280
-
281
-
282
-
weights = zeros(length(x),1);
280
+
weights = ones(length(x),1);
283
281
284
282
for i = 1 : length(x)
285
283
z_actual = z;
286
284
z_pred = s.h(x(i,:)');
287
285
z_diff = z_actual - z_pred;
288
-
innov = z_actual - s.Hx * x(i, :);
289
-
290
-
weights(i) = exp(-0.5 * z_diff ^ 2 - innov);
286
+
weights(i) = exp(-0.5 * z_diff' * L * z_diff) + weights(i);
287
+
% Equation from:
288
+
% Robotics, Vision & Control, Chap 6,
289
+
% Peter Corke,
290
+
% Springer 2011
291
291
end
292
292
weights
293
293
end
Assignment 3/code/matlab/log-1423005664.txt
Assignment 3/code/matlab/log-boxworld.txt
Assignment 3/code/matlab/log-1423005664.txt
Assignment 3/code/matlab/log-boxworld.txt
+6
-5
Assignment 3/code/matlab/script01_MotionModel.m
+6
-5
Assignment 3/code/matlab/script01_MotionModel.m
···
1
1
% demo08_squareWorldMCL
2
2
% Demo generic Monte Carlo Localization
3
+
path(path,'threedee')
3
4
4
-
path(path,'threedee')
5
+
log = 'log-boxworld.txt'
5
6
6
7
%% Create a map
7
8
A = [
···
19
20
%% and a robot with noisy odometry
20
21
V=diag([0.01, 0.1*pi/180].^2)
21
22
veh=Differential(V, 'x0', [-20, -3, 0])
22
-
veh.add_driver(DeterministicPath('log-1423005664.txt'));
23
+
veh.add_driver(DeterministicPath(log));
23
24
24
25
%% and then a sensor with noisy readings
25
26
W=0.05^2;
26
-
sensor = RangeSensor(veh,map, W,'log-1423005664.txt')
27
+
sensor = RangeSensor(veh,map, W,log)
27
28
28
29
%% define two covariances for random noise Q and L (hmmm!)
29
30
% For Q, use the uncertainly estimates from A2!
30
-
Q = diag([0.01,0.01,0.1*pi/180].^2);
31
-
L = diag(0.1);
31
+
Q = 25 .* diag([0.01,0.01,0.1*pi/180].^2);
32
+
L = 25 .* diag(0.1);
32
33
33
34
%% Finally, construct ParticleFilter
34
35
pf = GenericParticleFilter(veh, sensor, Q, L, 200);
+6
-4
Assignment 3/code/matlab/script02_SensorModel.m
+6
-4
Assignment 3/code/matlab/script02_SensorModel.m
···
2
2
% Demo generic Monte Carlo Localization
3
3
path(path,'threedee')
4
4
5
+
log = 'log-boxworld.txt'
6
+
5
7
%% Create a map
6
8
A = [
7
9
0, 0, 15.5, 11.5
···
18
20
%% and a robot with noisy odometry
19
21
V=diag([0.01, 0.1*pi/180].^2)
20
22
veh=Differential(V, 'x0', [-20, -3, 0])
21
-
veh.add_driver(DeterministicPath('log-1423005664.txt'));
23
+
veh.add_driver(DeterministicPath(log));
22
24
23
25
%% and then a sensor with noisy readings
24
26
W=0.05^2;
25
-
sensor = RangeSensor(veh,map, W,'log-1423005664.txt')
27
+
sensor = RangeSensor(veh,map, W, log)
26
28
27
29
%% define two covariances for random noise Q and L (hmmm!)
28
30
% For Q, use the uncertainly estimates from A2!
29
-
Q = diag([0.01,0.01,0.1*pi/180].^2);
30
-
L = diag(0.1);
31
+
Q = 25 .* diag([0.01,0.01,0.1*pi/180].^2);
32
+
L = 25 .* diag(0.1);
31
33
32
34
%% Finally, construct ParticleFilter
33
35
pf = GenericParticleFilter(veh, sensor, Q, L, 200);