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