function [uBR,uP] = TDOALoc_LER(S0,center,rd,Q,num)
%
% function [uBR,uP] = TDOALoc_LER(S0,center,rd,Q,num)
%
% The program returns the source location estimate where the localization scenario is LER.
% The program can be used for 2D(Dim=2) or 3D(Dim=3) localization.
%
% Input Parameter List
% S0:  (2xM) or (3xM) true sensor position matrix.
%      M: number of sensors.
%      N: dimension of the localization problem
%      S0(:,i) is the true position of the ith sensor.
% center: the origin that defines the sensor positions
% rd:  noisy TDOAs (range differences).
% Q:   (M-1)x(M-1) covariance matrix of the TDOAs (range differences).
% num: the number of iterations used to obtain a better solution if the
%      localziation scenario is non-LER
%
% Output Parameter List
% uBR: biased reduced solution
% uP:  preliminary solution, may have a large amount of bias
%
% Reference: S. Li and K. C. Ho, “Accurate and effective localization of an
% object in large equal radius scenario,” IEEE Trans. Wireless Commun.,
% vol. 15, no. 12, pp. 8273-8285, Dec. 2016.
%
% Sasa Li, K. C. Ho    12-10-2016
%
%       Copyright (C) 2016
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA.
%       hod@missouri.edu
%

M = size(S0, 2);        % number of sensors
N = size(S0, 1);        % localization dimension

origin = center;

W=inv(Q/mean(trace(Q)));     % weighting matrix

for i = 1: num,         % iterate if scenario is not LER
    
    % -- obtain preliminary solution --
    S1 = S0 - origin*ones(1,M);
    R = sqrt(sum(S1.^2))';  % sensor range matrix
    Rd = R(2:end) - R(1);   % sensor range differences
    h1 = rd - Rd;
    temp = 1./(R(2:end) + R(1));
    G1(:,1) = ((S1(1,2:M)'./R(2:M)+S1(1,1)'/R(1)).*rd - 2*(S1(1,2:M) - S1(1,1))').*temp;
    G1(:,2) = ((S1(2,2:M)'./R(2:M)+S1(2,1)'/R(1)).*rd - 2*(S1(2,2:M) - S1(2,1))').*temp;
    if N==3
        G1(:,3) = ((S1(3,2:M)'./R(2:M)+S1(3,1)'/R(1)).*rd - 2*(S1(3,2:M) - S1(3,1))').*temp;
    end
    temp1 = inv(G1'*W*G1)*G1'*W;
    phi = temp1*h1;
    uP = phi;
    
    % -- remove bias from uP --
    r1 = sum((phi*ones(1,M) - S1).^2)';
    cos_beta = (R.^2 + r1 - phi'*phi)./(2*R.*sqrt(r1));
    temp3 = (1 - 1./cos_beta(2:end)).*(R(2:end)-(phi'*S1(:,2:end))'./R(2:end));
    temp2 = (1 - 1./cos_beta(1))*(R(1)-phi'*S1(:,1)/R(1));
    d_h = rd.*temp.*(temp3 + temp2);
    bias = temp1*d_h;
    phi = phi - bias;
    
    pos = phi + origin;
    origin = pos;
    
end

uBR = pos;