function [SourceLocation] = TDOALoc_BiasSub(SensorPositions, rd, Q),
%
% This program realizes the BiasSub 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_BiasSub(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.
%
% There are two approaches to remove the bias.  The first is to subtract
% the bias in each stage and the second is to subtract the overall bias in
% the final solution.  The first approach could give smaller bias.  Which
% approach to use is decided by the variable BiasSubIndvStage 
%
% 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
%

BiasSubIndvStage = 1;   % =1 if remove bias in individual stage, 
                        % =0 if remove bias only once at the final solution

M = length(rd) + 1;

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 = 1;     % number of repetitions in the first stage

S=SensorPositions;
R=sqrt(sum(S.^2))';

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

%============= first stage ===================================
B = eye(M-1);
W1 = inv(B*Q*B');
u1 = inv(G1'*W1*G1)*G1'*W1*h1;

for j = 1:max(1,RptCnt),
    ri_hat = sqrt(sum((S-u1(1:end-1)*ones(1,M)).^2));
    B = 2*diag(ri_hat(2:M));
    W1 = inv(B*Q*B');
    u1 = inv(G1'*W1*G1)*G1'*W1*h1;
end

if (BiasSubIndvStage)
    % ----- remove bias in stage 1 solution -----
    [biasThy,biasThy_psi1]=BiasTDOALoc(S,[0;rd]+norm(u1(1:end-1)),Q,u1(1:end-1));
    u1 = u1-biasThy_psi1;
end;

%========== second stage =====================================
u1p = u1 - [S(:,1);0];

h2 = u1p.^2;
G2 = [eye(length(u1p)-1);ones(1,length(u1p)-1)];

for kk=1:2-1,
    B2 = 2*diag(u1p);
    W2 = inv(B2')*(G1'*W1*G1)*inv(B2);
    
    u2 = inv(G2'*W2*G2)*G2'*W2*h2;
    uhat=sign(diag(u1p(1:length(u2))))*sqrt(abs(u2)) + S(:,1);
    u1p = [ uhat-S(:,1); norm(uhat-S(:,1))];
end;

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

if u1(end) < 0 || min(u2) < 0
    SourceLocation = u1(1:length(u2));
end

% ----- remove bias in solution -----
[biasThy,biasThy_psi1,biasThy_psi2,biasThy_psi1unbiased] = BiasTDOALoc(S,[0;rd]+norm(SourceLocation-S(:,1)),Q,SourceLocation);

if (BiasSubIndvStage)
    SourceLocation = SourceLocation-biasThy_psi1unbiased;
else
    SourceLocation = SourceLocation-biasThy;
end; 
