function u = TDOALocSenPosErrCalEmtr(s,r,r_tilde,c,Q_alpha,Q_c,Q_beta)
%
% This program realizes the algorithm for localizing a stationary source 
% using TDOAs from the unknown source and a calibration emitter at perfect
% location. The details of the algorithm can be found in K. C. Ho and
% Le Yang, "On the Use of a Calibration Emitter for Source Localization in 
% the Presence of Sensor Position Uncertainty," IEEE Trans. Signal
% Processing, Vol. 56, pp. 5758-5772, Dec. 2008.
%
% Usage: u = TDOALocSenPosErrCalEmtr(s,r,r_tilde,c,Q_alpha,Q_c,Q_beta)
%
% Input parameter list:
% s: 2xM or 3xM sensor position matrix. 
%    M: the number of sensors.
%    s(:,i) is the known position of the ith sensor.
% r = [r_21,r_31,...,r_M1]' 
%    (M-1)x1 RD measurement vector from the unknown source.
% r_tilde = [rt_21,rt_31,...,rt_M1]' 
%    (M-1)x1 RD measurement vector from the calibration emitter.
% c : 2x1 or 3x1 calibration position vector.
% Q_alpha: (M-1)x(M-1) covariance matrix of r.
% Q_c: (M-1)x(M-1) covariance matrix of r_tilde.
% Q_beta: 2Mx2M or 3Mx3M covariance matrix of the sensor positions  
%                                       reshape(s,2M,1) or reshape(s,3M,1).
%
% The program returns a 2x1 or 3x1 estimated source location.
%
% Note: W1 is updated 3 times (RptCnt=3) in Stage-2, however in most
% cases updating W1 once (RptCnt=1) is sufficient.
%
% The program can be used for 2D(Dim=2) or 3D(Dim=3) localization.
%
% Le Yang, K. C. Ho      08-01-2009
%                        10-01-2010, revised
%
%       Copyright (C) 2009
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA.
%       hod@missouri.edu
%

RptCnt = 3;     % number of repetitions in Stage-2 to recompute W1

rt = r_tilde; Qa = Q_alpha; Qc = Q_c; Qb = Q_beta;
M = size(s,2);                      % Number of Sensors.
N = size(s,1);                      % Dimension of the localization problem.

% Stage-1 processing.
for j = 2 : M 
    hc(j-1,1) = rt(j-1) - norm(c-s(:,j)) + norm(c-s(:,1));
    Gc(j-1,:) = [-(c-s(:,1))'/norm(c-s(:,1)),zeros(1,N*(j-2)),(c-s(:,j))'/norm(c-s(:,j)),zeros(1,N*(M-j))];
end;

cov_s_hat = inv(inv(Qb) + Gc'*inv(Qc)*Gc);           
Delta_s_hat = cov_s_hat * Gc'*inv(Qc)*hc;
s_hat = reshape(reshape(s,N*M,1) - Delta_s_hat,N,M);  % Update sensor positions.

% Stage-2 processing.
for j = 2 : M
    h1(j-1,1) = r(j-1)^2- s_hat(:,j)' * s_hat(:,j) + s_hat(:,1)' * s_hat(:,1);
    G1(j-1,:) = -2 * [(s_hat(:,j) - s_hat(:,1))', r(j-1)];
end;
               
W1 = inv(Qa);          
phi1 = inv(G1'*W1*G1)*G1'*W1*h1;
u = phi1(1 : N);                    
    
for m = 1 : max(1,RptCnt)                       % Iterate to update W1.
    B1 = repmat(u,1,M) - s_hat;
    B1 = 2 * diag(sqrt(sum(B1(:,2:end).^2, 1)));
    b1 = (u - s_hat(:,1))'/norm(u - s_hat(:,1));
    for j = 2 : M
        D1(j-1,:) = 2 * [-(r(j-1) * b1 + (u-s_hat(:,1))'), zeros(1,N*(j-2)),(u-s(:,j))',zeros(1,N*(M-j))];
    end;
    W1 = inv(B1*Qa*B1'+D1*cov_s_hat*D1');
    phi1 = inv(G1'*W1*G1)*G1'*W1*h1;
    u = phi1(1 : N);
end;        
cov_phi1 = inv(G1'*W1*G1);

% Stage-3 processing.
h2(:,1) = [((u - s_hat(:,1)).^2); phi1(end)^2];
G2 = [eye(N);ones(1,N)];
B2 = 2 * diag([u-s_hat(:,1);norm(u-s_hat(:,1))]);
W2 = inv(B2*cov_phi1*B2');
phi2 = inv(G2'*W2*G2)*G2'*W2*h2;

% Return the source location estimate.
u = diag(sign(u-s_hat(:,1))) * sqrt(phi2) + s_hat(:,1); 