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