gusucode.com > UWB_matlab源码程序 > CP1002/cp1002_find_LSE_position.m
% % FUNCTION 10.3 : "cp1002_find_LSE_position" % % This function determines the LSE solution to a % positioning problem in a bidimensional space. % The function receives in input: % - A matrix Nx2 containing the (X,Y) positions of each % node % - A matrix NxN containing the distances between each pair % of nodes % - The ID of the target node Nx % - A vector Refs of length k, containing the ID of the % reference nodes % - The value of the sigma_2 % - A flag G to enable/disable the graphical output % % The function returns the estimated position of % the target node Nx and the error with respect to the % exact position ErrNx % % %Programmed by Luca De Nardis % function [PosNx, ErrNx] = ... cp1002_find_LSE_position(positions, ranges, Nx, Ref,... sigma_2, G); % Adding errors to the range estimation N = size(ranges,1); err_ranges = ranges + sqrt(sigma_2)*randn(N); % Defining the linear problem % Matrix A k = length(Ref); for i=1:(k-1) A(i,1) = positions(Ref(i),1) - positions(Ref(k),1); A(i,2) = positions(Ref(i),2) - positions(Ref(k),2); end A=-2*A; % Matrix b b=zeros(2,1); for i=1:(k-1) b(i) = err_ranges(Ref(i),Nx)^2 -... err_ranges(Ref(k),Nx)^2 - positions(Ref(i),1)^2 +... positions(Ref(k),1)^2 - positions(Ref(i),2)^2 +... positions(Ref(k),2)^2; end % Solving the problem PosNx=A\b; %Computing the error ErrNx = sqrt((PosNx(1)-positions(Nx,1))^2+(PosNx(2)-... positions(Nx,2))^2); %Graphical output if G scatter(positions(:,1),positions(:,2)); xlabel('X [m]'); ylabel('Y [m]'); box on; hold on; scatter(PosNx(1), PosNx(2), 200, 'filled', 'k','p'); scatter(positions(Nx,1),positions(Nx,2),200,... 'filled','^'); for i=1:k scatter(positions(Ref(i),1),positions(Ref(i),2),... 'filled','r','s'); end hold off; end