function [SourceLocation] = FindSensorPos(S, r, Cov)
%
%  [SourceLocation] = FindSensorPos(S, r, Cov)
%
% Find the source position by TOA using the two stage method.  It works for
% 2-D and 3-D.  For reference, please look at
% Y. T. Chan and K. C. Ho, "A simple and efficient estimator for hyperbolic
% location," IEEE Trans. Signal Process., vol. 42, no. 8, pp. 1905-1915, 
% Aug. 1994.
%
% Input Parameter List:
% S:        KxM, receiver positions, K is localization dimension, M is 
%                number of receivers.
% r:        Mx1, distance measurements.
% Cov:      MxM, covariance matrix of distance measurements.
%
% Output Parameter List:
% SourceLocation:   Kx1, source location. 
%
%
% Shanjie Chen, Benjian Hao, K. C. Ho    11-18-2019
%
%       Copyright (C) 2019
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA.
%       hod@missouri.edu
%

RptCnt = 1;                         % number of repetitions in first step

M = size(S,2);                      % Number of receivers.
dim = size(S,1);                    % Dimension of localization
P = sum(S.^2)';

%=========== construct related vector and matrix ============
h1 = r.^2-P;
G1 = [-2*S', 1*ones(M,1)];

%============= first step ===================================
rHat=r;
for cnt=1:RptCnt,
    W1=inv(diag(rHat)*Cov*diag(rHat));
    G1W1 = G1'*W1;
    u1 = (G1W1*G1)\(G1W1*h1);
    rHat=sqrt(sum((repmat(u1(1:dim),1,M)-S).^2))';
end;
u1p = u1;

%========== second step =====================================
h2 = [u1p(1:end-1).^2;u1p(end)];
G2 = [eye(dim);ones(1,dim)];
B2 = diag([u1p(1:end-1);1/2]);
W2 = (B2')\(G1'*W1*G1)/B2;
G2W2 = G2'*W2;
u2 = (G2W2*G2)\(G2W2*h2);

%=========== mapping ========================================
SourceLocation = sign(u1p(1:dim)).*sqrt(abs(u2));
%============================================================
%
% if u1(end) < 0 || min(u2) < 0
%     SourceLocation = u1(1:length(u2));
% end
