-
Notifications
You must be signed in to change notification settings - Fork 16
/
Copy pathrun_test_localize.m
98 lines (85 loc) · 3.95 KB
/
run_test_localize.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
close all;
clear all;
disp('== Unit-test of Algorithms for Triangulation Toolbox ==');
% The given landmark map and true pose for 2D localization %%%%%%%%%%%%%%%%%%%%
trueMap = ...
[ ...
% x, y, z, r_x, r_y, r_z ...
5, 0, 0, 0, 0, tran_deg2rad( +90); ...
5, 5, 0, 0, 0, tran_deg2rad(-180); ...
0, 5, 0, 0, 0, tran_deg2rad( -90); ...
0, 0, 0, 0, 0, 0; ...
];
truePose = [3, 2, 0, 0, 0, pi / 9];
% localize2d_sayed05_toa
disp('==== localize2d_sayed05_toa ====');
[obsData, obsMap] = observe_distance(trueMap, truePose);
[pose, valid] = localize2d_sayed05_toa(obsData, obsMap);
test_is_true(valid == [1, 1, 0, 0, 0, 0]);
test_is_near(pose(valid == true), truePose(valid == true));
% localize2d_sayed05_tdoa
disp('==== localize2d_sayed05_tdoa ====');
[obsData, obsMap] = observe_distance_relative(trueMap, truePose);
[pose, valid] = localize2d_sayed05_tdoa(obsData, obsMap);
test_is_true(valid == [1, 1, 0, 0, 0, 0]);
test_is_near(pose(valid == true), truePose(valid == true));
% localize2d_betke97
disp('==== localize2d_betke97 ====');
[obsData, obsMap] = observe_bearing(trueMap, truePose);
[pose, valid] = localize2d_betke97(obsData, obsMap);
test_is_true(valid == [1, 1, 0, 0, 0, 1]);
test_is_near(pose(valid == true), truePose(valid == true));
% localize2d_shimshoni02_algebraic
disp('==== localize2d_shimshoni02_algebraic ====');
[obsData, obsMap] = observe_bearing(trueMap, truePose);
[pose, valid] = localize2d_shimshoni02_algebraic(obsData, obsMap);
test_is_true(valid == [1, 1, 0, 0, 0, 1]);
test_is_near(pose(valid == true), truePose(valid == true));
% localize2d_shimshoni02_improved
disp('==== localize2d_shimshoni02_improved ====');
[obsData, obsMap] = observe_bearing(trueMap, truePose);
[pose, valid] = localize2d_shimshoni02_improved(obsData, obsMap);
test_is_true(valid == [1, 1, 0, 0, 0, 1]);
test_is_near(pose(valid == true), truePose(valid == true));
% localize2d_se05
disp('==== localize2d_se05 ====');
[obsData, obsMap] = observe_displacement(trueMap, truePose);
[pose, valid] = localize2d_se05(obsData, obsMap);
test_is_true(valid == [1, 1, 0, 0, 0, 1]);
test_is_near(pose(valid == true), truePose(valid == true));
% localize2d_sayed05_aoa
disp('==== localize2d_sayed05_aoa ====');
[obsData, obsMap] = observe_displacement(trueMap, truePose);
[pose, valid] = localize2d_sayed05_aoa(obsData, obsMap);
test_is_true(valid == [1, 1, 0, 0, 0, 1]);
test_is_near(pose(valid == true), truePose(valid == true));
% localize2d_pose
disp('==== localize2d_pose ====');
[obsData, obsMap] = observe_pose(trueMap, truePose);
[pose, valid] = localize2d_pose(obsData, obsMap);
test_is_true(valid == [1, 1, 0, 0, 0, 1]);
test_is_near(pose(valid == true), truePose(valid == true));
% The given landmark map and true pose for 3D localization %%%%%%%%%%%%%%%%%%%%
trueMap = ...
[ ...
% x, y, z, r_x, r_y, r_z ...
5, 0, 5, 0, 0, tran_deg2rad( +90); ...
5, 5, 0, 0, 0, tran_deg2rad(-180); ...
0, 5, 5, 0, 0, tran_deg2rad( -90); ...
0, 0, 5, 0, 0, 0 ...
];
truePose = [3, 2, 1, 0, 0, pi / 9];
% localize_sayed05_toa
disp('==== localize_sayed05_toa ====');
[obsData, obsMap] = observe_distance(trueMap, truePose);
[pose, valid] = localize3d_sayed05_toa(obsData, obsMap);
test_is_true(valid == [1, 1, 1, 0, 0, 0]);
test_is_near(pose(valid == true), truePose(valid == true));
% localize_thomas05
disp('==== localize_thomas05 ====');
[obsData, obsMap] = observe_distance(trueMap, truePose);
[pose, valid] = localize3d_thomas05(obsData, obsMap);
for i = 1:size(pose,1)
test_is_true(valid(i,:) == [1, 1, 1, 0, 0, 0]);
test_is_near(pose(i,valid(i,:) == true), truePose(valid(i,:) == true));
end