function u = TDOALocSenPosErrMultiCalEmtrIaccLoc(s,r,r_tilde,c,Q_alpha,Q_c,Q_beta,Q_e)
% This program realizes the algorithm for localizing a stationary source 
% using TDOAs from the unknown source and multiple calibration emitters at 
% inaccurate locations. 
% 
% The details of the algorithm can be found in Le Yang and K. C. Ho, 
% "Alleviating sensor position error in source localization using 
% calibration emitters at inaccurate locations," IEEE Trans. Signal 
% Processing, Vol. 58, pp. 67-83, Jan. 2010.
%
% Usage: u = TDOALocSenPosErrMultiCalEmtriaccLoc(s,r,r_tilde,c,Q_alpha,Q_c,Q_beta,Q_e)
%
% 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 TDOA (range difference) measurement vector from the unknown source.
% r_tilde = [rt_{21,1},rt_{31,1},...,rt_{M1,1},rt_{21,2},...,rt_{M1,N}]' 
%    N(M-1)x1 TDOA (range difference) measurement vector from the calibration emitters.
% c: 2Nx1 or 3Nx1 known calibration position vector.
% Q_alpha: (M-1)x(M-1) covariance matrix of r.
% Q_c: N(M-1)xN(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).
% Q_e: 2Nx2N or 3Nx3N covariance matrix of the calibration position vector c.
%
% 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 and K. C. Ho   10-01-2010
%
%       Copyright (C) 2010
%       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

M = size(s,2);        % Number of sensors.
D = size(s,1);        % Dimension of the localization problem.
N = length(c)/D;      % Number of calibration emitters.

Qa = Q_alpha;   Qc = Q_c;   Qb = Q_beta;   Qe = Q_e;

% Stage-1 processing.
for i = 1 : N
    c1 = c((i-1)*D+1:i*D);
    for j = 2 : M 
        hc((i-1)*(M-1)+j-1,1) = r_tilde((i-1)*(M-1)+j-1)-norm(c1-s(:,j))+norm(c1-s(:,1));
        Gc((i-1)*(M-1)+j-1,:) = [-(c1-s(:,1))'/norm(c1-s(:,1)),zeros(1,D*(j-2)),(c1-s(:,j))'/norm(c1-s(:,j)), zeros(1,D*(M-j))];
        Ge((i-1)*(M-1)+j-1,:) = [zeros(1,D*(i-1)),(c1-s(:,1))'/norm(c1-s(:,1))-(c1-s(:,j))'/norm(c1-s(:,j)),zeros(1,D*(N-i))];
   end;
end;
Deltas_hat = inv(inv(Qb)+Gc'*inv(Qc + Ge*Qe*Ge')*Gc)*Gc'*inv(Qc + Ge*Qe*Ge')*hc;
cov_Deltas_hat = inv(inv(Qb)+Gc'*inv(Qc+Ge*Qe*Ge')*Gc);
               
% Stage-2 processing.
s_hat = reshape(reshape(s,D*M,1) - Deltas_hat,D,M);
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);          % Initial guess for W1.
phi1 = inv(G1'*W1*G1)*G1'*W1*h1;
u = phi1(1:D);
    
for m = 1 : max(1,RptCnt)          % Iterate to update W1.
    B1 = sqrt(sum((repmat(u,1,M-1) - s_hat(:,2:M)).^2,1));    B1 = 2 * diag(B1);
    b1 = (u - s_hat(:,1))'/norm(u - s_hat(:,1));
    for j = 2 : M
        D1_temp = r(j-1) * b1 + (u-s_hat(:,1))';
        D1(j-1,:) = 2*[-D1_temp, zeros(1,D*(j-2)), (u-s_hat(:,j))', zeros(1,D*(M-j))];
    end;
    W1 = inv(B1*Qa*B1'+D1*cov_Deltas_hat*D1');
    phi1 = inv(G1'*W1*G1)*G1'*W1*h1;
    u = phi1(1:D);
end;        
cov_phi1 = inv(G1'*W1*G1);
              
% Stage-3 processing.
h2 = [(u - s_hat(:,1)).^2;phi1(end)^2];
G2 = [eye(D);ones(1,D)];
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;
            
u = diag(sign(u-s_hat(:,1))) * sqrt(abs(phi2)) + s_hat(:,1);