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