Skip to content

Commit d18aee0

Browse files
sungloksunglok
committed
Add 'localize2d_sayed05_aoa'
1 parent 7644844 commit d18aee0

8 files changed

+116
-50
lines changed

‎README.md

+2-2
Original file line numberDiff line numberDiff line change
@@ -39,8 +39,8 @@ trueMap = ...
3939
8, 8, 0, 0, 0, tran_deg2rad(-180); ...
4040
];
4141
truePose = [3, 2, 0, 0, 0, pi / 9];
42-
obsData = observe_distance(trueMap, truePose); % Simulate observation
43-
estPose = localize2d_sayed05(obsData, trueMap); % Estimate position
42+
obsData = observe_distance(trueMap, truePose); % Simulate observation
43+
estPose = localize2d_sayed05_toa(obsData, trueMap); % Estimate position
4444
```
4545
See [`run_example.m`][] for more complex example with visualization.
4646

‎localize2d_sayed05_aoa.m

+49
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,49 @@
1+
function [pose, valid] = localize2d_sayed05_aoa(data, map)
2+
%LOCALIZE2D_SAYED05_AOA Estimate 2D position and orientation using displacements
3+
% measured from landmarks (N >= 2)
4+
%
5+
% [POSE, VALID] = LOCALIZE2D_SAYED05_AOA(DATA, MAP)
6+
% (matrix) DATA : The measured displacement from landmarks (Nx3 matrix)
7+
% (matrix) MAP : The corresponding landmark map (Nx6 matrix)
8+
% (matrix) POSE : The estimated pose (1x6 matrix)
9+
% (matrix) VALID: A flag to represent validity of the estimated pose (1x6 matrix)
10+
%
11+
% Note: Please refer to the command, OBSERVE_DISPLACEMENT, for the convention of DATA,
12+
% MAP, and POSE.
13+
%
14+
% Note: A flag for validity, VALID, is 1x6 matrix whose elements correspond to each
15+
% element of POSE. Since this algorithm estimates 2D position and orientation,
16+
% the expected VALID is [true, true, false, false, false, true].
17+
%
18+
% Note: This implementation is extended from Sayed et al. [1], so it can utilize
19+
% angle-of-arrival (AOA) obtained at a mobile station (MS), not base station (BS).
20+
%
21+
% Reference:
22+
% [1] A. H. Sayed et al., Network-based Wireless Location,
23+
% IEEE Signal Processing Magazine, Vol. 24, No. 4, 2005
24+
% URL: http://ieeexplore.ieee.org/xpls/abs_all.jsp?arnumber=1458275
25+
%
26+
% Example:
27+
% N = 2;
28+
% map = [10 * rand(N,2), zeros(N,4)]; % Random 2D landmark map
29+
% data = [10 * rand(N,2), zeros(N,1)]; % Random measurement
30+
% [pose, valid] = localize2d_sayed05_aoa(data, map)
31+
32+
if size(data,1) < 2
33+
error('DATA has less number of observations!');
34+
end
35+
if size(data,2) ~= 3
36+
error('DATA has wrong size!');
37+
end
38+
39+
n = size(data,1);
40+
A4 = zeros(2 * n,1);
41+
A4(1:2:end) = -data(:,2);
42+
A4(2:2:end) = +data(:,1);
43+
A = [repmat(eye(2,2), n, 1), reshape(data(:,1:2)', 2 * n, []), A4];
44+
b = reshape(map(:,1:2)', 2 * n, []);
45+
x = pinv(A) * b;
46+
x = x / norm(x(3:4));
47+
48+
pose = [x(1), x(2), 0, 0, 0, atan2(x(4), x(3))];
49+
valid = [true, true, false, false, false, true];

‎localize2d_sayed05.m ‎localize2d_sayed05_toa.m

+5-5
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
1-
function [pose, valid] = localize2d_sayed05(data, map)
2-
%LOCALIZE2D_SAYED05 Estimate 2D position using distances measured from landmarks (N >= 3)
1+
function [pose, valid] = localize2d_sayed05_toa(data, map)
2+
%LOCALIZE2D_SAYED05_TOA Estimate 2D position using distances measured from landmarks (N >= 3)
33
%
4-
% [POSE, VALID] = LOCALIZE2D_SAYED05(DATA, MAP)
4+
% [POSE, VALID] = LOCALIZE2D_SAYED05_TOA(DATA, MAP)
55
% (matrix) DATA : The measured distances from landmarks (Nx1 matrix)
66
% (matrix) MAP : The corresponding landmark map (Nx6 matrix)
77
% (matrix) POSE : The estimated pose (1x6 matrix)
@@ -23,9 +23,9 @@
2323
% N = 3;
2424
% map = [10 * rand(N,2), zeros(N,4)]; % Random 2D landmark map
2525
% data = 10 * rand(N,1); % Random measurement
26-
% [pose, valid] = localize2d_sayed05(data, map)
26+
% [pose, valid] = localize2d_sayed05_toa(data, map)
2727
%
28-
% See also localize3d_sayed05, localize3d_thomas05.
28+
% See also localize3d_sayed05_toa, localize3d_thomas05.
2929

3030
if size(data,1) < 3
3131
error('DATA has less number of observations!');

‎localize3d_sayed05.m ‎localize3d_sayed05_toa.m

+5-5
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
1-
function [pose, valid] = localize3d_sayed05(data, map)
2-
%LOCALIZE3D_SAYED05 Estimate 3D position using distances measured from landmarks (N >= 4)
1+
function [pose, valid] = localize3d_sayed05_toa(data, map)
2+
%LOCALIZE3D_SAYED05_TOA Estimate 3D position using distances measured from landmarks (N >= 4)
33
%
4-
% [POSE, VALID] = LOCALIZE3D_SAYED05(DATA, MAP)
4+
% [POSE, VALID] = LOCALIZE3D_SAYED05_TOA(DATA, MAP)
55
% (matrix) DATA : The measured distances from landmarks (Nx1 matrix)
66
% (matrix) MAP : The corresponding landmark map (Nx6 matrix)
77
% (matrix) POSE : The estimated pose (1x6 matrix)
@@ -23,9 +23,9 @@
2323
% N = 4;
2424
% map = [10 * rand(N,3), zeros(N,3)]; % Random 2D landmark map
2525
% data = 10 * rand(N,1); % Random measurement
26-
% [pose, valid] = localize3d_sayed05(data, map)
26+
% [pose, valid] = localize3d_sayed05_toa(data, map)
2727
%
28-
% See also localize2d_sayed05, localize3d_thomas05.
28+
% See also localize2d_sayed05_toa, localize3d_thomas05.
2929

3030
if size(data,1) < 4
3131
error('DATA has less number of observations!');

‎run_eval_mrclam.m

+9-7
Original file line numberDiff line numberDiff line change
@@ -18,14 +18,16 @@
1818
};
1919
config.pose = [];
2020
config.algorithm = ... % Description of localization algorithms
21-
{ ...
22-
% #, Dim, Name, Local. Function, Observation Function, Min. N, Valid, Line Sytle; ...
23-
1, 2, 'Sayed05-2D', @localize2d_sayed05, @observe_distance, 3, [1 1 0 0 0 0], 'kx-'; ...
24-
2, 2, 'Betke97', @localize2d_betke97, @observe_bearing, 3, [1 1 0 0 0 1], 'gd-'; ...
25-
3, 2, 'Shim02-Alg', @localize2d_shimshoni02_algebraic, @observe_bearing, 3, [1 1 0 0 0 1], 'b--'; ...
26-
4, 2, 'Shim02-Imp', @localize2d_shimshoni02_improved, @observe_bearing, 3, [1 1 0 0 0 1], 'b+-'; ...
27-
5, 2, 'Se05', @localize2d_se05, @observe_displacement, 2, [1 1 0 0 0 1], 'rs-'; ...
21+
{ ...
22+
% #, Dim, Name, Local. Function, Observation Function, Min. N, Valid, Line Sytle; ...
23+
1, 2, 'Sayed05-TOA2D',@localize2d_sayed05_toa,@observe_distance, 3, [1 1 0 0 0 0], 'kx-'; ...
24+
2, 2, 'Betke97', @localize2d_betke97, @observe_bearing, 3, [1 1 0 0 0 1], 'gd-'; ...
25+
3, 2, 'Shim02-Alg', @localize2d_shimshoni02_algebraic, @observe_bearing, 3, [1 1 0 0 0 1], 'b--'; ...
26+
4, 2, 'Shim02-Imp', @localize2d_shimshoni02_improved, @observe_bearing, 3, [1 1 0 0 0 1], 'b+-'; ...
27+
5, 2, 'Se05', @localize2d_se05, @observe_displacement, 2, [1 1 0 0 0 1], 'rs-'; ...
28+
6, 2, 'Sayed05-AOA', @localize2d_sayed05_aoa,@observe_displacement, 2, [1 1 0 0 0 1], 'ks-'; ...
2829
};
30+
2931
config.matFile = 'run_eval_mrclam.mat';
3032

3133
criteria.name = {'Position Error [m]', 'Orientation Error [deg]', ...

‎run_eval_random.m

+16-10
Original file line numberDiff line numberDiff line change
@@ -14,17 +14,18 @@
1414
config.fixNoise = 0.1; % Standard deviation of noise (default)
1515
config.fixN = 4; % The number of landmarks for localization (default)
1616
config.varNoise = 0:0.1:1; % Range of std. of noise
17-
config.varN = [2, 3, 4, 6, 8, 16, 32, 64, 128]; % Range of the number of landmarks for localization
17+
config.varN = [2, 3, 4, 5, 8, 16, 32, 64, 128]; % Range of the number of landmarks for localization
1818
config.algorithm = ... % Description of localization algorithms
19-
{ ...
20-
% #, Dim, Name, Local. Function, Observation Function, Min. N, Valid, Line Sytle; ...
21-
1, 2, 'Sayed05-2D', @localize2d_sayed05, @observe_distance, 3, [1 1 0 0 0 0], 'kx-'; ...
22-
2, 2, 'Betke97', @localize2d_betke97, @observe_bearing, 3, [1 1 0 0 0 1], 'gd-'; ...
23-
3, 2, 'Shim02-Alg', @localize2d_shimshoni02_algebraic, @observe_bearing, 3, [1 1 0 0 0 1], 'b--'; ...
24-
4, 2, 'Shim02-Imp', @localize2d_shimshoni02_improved, @observe_bearing, 3, [1 1 0 0 0 1], 'b+-'; ...
25-
5, 2, 'Se05', @localize2d_se05, @observe_displacement, 2, [1 1 0 0 0 1], 'rs-'; ...
26-
6, 3, 'Sayed05-3D', @localize3d_sayed05, @observe_distance, 4, [1 1 1 0 0 0], 'ko-'; ...
27-
7, 3, 'Thomas05', @localize3d_thomas05, @observe_distance, 3, [1 1 1 0 0 0], 'm+-'; ...
19+
{ ...
20+
% #, Dim, Name, Local. Function, Observation Function, Min. N, Valid, Line Sytle; ...
21+
1, 2, 'Sayed05-TOA2D',@localize2d_sayed05_toa,@observe_distance, 3, [1 1 0 0 0 0], 'kx-'; ...
22+
2, 2, 'Betke97', @localize2d_betke97, @observe_bearing, 3, [1 1 0 0 0 1], 'gd-'; ...
23+
3, 2, 'Shim02-Alg', @localize2d_shimshoni02_algebraic, @observe_bearing, 3, [1 1 0 0 0 1], 'b--'; ...
24+
4, 2, 'Shim02-Imp', @localize2d_shimshoni02_improved, @observe_bearing, 3, [1 1 0 0 0 1], 'b+-'; ...
25+
5, 2, 'Se05', @localize2d_se05, @observe_displacement, 2, [1 1 0 0 0 1], 'rs-'; ...
26+
6, 2, 'Sayed05-AOA', @localize2d_sayed05_aoa,@observe_displacement, 2, [1 1 0 0 0 1], 'ks-'; ...
27+
7, 3, 'Sayed05-TOA3D',@localize3d_sayed05_toa,@observe_distance, 4, [1 1 1 0 0 0], 'ko-'; ...
28+
8, 3, 'Thomas05', @localize3d_thomas05, @observe_distance, 3, [1 1 1 0 0 0], 'm+-'; ...
2829
};
2930
config.verbose = true; % Show progress of experiments (true or false)
3031
config.warning = 'off'; % Show warning during experiments ('on' or 'off')
@@ -223,6 +224,11 @@
223224
config.algorithm{m,config.algoLine}, 'LineWidth', 2, 'MarkerSize', 10);
224225
isDrawn = [isDrawn, m];
225226
end
227+
if ex == 2
228+
set(gca, 'XScale', 'log');
229+
set(gca, 'XTick', [2, 4, 8, 16, 32, 64, 128]);
230+
set(gca, 'XLim', [2, 128]);
231+
end
226232
%title(sprintf('Experiment #%d: %s - %s', ex, variable.name{ex}, criteria.name{cr}), 'FontSize', 12);
227233
xlabel(variable.name{ex}, 'FontSize', 12);
228234
ylabel(criteria.name{cr}, 'FontSize', 12);

‎run_eval_roh.m

+5-5
Original file line numberDiff line numberDiff line change
@@ -30,11 +30,11 @@
3030
4.5, 4.5, 0, 0, 0, pi/2; ...
3131
];
3232
config.algorithm = ... % Description of localization algorithms
33-
{ ...
34-
% #, Dim, Name, Local. Function, Observation Function, Min. N, Valid, Line Sytle; ...
35-
1, 2, 'Betke97', @localize2d_betke97, @observe_bearing, 3, [1 1 0 0 0 1], 'gd-'; ...
36-
2, 2, 'Shim02-Alg', @localize2d_shimshoni02_algebraic, @observe_bearing, 3, [1 1 0 0 0 1], 'b--'; ...
37-
3, 2, 'Shim02-Imp', @localize2d_shimshoni02_improved, @observe_bearing, 3, [1 1 0 0 0 1], 'b+-'; ...
33+
{ ...
34+
% #, Dim, Name, Local. Function, Observation Function, Min. N, Valid, Line Sytle; ...
35+
1, 2, 'Betke97', @localize2d_betke97, @observe_bearing, 3, [1 1 0 0 0 1], 'gd-'; ...
36+
2, 2, 'Shim02-Alg', @localize2d_shimshoni02_algebraic, @observe_bearing, 3, [1 1 0 0 0 1], 'b--'; ...
37+
3, 2, 'Shim02-Imp', @localize2d_shimshoni02_improved, @observe_bearing, 3, [1 1 0 0 0 1], 'b+-'; ...
3838
};
3939
config.matFile = 'run_eval_roh.mat';
4040

‎run_test_localize.m

+25-16
Original file line numberDiff line numberDiff line change
@@ -14,17 +14,17 @@
1414
];
1515
truePose = [3, 2, 0, 0, 0, pi / 9];
1616

17-
% localize2d_sayed05
18-
disp('==== localize2d_sayed05 ====');
17+
% localize2d_sayed05_toa
18+
disp('==== localize2d_sayed05_toa ====');
1919
[obsData, obsMap] = observe_distance(trueMap, truePose);
20-
[pose, valid] = localize2d_sayed05(obsData, obsMap);
20+
[pose, valid] = localize2d_sayed05_toa(obsData, obsMap);
2121
test_is_true(valid == [1, 1, 0, 0, 0, 0]);
2222
test_is_near(pose(valid == true), truePose(valid == true));
2323

24-
% localize2d_se05
25-
disp('==== localize2d_se05 ====');
26-
[obsData, obsMap] = observe_displacement(trueMap, truePose);
27-
[pose, valid] = localize2d_se05(obsData, obsMap);
24+
% localize2d_betke97
25+
disp('==== localize2d_betke97 ====');
26+
[obsData, obsMap] = observe_bearing(trueMap, truePose);
27+
[pose, valid] = localize2d_betke97(obsData, obsMap);
2828
test_is_true(valid == [1, 1, 0, 0, 0, 1]);
2929
test_is_near(pose(valid == true), truePose(valid == true));
3030

@@ -42,10 +42,17 @@
4242
test_is_true(valid == [1, 1, 0, 0, 0, 1]);
4343
test_is_near(pose(valid == true), truePose(valid == true));
4444

45-
% localize2d_betke97
46-
disp('==== localize2d_betke97 ====');
47-
[obsData, obsMap] = observe_bearing(trueMap, truePose);
48-
[pose, valid] = localize2d_betke97(obsData, obsMap);
45+
% localize2d_se05
46+
disp('==== localize2d_se05 ====');
47+
[obsData, obsMap] = observe_displacement(trueMap, truePose);
48+
[pose, valid] = localize2d_se05(obsData, obsMap);
49+
test_is_true(valid == [1, 1, 0, 0, 0, 1]);
50+
test_is_near(pose(valid == true), truePose(valid == true));
51+
52+
% localize2d_sayed05_aoa
53+
disp('==== localize2d_sayed05_aoa ====');
54+
[obsData, obsMap] = observe_displacement(trueMap, truePose);
55+
[pose, valid] = localize2d_sayed05_aoa(obsData, obsMap);
4956
test_is_true(valid == [1, 1, 0, 0, 0, 1]);
5057
test_is_near(pose(valid == true), truePose(valid == true));
5158

@@ -60,16 +67,18 @@
6067
];
6168
truePose = [3, 2, 1, 0, 0, pi / 9];
6269

63-
% localize_sayed05
64-
disp('==== localize_sayed05 ====');
70+
% localize_sayed05_toa
71+
disp('==== localize_sayed05_toa ====');
6572
[obsData, obsMap] = observe_distance(trueMap, truePose);
66-
[pose, valid] = localize3d_sayed05(obsData, obsMap);
73+
[pose, valid] = localize3d_sayed05_toa(obsData, obsMap);
6774
test_is_true(valid == [1, 1, 1, 0, 0, 0]);
6875
test_is_near(pose(valid == true), truePose(valid == true));
6976

7077
% localize_thomas05
7178
disp('==== localize_thomas05 ====');
7279
[obsData, obsMap] = observe_distance(trueMap, truePose);
7380
[pose, valid] = localize3d_thomas05(obsData, obsMap);
74-
test_is_true(valid == [1, 1, 1, 0, 0, 0]);
75-
test_is_near(pose(valid == true), truePose(valid == true));
81+
for i = 1:size(pose,1)
82+
test_is_true(valid(i,:) == [1, 1, 1, 0, 0, 0]);
83+
test_is_near(pose(i,valid(i,:) == true), truePose(valid(i,:) == true));
84+
end

0 commit comments

Comments
 (0)