1 % localization simulation
2 % first estimate the node location approximately
3 % then optimize the node location through linearized least square
4 % heuristics:
5 % 1) initial location estimate variance is amplified by 4 to compensate non-Gaussian location distribution
6 % 2) optimization step size is reduced by half to compensate non-linearity of range constraints
7 % 3) optimization stops after the step size is less than a threshold
8 % coordinate systems:
9 % xxx_loc is Cartesian coordinates
10 % xxx_R is Spherical coordinate radius, [0, +inf)
11 % xxx_Theta is Spherical coordinate azimuth, [0, 2*pi)
12 % xxx_Phi is Spherical coordinate zenith, [0, pi]
13 % xxx_Omiga is orientation of node, Spherical coordinate azimuth, [0, 2*pi)
14
15 clear all;
16 close all;
17
18 % step size threshold to stop optimization
19 step = 0.5;
20 %initialize random generator
21 %rand('state',80);
22 % side length of cubic area for localizaiton simulation
23 side = 40; height = 10;
24 % node within max_range have measurements with each other
25 max_range = 30;
26 % max_iteration in optimization
27 max_opt = 10;
28 % num of nodes
29 N_node = 25;
30 N_original_anchor = 5;
31
32 % observation error standard deviation
33 err_R_std = 0.05;
34 err_Theta_std = 5*pi/180;
35 err_Phi_std = 30*pi/180;
36
37 % true location of node is generated through cylinda coordinate
38 true_loc = rand(3,N_node);
39 true_loc(1,:) = true_loc(1,:) * side;
40 true_loc(2,:) = true_loc(2,:) * side;
41 true_loc(3,:) = true_loc(3,:) * height;
42 % 1:N_original_anchor are original anchor nodes
43 true_loc(:,1) = [0; 0; height];
44 true_loc(:,2) = [side; 0; height/2];
45 true_loc(:,3) = [side; side; height/2];
46 true_loc(:,4) = [0; side; height/2];
47 true_loc(:,5) = [side/2; side/2; 0];
48
49 % if j's range measurement about i is outlier,
50 % or if i and j are not of range of each other
51 % then usable(i,j) == 0
52 usable = ones(N_node, N_node);
53 for i = 1:N_node
54 usable(i,i) = 0;
55 end
56
57
58 for i = 1:N_node
59 for j = 1:N_node
60 % true distance from node j to node i
61 true_R(i,j) = sqrt((true_loc(1,i)-true_loc(1,j))^2+(true_loc(2,i)-true_loc(2,j))^2+(true_loc(3,i)-true_loc(3,j))^2);
62 if true_R(i,j) > max_range
63 usable(i,j) = 0;
64 end
65 tmp_val = atan2((true_loc(2,i) - true_loc(2,j)), (true_loc(1,i)-true_loc(1,j)));
66 % atan2 return angle (-pi, pi], change it to [0, 2*pi)
67 if tmp_val < 0
68 tmp_val = tmp_val + 2*pi;
69 end
70 true_Theta(i,j) = tmp_val;
71 true_Phi(i,j) = atan2(sqrt((true_loc(1,i)-true_loc(1,j))^2+(true_loc(2,i)-true_loc(2,j))^2), (true_loc(3,i)-true_loc(3,j)));
72 end
73 end
74
75
76 % upper triangle matrix template
77 err_template = zeros(N_node, N_node);
78 for i = 1:N_node
79 for j = i+1:N_node
80 err_template(i,j) = 1;
81 end
82 end
83 err_template = err_template + err_template';
84
85 err_R = sprandn(err_template)*err_R_std;
86 obs_R = true_R + err_R;
87
88 err_Phi = sprandn(err_template)*err_Phi_std;
89 obs_Phi = true_Phi + err_Phi;
90
91 err_Theta = sprandn(err_template)*err_Theta_std;
92 obs_Theta = true_Theta + err_Theta;
93
94 % node orientation
95 true_Omiga = rand(1,N_node)*2*pi;
96 for i = 1:N_original_anchor
97 true_Omiga(i) = 0;
98 end
99
100 % take account of orientation in simulaiton of Theta observation
101 for j = 1:N_node
102 for i = 1:N_node
103 if ( i ~= j)
104 tmp_val = obs_Theta(i,j) - true_Omiga(j);
105 if tmp_val < 0
106 tmp_val = tmp_val + 2*pi;
107 end
108 if tmp_val > 2*pi
109 tmp_val = tmp_val - 2*pi;
110 end
111 obs_Theta(i,j) = tmp_val;
112 end
113 end
114 end
115
116 % screw some range measurements to simulate outliers
117 obs_R(5,6) = obs_R(5,6) + 10;
118 obs_R(10,15) = obs_R(10,15) + 2;
119 % screw up of zenith does not matter
120 obs_phi(14,9) = obs_Phi(14,9)+pi/3;
121
122 % from now on, to localize nodes
123 est_loc = zeros(3, N_node);
124 est_loc_var = zeros(3,3,N_node);
125 est_Omiga = zeros(1, N_node);
126 est_Omiga_var = zeros(1, N_node);
127 est_Theta = zeros(N_node, N_node);
128 % anchor node has non-zero location uncertainty and orientation uncertainty
129 for i = 1:N_original_anchor
130 est_loc(:,i) = true_loc(:,i) + randn(3,1)*2*err_R_std;
131 est_loc_var(:,:,i) = eye(3,3)*(2*err_R_std)^2;
132 est_Omiga(i) = true_Omiga(i);
133 est_Omiga_var(i) = (err_Theta_std/100)^2;
134 end
135
136 % scan range measurement outliers
137 % if difference between j's range measurement to i and i's range measurement to j
138 % great than 4 * sqrt(2) * err_R_std,
139 % then either i's measurement or j's measurement is an outliner.
140 % Since it is hard to differentiate them, we get rid of both
141 for i = 1:N_node
142 for j = 1:N_node
143 if abs(obs_R(i,j) - obs_R(j,i)) > 5.6569*err_R_std
144 usable(i,j) = 0;
145 usable(j,i) = 0;
146 end
147 end
148 end
149
150 % if is_anchor(i) ==1, then node i can be used as an anchor node,
151 % either original anchor node, or derived anchor node
152 is_anchor = zeros(N_node,1);
153 is_anchor(1:N_original_anchor,1) = ones(N_original_anchor,1);
154
155 % if anchor(i,j) == 1, then node i can use node j as anchor node
156 anchor = zeros(N_node, N_node);
157 for i = N_original_anchor+1:N_node
158 for j = 1:N_node
159 if usable(i,j) == 1 & is_anchor(j)
160 anchor(i,j) = 1;
161 end
162 end
163 end
164
165
166 delt_loc = zeros(3, N_node);
167 continue = 1;
168 while continue == 1;
169
170 % select the node with maximum number of anchor nodes to localize
171 tmp_col = sum(anchor, 2);
172 [N_anchor, i] = max(tmp_col);
173 if N_anchor <= 0 | i <= N_original_anchor
174 continue = 0;
175 break;
176 end
177
178 %i
179 %N_anchor
180
181 anchor_idx = [];
182 for j = 1:N_node
183 if anchor(i,j) == 1
184 anchor_idx = [anchor_idx; j];
185 end
186 end
187
188 fused_loc = zeros(3,1);
189 fused_loc_var_inv = zeros(3,3);
190 % iterate through all anchor nodes
191 % (1) only use original anchor nodes
192 %N_anchor = N_original_anchor;
193 % (2) also use localized nodes (derived anchor nodes)
194 % (2) performs much better in terms accuracy and convergence
195
196 % phase 1: estimate node location approximately
197 for h = 1:N_anchor
198 j = anchor_idx(h);
199
200 tmp_R = (obs_R(i,j) + obs_R(j,i))/2;
201 tmp_R_var = err_R_std^2/2;
202 tmp_Theta = obs_Theta(i,j) + est_Omiga(j);
203 tmp_Theta_var = err_Theta^2 + est_Omiga_var(j);
204 tmp_Phi = (obs_Phi(i,j) + pi - obs_Phi(j,i))/2;
205 tmp_Phi_var = err_Phi_std^2/2;
206
207 tmp_loc = zeros(3,1);
208 tmp_loc(1) = est_loc(1,j) + tmp_R * sin(tmp_Phi) * cos(tmp_Theta);
209 tmp_loc(2) = est_loc(2,j) + tmp_R * sin(tmp_Phi) * sin(tmp_Theta);
210 tmp_loc(3) = est_loc(3,j) + tmp_R * cos(tmp_Phi);
211
212 tmp_loc_var = zeros(3,3);
213
214 tmp_loc_var(1,1) = est_loc_var(1,1,j) + (sin(tmp_Phi))^2*(cos(tmp_Theta))^2*err_R_std^2;
215 tmp_loc_var(1,1) = tmp_loc_var(1,1) + tmp_R^2*(cos(tmp_Phi))^2*(cos(tmp_Theta))^2*err_Phi_std^2;
216 tmp_loc_var(1,1) = tmp_loc_var(1,1) + tmp_R^2*(sin(tmp_Phi))^2*(sin(tmp_Theta))^2*err_Theta_std^2;
217
218 tmp_loc_var(2,2) = est_loc_var(2,2,j) + (sin(tmp_Phi))^2*(sin(tmp_Theta))^2*err_R_std^2;
219 tmp_loc_var(2,2) = tmp_loc_var(2,2) + tmp_R^2*(cos(tmp_Phi))^2*(sin(tmp_Theta))^2*err_Phi_std^2;
220 tmp_loc_var(2,2) = tmp_loc_var(2,2) + tmp_R^2*(sin(tmp_Phi))^2*(cos(tmp_Theta))^2*err_Theta_std^2;
221
222 tmp_loc_var(3,3) = est_loc_var(3,3,j)+(cos(tmp_Phi))^2*err_R_std^2 + tmp_R^2*(sin(tmp_Phi))^2*err_Phi_std^2;
223
224 tmp_loc_var(1,2) = est_loc_var(1,2,j) + 0.5*(sin(tmp_Phi))^2*sin(2*tmp_Theta)*err_R_std^2;
225 tmp_loc_var(1,2) = tmp_loc_var(1,2) + 0.5*tmp_R^2*(cos(tmp_Phi))^2*sin(2*tmp_Theta)*err_Phi_std^2;
226 tmp_loc_var(1,2) = tmp_loc_var(1,2) - 0.5*tmp_R^2*(sin(tmp_Phi))^2*sin(2*tmp_Theta)*err_Theta_std^2;
227 tmp_loc_var(2,1) = tmp_loc_var(1,2);
228
229 tmp_loc_var(1,3) = est_loc_var(1,3,j) + 0.5*sin(2*tmp_Phi)*cos(tmp_Theta)*err_R_std^2;
230 tmp_loc_var(1,3) = tmp_loc_var(1,3) - 0.5*tmp_R^2*sin(2*tmp_Phi)*cos(tmp_Theta)*err_Phi_std^2;
231 tmp_loc_var(3,1) = tmp_loc_var(1,3);
232
233 tmp_loc_var(2,3) = est_loc_var(2,3,j) + 0.5*sin(2*tmp_Phi)*sin(tmp_Theta)*err_R_std^2;
234 tmp_loc_var(2,3) = tmp_loc_var(2,3) - 0.5*tmp_R^2*sin(2*tmp_Phi)*sin(tmp_Theta)*err_Phi_std^2;
235 tmp_loc_var(3,2) = tmp_loc_var(2,3);
236
237 tmp_loc_var_inv = inv(tmp_loc_var);
238
239 fused_loc_var_inv = fused_loc_var_inv + tmp_loc_var_inv;
240 fused_loc = fused_loc + tmp_loc_var_inv * tmp_loc;
241
242 % update the observed R with fused R
243 obs_R(i,j) = tmp_R;
244 obs_R(j,i) = tmp_R;
245
246 end
247 est_loc_var(:,:,i) = inv(fused_loc_var_inv);
248 est_loc(:,i) = est_loc_var(:,:,i)*fused_loc;
249 % increase the variance estimation by 4 times to compensate the non-gaussian reality
250 est_loc_var(:,:,i) = est_loc_var(:,:,i)*4;
251
252 % Phase 2: we need local optimization of node localization
253 % if it is well constrained by enough anchor nodes
254
255 delt_loc(:,i) = ones(3,1);
256
257 n_opt = 0;
258 while N_anchor >= 4 & delt_loc(:,i)'*delt_loc(:,i) > step^2 & n_opt < max_opt
259 n_opt = n_opt + 1;
260 A = [];
261 b = [];
262 for h = 1:N_anchor
263 j = anchor_idx(h);
264 A(h,1) = est_loc(1,i) - est_loc(1,j);
265 A(h,2) = est_loc(2,i) - est_loc(2,j);
266 A(h,3) = est_loc(3,i) - est_loc(3,j);
267 b(h) = 0.5*(obs_R(i,j)^2 - ((A(h,1))^2+(A(h,2))^2+(A(h,3))^2));
268 % weighted least square is no better than least square
269 % weight the equation with max variance of Xj, Yj, and Zj
270 [max_var, max_dim] = max(diag(est_loc_var(:,:,j)),[],1);
271 A(h,:) = A(h,:)/sqrt(max_var);
272 b(h) = b(h)/sqrt(max_var);
273 end
274 % cut adjustment step length by half to compensate the no-trivial deviation from optimal solution
275 b = b';
276 tmp_mtx = A'*A;
277 if cond(tmp_mtx) < 1000 & cond(tmp_mtx) > 1
278 delt_loc(:,i) = inv(A'*A)*A'*b;
279 est_loc(:,i) = est_loc(:,i) + delt_loc(:,i);
280 else
281 delt_loc(:,i) = zeros(3,1);
282 end
283 end
284
285 % Phase 3: estimate node orientation
286 % estimate azimuth of node i relative to node j's
287 for h = 1:N_anchor
288 j = anchor_idx(h);
289 % estimate Theta(i,j) using node i and j's location
290 tmp_val = atan2(est_loc(2,i)-est_loc(2,j), est_loc(1,i)-est_loc(1,j));
291 if tmp_val < 0
292 tmp_val = tmp_val + 2*pi;
293 end
294 est_Theta(i,j) = tmp_val;
295 end
296
297 % to fuse multiple orientation estimates is hard
298 % because two close orientations might have a value difference of about 2*pi
299 % we first estimate orientation of node i using its azimuth measurement of anchor 1
300 % we then use this estimate as a reference in the fusion of multiple orientation estimates
301 % (omiga(i) + theta(j,i)) - theta(i,j) = 2*k*pi + pi, k is integer
302 tmp_val = pi + est_Theta(i,anchor_idx(1)) - obs_Theta(anchor_idx(1),i);
303 % put it in degree range of [0, 360)
304 ref_Omiga = mod(tmp_val*180/pi, 360);
305
306 tmp_Omiga = ref_Omiga;
307 tmp_Omiga_var = 0;
308 for h = 2:N_anchor
309 j = anchor_idx(h);
310 % (omiga(i) + theta(j,i)) - theta(i,j) = 2*k*pi + pi, k is integer
311 tmp_val = pi + est_Theta(i,1) - obs_Theta(1,i);
312 % put it in degree range of [0, 360)
313 tmp_val = mod(tmp_val*180/pi, 360);
314 if tmp_val - ref_Omiga > 180
315 tmp_val = tmp_val - 360;
316 end
317 if ref_Omiga - tmp_val > 180
318 tmp_val = tmp_val + 360;
319 end
320 tmp_Omiga = tmp_Omiga + tmp_val;
321 tmp_Omiga_var = tmp_Omiga_var + (tmp_val-tmp_Omiga/h)^2;
322 end
323 tmp_Omiga = tmp_Omiga/N_anchor;
324 tmp_Omiga = mod(tmp_Omiga, 360);
325 est_Omiga(i) = tmp_Omiga*pi/180;
326 % increase the variance estimation by 16 times to compensate the non-gaussian reality
327 est_Omiga_var(i) = 16*max((tmp_Omiga_var/N_anchor)*(pi/180)^2, err_Theta_std^2);
328
329 % update anchor matrix so that this node will not compete for localization
330 anchor(i,:) = -1 * ones(1, N_node);
331 if delt_loc(:,i) ~= zeros(3,1)
332 is_anchor(i) = 1;
333 for j =1:N_node
334 if usable(j,i) == 1 & ~is_anchor(j)
335 anchor(j,i) = 1;
336 end
337 end
338 end
339
340 end
341
342 loc_err3 = sum(sum(abs(est_loc-true_loc),1),2)/N_node;
343
344 % phase 4: additional round of location optimizaiton
345 for i = N_original_anchor+1:N_node
346 if 1 %delt_loc(:,i) == zeros(3,1);
347 mk_delt_loc(:,i) = ones(3,1);
348 n_opt = 0;
349 while mk_delt_loc(:,i)'*mk_delt_loc(:,i) > step^2 & n_opt < max_opt
350 n_opt = n_opt + 1;
351 A = [];
352 b = [];
353 k = 0;
354 for j = 1:N_node
355 if usable(i,j) == 1
356 k = k + 1;
357 A(k,1) = est_loc(1,i) - est_loc(1,j);
358 A(k,2) = est_loc(2,i) - est_loc(2,j);
359 A(k,3) = est_loc(3,i) - est_loc(3,j);
360 b(k) = 0.5*(obs_R(i,j)^2 - ((A(k,1))^2+(A(k,2))^2+(A(k,3))^2));
361 % weighted least square is no better than least square
362 % weight the equation with max variance of Xj, Yj, and Zj
363 [max_var, max_dim] = max(diag(est_loc_var(:,:,j)),[],1);
364 A(k,:) = A(k,:)/sqrt(max_var);
365 b(k) = b(k)/sqrt(max_var);
366 end
367 end
368 % cut adjustment step length by half to compensate the no-trivial deviation from optimal solution
369 b = b';
370 tmp_mtx = A'*A;
371 if cond(tmp_mtx) < 1000 & cond(tmp_mtx) > 1
372 mk_delt_loc(:,i) = inv(A'*A)*A'*b;
373 est_loc(:,i) = est_loc(:,i) + mk_delt_loc(:,i);
374 else
375 mk_delt_loc(:,i) = zeros(3,1);
376 end
377 end
378 end
379 end
380
381 loc_err4 = sum(sum(abs(est_loc-true_loc),1),2)/N_node;
382
383 % phase 5: additional round Omiga optimization
384 for i = 1:0%N_node
385 for j = 1:N_node
386 if usable(i,j) == 1
387 % estimate Theta(i,j) using node i and j's location
388 tmp_val = atan2(est_loc(2,i)-est_loc(2,j), est_loc(1,i)-est_loc(1,j));
389 if tmp_val < 0
390 tmp_val = tmp_val + 2*pi;
391 end
392 est_Theta(i,j) = tmp_val;
393 end
394 end
395
396
397 ref_Omiga = est_Omiga(i)*180/pi;
398 tmp_Omiga = 0;
399 n_neighbor = 0;
400 for j = 1:N_node
401 if usable(i,j) == 1
402 n_neighbor = n_neighbor + 1;
403 % (omiga(i) + theta(j,i)) - theta(i,j) = 2*k*pi + pi, k is integer
404 tmp_val = pi + est_Theta(i,1) - obs_Theta(1,i);
405 % put it in degree range of [0, 360)
406 tmp_val = mod(tmp_val*180/pi, 360);
407 if tmp_val - ref_Omiga > 180
408 tmp_val = tmp_val - 360;
409 end
410 if ref_Omiga - tmp_val > 180
411 tmp_val = tmp_val + 360;
412 end
413 tmp_Omiga = tmp_Omiga + tmp_val;
414 end
415
416 end
417 tmp_Omiga = tmp_Omiga/n_neighbor;
418 tmp_Omiga = mod(tmp_Omiga, 360);
419 est_Omiga(i) = tmp_Omiga*pi/180;
420 est_Omiga_var(i) = err_Theta_std^2/n_neighbor;
421
422 end
423
424 % remove translation in plot
425 trans = (sum(est_loc, 2) - sum(true_loc, 2))/N_node;
426 for i = 1:N_node
427 est_loc(:,i) = est_loc(:,i) - trans;
428 end
429
430 figure(1);
431 plot3(true_loc(1, 1), true_loc(2,1), true_loc(3,1), 'cs', 'LineWidth',2);
432 hold on;
433 plot3(true_loc(1,N_node),true_loc(2,N_node),true_loc(3,N_node),'ro');
434 plot3(est_loc(1,N_node),est_loc(2,N_node),est_loc(3,N_node),'.');
435 legend('Anchor Node', 'True Location', 'Estimated Location');
436 plot3(true_loc(1,N_original_anchor+1:N_node),true_loc(2,N_original_anchor+1:N_node),true_loc(3,N_original_anchor+1:N_node), 'ro');
437 for i = 1:N_original_anchor
438 plot3(true_loc(1,i), true_loc(2,i), true_loc(3,i), 'cs','LineWidth',2);
439 end
440
441 plot3(est_loc(1,N_original_anchor+1:N_node), est_loc(2,N_original_anchor+1:N_node), est_loc(3,N_original_anchor+1:N_node), '.');
442 for i=N_original_anchor+1:N_node
443 plot3([true_loc(1,i) est_loc(1,i)], [true_loc(2,i) est_loc(2,i)], [true_loc(3,i) est_loc(3,i)]);
444 end
445
446 axis([0 side 0 side 0 height]); %axis vis3d;
447 xlabel('x');
448 ylabel('y');
449 zlabel('z');
450 grid on;
451
452 hold off;
453
454 % to remove the difference of about 360 degree in the plot of Orientation
455 for i = 1:N_node
456 if est_Omiga(i) - true_Omiga(i) > pi
457 est_Omiga(i) = est_Omiga(i) - 2*pi;
458 end
459 if true_Omiga(i) - est_Omiga(i) > pi
460 est_Omiga(i) = est_Omiga(i) + 2*pi;
461 end
462 end
463
464 figure(2);
465 plot(N_original_anchor+1,true_Omiga(N_original_anchor+1),'ro');
466 hold on;
467 plot(N_original_anchor+1, est_Omiga(N_original_anchor+1), '.');
468 legend('True Orientation', 'Estimated Orientation');
469 plot(N_original_anchor+1:N_node, true_Omiga(N_original_anchor+1:N_node),'ro');
470 plot(N_original_anchor+1:N_node, est_Omiga(N_original_anchor+1:N_node),'.');
471 for i = N_original_anchor+1:N_node
472 plot([i,i], [true_Omiga(i), est_Omiga(i)]);
473 end
474 hold off;
475 xlabel('Node ID');
476 ylabel('Orientation');
477
478 figure(3);
479 imagesc(usable);
480 colorbar;
This page was automatically generated by the
LXR engine.
Visit the LXR main site for more
information.