function [bias,bias_psi1,bias_psi2,bias_psi1unbiased]=BiasTDOALoc(S,ro,Q,uo)
%
% This program computes the theoretical bias of the Chan&Ho1994 solution for
% localizing a stationary source using TDOAs. The sensors are stationary as well.
% The details of the bias derivation 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: [bias,bias_psi1,bias_psi2,bias_psi1unbiased]=BiasTDOALoc(S,ro,Q,uo);
%
% Input Parameter List:
% S:    2xM or 3xM sensor position matrix.
%       M: number of sensors.
%       S(:,i) is the known position of the ith sensor.
% ro:   Mx1 range vector.
% Q:    (M-1)x(M-1) covariance matrix of range difference.
%
% Output Parameter List:
% bias:         bias in the final source location estimate
% bias_psi1:    bias in the first stage solution psi1
% bias_psi2:    bias in the second stage solution psi2
% bias_psi1ub:  bias in final source location estimate when psi1 is unbiased
%               (used in BiasSub solution method)
%
%
% The program returns a 2x1 or 3x1 source location estimate.
%
%
% The program can be used for 2D(Dim=2) or 3D(Dim=3) localization.
%
% ********************************************************************
% CORRECTION IN THE PAPER: In eq. (76), P4(M-1,:) should be P4(N+1,:)
% ********************************************************************
%
% K. C. Ho      07-01-2012
%
%
%       Copyright (C) 2011
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA.
%       hod@missouri.edu
%

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

% ========== bias computation for psi1 ==========
G1o = -2*[S(:,2:end)'-ones(M-1,1)*S(:,1)' ,  (ro(2:end)-ro(1))];
B1o = 2*diag(ro(2:end));
W1 = inv(B1o*Q*B1o);
psi1o = [uo;ro(1)];

GWG = G1o'*W1*G1o;
C1 = inv(GWG);
H1 = C1*G1o'*W1;
bias_psi1 = H1*(ones(M-1,1)*Q(1,1) + 2*Q*B1o*H1(end,:)') + 2*C1*[ zeros(N,1); trace(W1*(G1o*H1-eye(M-1))*B1o*Q)];

% ========== bias computation for psi2 ==========
G2 = [eye(N); ones(1,N)];
B2o = 2*diag([uo-S(:,1); ro(1)]);
iB2o = inv(B2o);
W2o = iB2o*(GWG)*iB2o;
H2 = inv(G2'*W2o*G2)*G2'*W2o;
c1 = diag(C1);
psi2o = (uo-S(:,1)).^2;

P3 = eye(N+1)-G2*H2;
P4 = iB2o*P3*B2o*H1*B1o;
P5 = P3*B2o*C1;
P5W = W2o*P5;
alpha = -2*iB2o*(G1o'*W1*Q*P4(end,:)'+[zeros(N,1); trace(W1*G1o*P4*Q)]);
beta = -2*iB2o*diag(P5W);
gamma = -2*W2o*iB2o*diag(P5);

iW2abg = inv(W2o)*(alpha+beta+gamma);
bias_psi2 = H2*(c1+B2o*bias_psi1+iW2abg);
bias_psi2_psi1ub = H2*(c1+iW2abg);

% ========== total bias in solution ==========
B3o = 2*diag(uo-S(:,1));
iB3o = inv(B3o);
C3 = iB3o*inv(G2'*W2o*G2)*iB3o;
tmp = 0.5*diag(sign(uo-S(:,1)))*diag(1./sqrt(psi2o));

bias = iB3o*(bias_psi2-diag(C3));
bias_psi1unbiased = iB3o*(bias_psi2_psi1ub-diag(C3));
