Return to loc_test.m CVS log

Jump to this file's LXR Page

Up to [CENS] / emstar / devel / loc / multilat

initial checkin of multilat code... none of it works yet and is very incomplete

% 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;