function [SourceLocation] = TDOALoc_BiasRed(SensorPositions, rd, Q)
%
% This program realizes the BiasRed closed-form solution for localizing a
% stationary source using TDOAs. The sensors are stationary as well.
% The details of the algorithm development can be found in K. C. Ho,
% "Bias reduction for an explicit solution of source localization using TDOA,"
% IEEE Trans. Signal Process., vol. 60, pp. 2101-2114, May 2012.
%
% Usage: [SourceLocation] = TDOALoc_BiasRed(SensorPositions, rd, Q);
%
% Input Parameter List:
% SensorPositions:  2xM or 3xM sensor position matrix.
%                   M: number of sensors.
%                   SensorPositions(:,i) is the known position of the ith sensor.
% rd:               (M-1)x1 TDOA(range difference) measurement vector.
% Q:                (M-1)x(M-1) covariance matrix of rd.
%
% The program returns a 2x1 or 3x1 source location estimate.
%
%
% The program can be used for 2D(Dim=2) or 3D(Dim=3) localization.
%
%
% K. C. Ho      07-01-2012
%
%
%       Copyright (C) 2011
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA.
%       hod@missouri.edu
%

M = length(rd) + 1;     % number of sensors

if (M<size(SensorPositions,1)+2)
    fprintf('Number of sensors must be at least %d\n',size(SensorPositions,1)+2);
    SourceLocation=NaN;
    return;
end;

if (rank(SensorPositions) < size(SensorPositions,1))
    disp('The sensors should not lie in one plane or line!');
    SourceLocation=NaN;
    return;
end

RptCnt = 2;      % number of repetitions to improve weighting matrix W1

S = SensorPositions;
SenPosNormSq = sqrt(sum(S.^2))';
N = min(size(S));

%=========== construct related vector and matrix ============
h1 = rd.^2 - SenPosNormSq(2:end).^2 + SenPosNormSq(1)^2;
G1 = -2 * [ S(:,2:end)' - ones(M-1,1)*S(:,1)' , rd ]; 
A=[ G1, -h1 ];

%============= first stage ==================================
B = eye(M-1);

rd_hat=rd;
for j = 1:max(1,RptCnt)+1,  % repeat to improve weighting matrix W1
    W1 = inv(B * Q * B');
    At = A' *W1 * A;
    A11 = At(1:N,1:N);  A12 = At(1:N,N+1:end);  A21 = A12';  A22 = At(N+1:end,N+1:end);
    Av1 = -inv(A11) * A12;
    OmegaTilde(1,1) = trace(W1*Q); 
    OmegaTilde(1,2) = trace(W1*diag(rd_hat)*Q);
    OmegaTilde(2,1) = OmegaTilde(1,2); 
    OmegaTilde(2,2) = trace(diag(rd_hat)*W1*diag(rd_hat)*Q);
    OmegaTilde = 4 * OmegaTilde;    % not really needed
    J = inv(OmegaTilde) * (A22+A21*Av1);
    lambda = 0.5 * ( (J(1,1)+J(2,2)) - sqrt((J(1,1)-J(2,2))^2+4*J(1,2)*J(2,1)) );
    v2 = [ (lambda-J(2,2)) / J(2,1) ; 1 ];
    v1 = Av1 * v2;
    psi1 = [ v1 ; v2(1) ];
    
    ri_hat = sqrt(sum((S-psi1(1:end-1)*ones(1,M)).^2));
    B = 2 * diag(ri_hat(2:M));
    
    rd_hat = rd;
    W1 = inv(B*Q*B');
end;

%========== second stage ====================================
psi1p = psi1 - [ S(:,1) ; 0 ];
h2 = psi1p.^2;
G2 = [ eye(length(psi1p)-1); ones(1,length(psi1p)-1) ];
B2 = 2 * diag(psi1p);
iB2 = inv(B2);
W2 = iB2' * (G1'*W1*G1) * iB2;
psi2 = inv(G2'*W2*G2) * G2'*W2*h2;

%=========== third stage ====================================
SourceLocation = sign(diag(psi1p(1:length(psi2)))) * sqrt(abs(psi2)) + S(:,1);


