find_lse_position.m

LSE参考点定位算法在UWB中的仿真

% 
% FUNCTION  : "find_LSE_position"
%
% 该函数通过LSE算法定位
%
% 输入参数:
%  positions：包含各个结点坐标参数的大小为Nx2的二维数组
%  ranges：包含任两点间距离大小为NxN的二维数组
%  Nx:目标结点（待定位结点）序号
%  Ref: 长度为k的参考结点数组 
%  sigma_2:测距误差
%  G:是否输出图形
%
% 函数返回计算所得的目标结点位置并参照其实际位置计算定位误差
% 


function [PosNx, ErrNx] = find_LSE_position(positions, ranges, Nx, Ref,sigma_2, G);

% 加入误差

N = size(ranges,1);
err_ranges = ranges + sqrt(sigma_2)*randn(N);

% 构造矩阵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;
% 构造矩阵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


% 计算目标结点位置
PosNx=A\b;

% 计算定位误差
ErrNx = sqrt((PosNx(1)-positions(Nx,1))^2+(PosNx(2)-...
   positions(Nx,2))^2);

% 图形输出
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