Show loc_test.m syntax highlighted
% localization simulation
% first estimate the node location approximately
% then optimize the node location through linearized least square
% heuristics:
% 1) initial location estimate variance is amplified by 4 to compensate non-Gaussian location distribution
% 2) optimization step size is reduced by half to compensate non-linearity of range constraints
% 3) optimization stops after the step size is less than a threshold
% coordinate systems:
% xxx_loc is Cartesian coordinates
% xxx_R is Spherical coordinate radius, [0, +inf)
% xxx_Theta is Spherical coordinate azimuth, [0, 2*pi)
% xxx_Phi is Spherical coordinate zenith, [0, pi]
% xxx_Omiga is orientation of node, Spherical coordinate azimuth, [0, 2*pi)
clear all;
close all;
% step size threshold to stop optimization
step = 0.5;
%initialize random generator
%rand('state',80);
% side length of cubic area for localizaiton simulation
side = 40; height = 10;
% node within max_range have measurements with each other
max_range = 30;
% max_iteration in optimization
max_opt = 10;
% num of nodes
N_node = 25;
N_original_anchor = 5;
% observation error standard deviation
err_R_std = 0.05;
err_Theta_std = 5*pi/180;
err_Phi_std = 30*pi/180;
% true location of node is generated through cylinda coordinate
true_loc = rand(3,N_node);
true_loc(1,:) = true_loc(1,:) * side;
true_loc(2,:) = true_loc(2,:) * side;
true_loc(3,:) = true_loc(3,:) * height;
% 1:N_original_anchor are original anchor nodes
true_loc(:,1) = [0; 0; height];
true_loc(:,2) = [side; 0; height/2];
true_loc(:,3) = [side; side; height/2];
true_loc(:,4) = [0; side; height/2];
true_loc(:,5) = [side/2; side/2; 0];
% if j's range measurement about i is outlier,
% or if i and j are not of range of each other
% then usable(i,j) == 0
usable = ones(N_node, N_node);
for i = 1:N_node
usable(i,i) = 0;
end
for i = 1:N_node
for j = 1:N_node
% true distance from node j to node i
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);
if true_R(i,j) > max_range
usable(i,j) = 0;
end
tmp_val = atan2((true_loc(2,i) - true_loc(2,j)), (true_loc(1,i)-true_loc(1,j)));
% atan2 return angle (-pi, pi], change it to [0, 2*pi)
if tmp_val < 0
tmp_val = tmp_val + 2*pi;
end
true_Theta(i,j) = tmp_val;
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)));
end
end
% upper triangle matrix template
err_template = zeros(N_node, N_node);
for i = 1:N_node
for j = i+1:N_node
err_template(i,j) = 1;
end
end
err_template = err_template + err_template';
err_R = sprandn(err_template)*err_R_std;
obs_R = true_R + err_R;
err_Phi = sprandn(err_template)*err_Phi_std;
obs_Phi = true_Phi + err_Phi;
err_Theta = sprandn(err_template)*err_Theta_std;
obs_Theta = true_Theta + err_Theta;
% node orientation
true_Omiga = rand(1,N_node)*2*pi;
for i = 1:N_original_anchor
true_Omiga(i) = 0;
end
% take account of orientation in simulaiton of Theta observation
for j = 1:N_node
for i = 1:N_node
if ( i ~= j)
tmp_val = obs_Theta(i,j) - true_Omiga(j);
if tmp_val < 0
tmp_val = tmp_val + 2*pi;
end
if tmp_val > 2*pi
tmp_val = tmp_val - 2*pi;
end
obs_Theta(i,j) = tmp_val;
end
end
end
% screw some range measurements to simulate outliers
obs_R(5,6) = obs_R(5,6) + 10;
obs_R(10,15) = obs_R(10,15) + 2;
% screw up of zenith does not matter
obs_phi(14,9) = obs_Phi(14,9)+pi/3;
% from now on, to localize nodes
est_loc = zeros(3, N_node);
est_loc_var = zeros(3,3,N_node);
est_Omiga = zeros(1, N_node);
est_Omiga_var = zeros(1, N_node);
est_Theta = zeros(N_node, N_node);
% anchor node has non-zero location uncertainty and orientation uncertainty
for i = 1:N_original_anchor
est_loc(:,i) = true_loc(:,i) + randn(3,1)*2*err_R_std;
est_loc_var(:,:,i) = eye(3,3)*(2*err_R_std)^2;
est_Omiga(i) = true_Omiga(i);
est_Omiga_var(i) = (err_Theta_std/100)^2;
end
% scan range measurement outliers
% if difference between j's range measurement to i and i's range measurement to j
% great than 4 * sqrt(2) * err_R_std,
% then either i's measurement or j's measurement is an outliner.
% Since it is hard to differentiate them, we get rid of both
for i = 1:N_node
for j = 1:N_node
if abs(obs_R(i,j) - obs_R(j,i)) > 5.6569*err_R_std
usable(i,j) = 0;
usable(j,i) = 0;
end
end
end
% if is_anchor(i) ==1, then node i can be used as an anchor node,
% either original anchor node, or derived anchor node
is_anchor = zeros(N_node,1);
is_anchor(1:N_original_anchor,1) = ones(N_original_anchor,1);
% if anchor(i,j) == 1, then node i can use node j as anchor node
anchor = zeros(N_node, N_node);
for i = N_original_anchor+1:N_node
for j = 1:N_node
if usable(i,j) == 1 & is_anchor(j)
anchor(i,j) = 1;
end
end
end
delt_loc = zeros(3, N_node);
continue = 1;
while continue == 1;
% select the node with maximum number of anchor nodes to localize
tmp_col = sum(anchor, 2);
[N_anchor, i] = max(tmp_col);
if N_anchor <= 0 | i <= N_original_anchor
continue = 0;
break;
end
%i
%N_anchor
anchor_idx = [];
for j = 1:N_node
if anchor(i,j) == 1
anchor_idx = [anchor_idx; j];
end
end
fused_loc = zeros(3,1);
fused_loc_var_inv = zeros(3,3);
% iterate through all anchor nodes
% (1) only use original anchor nodes
%N_anchor = N_original_anchor;
% (2) also use localized nodes (derived anchor nodes)
% (2) performs much better in terms accuracy and convergence
% phase 1: estimate node location approximately
for h = 1:N_anchor
j = anchor_idx(h);
tmp_R = (obs_R(i,j) + obs_R(j,i))/2;
tmp_R_var = err_R_std^2/2;
tmp_Theta = obs_Theta(i,j) + est_Omiga(j);
tmp_Theta_var = err_Theta^2 + est_Omiga_var(j);
tmp_Phi = (obs_Phi(i,j) + pi - obs_Phi(j,i))/2;
tmp_Phi_var = err_Phi_std^2/2;
tmp_loc = zeros(3,1);
tmp_loc(1) = est_loc(1,j) + tmp_R * sin(tmp_Phi) * cos(tmp_Theta);
tmp_loc(2) = est_loc(2,j) + tmp_R * sin(tmp_Phi) * sin(tmp_Theta);
tmp_loc(3) = est_loc(3,j) + tmp_R * cos(tmp_Phi);
tmp_loc_var = zeros(3,3);
tmp_loc_var(1,1) = est_loc_var(1,1,j) + (sin(tmp_Phi))^2*(cos(tmp_Theta))^2*err_R_std^2;
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;
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;
tmp_loc_var(2,2) = est_loc_var(2,2,j) + (sin(tmp_Phi))^2*(sin(tmp_Theta))^2*err_R_std^2;
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;
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;
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;
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;
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;
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;
tmp_loc_var(2,1) = tmp_loc_var(1,2);
tmp_loc_var(1,3) = est_loc_var(1,3,j) + 0.5*sin(2*tmp_Phi)*cos(tmp_Theta)*err_R_std^2;
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;
tmp_loc_var(3,1) = tmp_loc_var(1,3);
tmp_loc_var(2,3) = est_loc_var(2,3,j) + 0.5*sin(2*tmp_Phi)*sin(tmp_Theta)*err_R_std^2;
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;
tmp_loc_var(3,2) = tmp_loc_var(2,3);
tmp_loc_var_inv = inv(tmp_loc_var);
fused_loc_var_inv = fused_loc_var_inv + tmp_loc_var_inv;
fused_loc = fused_loc + tmp_loc_var_inv * tmp_loc;
% update the observed R with fused R
obs_R(i,j) = tmp_R;
obs_R(j,i) = tmp_R;
end
est_loc_var(:,:,i) = inv(fused_loc_var_inv);
est_loc(:,i) = est_loc_var(:,:,i)*fused_loc;
% increase the variance estimation by 4 times to compensate the non-gaussian reality
est_loc_var(:,:,i) = est_loc_var(:,:,i)*4;
% Phase 2: we need local optimization of node localization
% if it is well constrained by enough anchor nodes
delt_loc(:,i) = ones(3,1);
n_opt = 0;
while N_anchor >= 4 & delt_loc(:,i)'*delt_loc(:,i) > step^2 & n_opt < max_opt
n_opt = n_opt + 1;
A = [];
b = [];
for h = 1:N_anchor
j = anchor_idx(h);
A(h,1) = est_loc(1,i) - est_loc(1,j);
A(h,2) = est_loc(2,i) - est_loc(2,j);
A(h,3) = est_loc(3,i) - est_loc(3,j);
b(h) = 0.5*(obs_R(i,j)^2 - ((A(h,1))^2+(A(h,2))^2+(A(h,3))^2));
% weighted least square is no better than least square
% weight the equation with max variance of Xj, Yj, and Zj
[max_var, max_dim] = max(diag(est_loc_var(:,:,j)),[],1);
A(h,:) = A(h,:)/sqrt(max_var);
b(h) = b(h)/sqrt(max_var);
end
% cut adjustment step length by half to compensate the no-trivial deviation from optimal solution
b = b';
tmp_mtx = A'*A;
if cond(tmp_mtx) < 1000 & cond(tmp_mtx) > 1
delt_loc(:,i) = inv(A'*A)*A'*b;
est_loc(:,i) = est_loc(:,i) + delt_loc(:,i);
else
delt_loc(:,i) = zeros(3,1);
end
end
% Phase 3: estimate node orientation
% estimate azimuth of node i relative to node j's
for h = 1:N_anchor
j = anchor_idx(h);
% estimate Theta(i,j) using node i and j's location
tmp_val = atan2(est_loc(2,i)-est_loc(2,j), est_loc(1,i)-est_loc(1,j));
if tmp_val < 0
tmp_val = tmp_val + 2*pi;
end
est_Theta(i,j) = tmp_val;
end
% to fuse multiple orientation estimates is hard
% because two close orientations might have a value difference of about 2*pi
% we first estimate orientation of node i using its azimuth measurement of anchor 1
% we then use this estimate as a reference in the fusion of multiple orientation estimates
% (omiga(i) + theta(j,i)) - theta(i,j) = 2*k*pi + pi, k is integer
tmp_val = pi + est_Theta(i,anchor_idx(1)) - obs_Theta(anchor_idx(1),i);
% put it in degree range of [0, 360)
ref_Omiga = mod(tmp_val*180/pi, 360);
tmp_Omiga = ref_Omiga;
tmp_Omiga_var = 0;
for h = 2:N_anchor
j = anchor_idx(h);
% (omiga(i) + theta(j,i)) - theta(i,j) = 2*k*pi + pi, k is integer
tmp_val = pi + est_Theta(i,1) - obs_Theta(1,i);
% put it in degree range of [0, 360)
tmp_val = mod(tmp_val*180/pi, 360);
if tmp_val - ref_Omiga > 180
tmp_val = tmp_val - 360;
end
if ref_Omiga - tmp_val > 180
tmp_val = tmp_val + 360;
end
tmp_Omiga = tmp_Omiga + tmp_val;
tmp_Omiga_var = tmp_Omiga_var + (tmp_val-tmp_Omiga/h)^2;
end
tmp_Omiga = tmp_Omiga/N_anchor;
tmp_Omiga = mod(tmp_Omiga, 360);
est_Omiga(i) = tmp_Omiga*pi/180;
% increase the variance estimation by 16 times to compensate the non-gaussian reality
est_Omiga_var(i) = 16*max((tmp_Omiga_var/N_anchor)*(pi/180)^2, err_Theta_std^2);
% update anchor matrix so that this node will not compete for localization
anchor(i,:) = -1 * ones(1, N_node);
if delt_loc(:,i) ~= zeros(3,1)
is_anchor(i) = 1;
for j =1:N_node
if usable(j,i) == 1 & ~is_anchor(j)
anchor(j,i) = 1;
end
end
end
end
loc_err3 = sum(sum(abs(est_loc-true_loc),1),2)/N_node;
% phase 4: additional round of location optimizaiton
for i = N_original_anchor+1:N_node
if 1 %delt_loc(:,i) == zeros(3,1);
mk_delt_loc(:,i) = ones(3,1);
n_opt = 0;
while mk_delt_loc(:,i)'*mk_delt_loc(:,i) > step^2 & n_opt < max_opt
n_opt = n_opt + 1;
A = [];
b = [];
k = 0;
for j = 1:N_node
if usable(i,j) == 1
k = k + 1;
A(k,1) = est_loc(1,i) - est_loc(1,j);
A(k,2) = est_loc(2,i) - est_loc(2,j);
A(k,3) = est_loc(3,i) - est_loc(3,j);
b(k) = 0.5*(obs_R(i,j)^2 - ((A(k,1))^2+(A(k,2))^2+(A(k,3))^2));
% weighted least square is no better than least square
% weight the equation with max variance of Xj, Yj, and Zj
[max_var, max_dim] = max(diag(est_loc_var(:,:,j)),[],1);
A(k,:) = A(k,:)/sqrt(max_var);
b(k) = b(k)/sqrt(max_var);
end
end
% cut adjustment step length by half to compensate the no-trivial deviation from optimal solution
b = b';
tmp_mtx = A'*A;
if cond(tmp_mtx) < 1000 & cond(tmp_mtx) > 1
mk_delt_loc(:,i) = inv(A'*A)*A'*b;
est_loc(:,i) = est_loc(:,i) + mk_delt_loc(:,i);
else
mk_delt_loc(:,i) = zeros(3,1);
end
end
end
end
loc_err4 = sum(sum(abs(est_loc-true_loc),1),2)/N_node;
% phase 5: additional round Omiga optimization
for i = 1:0%N_node
for j = 1:N_node
if usable(i,j) == 1
% estimate Theta(i,j) using node i and j's location
tmp_val = atan2(est_loc(2,i)-est_loc(2,j), est_loc(1,i)-est_loc(1,j));
if tmp_val < 0
tmp_val = tmp_val + 2*pi;
end
est_Theta(i,j) = tmp_val;
end
end
ref_Omiga = est_Omiga(i)*180/pi;
tmp_Omiga = 0;
n_neighbor = 0;
for j = 1:N_node
if usable(i,j) == 1
n_neighbor = n_neighbor + 1;
% (omiga(i) + theta(j,i)) - theta(i,j) = 2*k*pi + pi, k is integer
tmp_val = pi + est_Theta(i,1) - obs_Theta(1,i);
% put it in degree range of [0, 360)
tmp_val = mod(tmp_val*180/pi, 360);
if tmp_val - ref_Omiga > 180
tmp_val = tmp_val - 360;
end
if ref_Omiga - tmp_val > 180
tmp_val = tmp_val + 360;
end
tmp_Omiga = tmp_Omiga + tmp_val;
end
end
tmp_Omiga = tmp_Omiga/n_neighbor;
tmp_Omiga = mod(tmp_Omiga, 360);
est_Omiga(i) = tmp_Omiga*pi/180;
est_Omiga_var(i) = err_Theta_std^2/n_neighbor;
end
% remove translation in plot
trans = (sum(est_loc, 2) - sum(true_loc, 2))/N_node;
for i = 1:N_node
est_loc(:,i) = est_loc(:,i) - trans;
end
figure(1);
plot3(true_loc(1, 1), true_loc(2,1), true_loc(3,1), 'cs', 'LineWidth',2);
hold on;
plot3(true_loc(1,N_node),true_loc(2,N_node),true_loc(3,N_node),'ro');
plot3(est_loc(1,N_node),est_loc(2,N_node),est_loc(3,N_node),'.');
legend('Anchor Node', 'True Location', 'Estimated Location');
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');
for i = 1:N_original_anchor
plot3(true_loc(1,i), true_loc(2,i), true_loc(3,i), 'cs','LineWidth',2);
end
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), '.');
for i=N_original_anchor+1:N_node
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)]);
end
axis([0 side 0 side 0 height]); %axis vis3d;
xlabel('x');
ylabel('y');
zlabel('z');
grid on;
hold off;
% to remove the difference of about 360 degree in the plot of Orientation
for i = 1:N_node
if est_Omiga(i) - true_Omiga(i) > pi
est_Omiga(i) = est_Omiga(i) - 2*pi;
end
if true_Omiga(i) - est_Omiga(i) > pi
est_Omiga(i) = est_Omiga(i) + 2*pi;
end
end
figure(2);
plot(N_original_anchor+1,true_Omiga(N_original_anchor+1),'ro');
hold on;
plot(N_original_anchor+1, est_Omiga(N_original_anchor+1), '.');
legend('True Orientation', 'Estimated Orientation');
plot(N_original_anchor+1:N_node, true_Omiga(N_original_anchor+1:N_node),'ro');
plot(N_original_anchor+1:N_node, est_Omiga(N_original_anchor+1:N_node),'.');
for i = N_original_anchor+1:N_node
plot([i,i], [true_Omiga(i), est_Omiga(i)]);
end
hold off;
xlabel('Node ID');
ylabel('Orientation');
figure(3);
imagesc(usable);
colorbar;
See more files for this project here