function u = TDOALocSenPosErrMultiDisjSrc(s,r,Q_alpha,Q_beta)
% This program realizes the algorithm for localizing multiple disjoint sources 
% using TDOAs from the unknown sources when the sensor positions are subject to
% random noise. 
%
% The details of the algorithm can be found in Le Yang and K. C. Ho,
% "An approximately efficient TDOA localization algorithm in closed-form 
% for locating multiple disjoint sources with erroneous sensor positions," 
% IEEE Trans. Signal Processing, Vol. 57, pp. 4598-4615, Dec. 2009.
%
% Usage: u = TDOALocSenPosErrMultiDisjSrc(s,r,Q_alpha,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,1},r_{31,1},...,r_{M1,1},r_{21,2},...,r_{M1,N}]' 
%    N(M-1)x1 TDOA (range difference) measurement vector from the unknown sources.
%    N: the number of sources to be localized.
% Q_alpha: N(M-1)xN(M-1) covariance matrix of r.
% Q_beta: 2Mx2M or 3Mx3M covariance matrix of the sensor positions  
%                                       reshape(s,2M,1) or reshape(s,3M,1).
% This program requires TDOALocStnySenPosErr(s,rt,Q_beta,Qr) to produce the
% hypothesized source positions.
%
% The program returns a 2Nx1 or 3Nx1 estimated source location vector.
%
% 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
%

M = size(s,2);         % Number of sensors.
D = size(s,1);         % Dimension of the localization problem.
N = length(r)/(M-1);   % Number of sources to be localized.

% Stage-1 Processing.
for i = 1 : N
    rt = r((i-1)*(M-1)+1:i*(M-1));          % TDOAs from source i.
    Qa = Q_alpha((i-1)*(M-1)+1:i*(M-1),(i-1)*(M-1)+1:i*(M-1));
    c = TDOALocStnySenPosErr(s,rt,Q_beta,Qa);
    u((i-1)*D+1:i*D,1) = c;                 % Hypothesized source position.
    
    for j = 2 : M
        G((i-1)*(M-1)+(j-1),:) = [zeros(1,(i-1)*(D+1)),2*[(s(:,j)-s(:,1))',-rt(j-1)],zeros(1,(N-i)*(D+1))];
        eta((i-1)*(M-1)+(j-1),1) = rt(j-1)^2-s(:,j)'*s(:,j)+s(:,1)'*s(:,1)+ 2*(s(:,j)-s(:,1))'*c;
        D1((i-1)*(M-1)+(j-1),:) = 2*[-(c-s(:,1))'-rt(j-1)*(c-s(:,1))'/norm(c-s(:,1)),zeros(1,(j-2)*D),(c-s(:,j))',zeros(1,(M-j)*D)];
        
        b1((i-1)*(M-1)+(j-1),1) = norm(c-s(:,j));
    end;
end;

h1 = [eta; zeros(M*D,1)];         G1 = [G,D1];      
G1 = [G1; zeros(M*D,(D+1)*N),eye(M*D)];

B1 = 2 * diag(b1);
W1 = [B1 * Q_alpha * B1',   zeros((M-1)*N,M*D);
     zeros((M-1)*N,M*D)',   Q_beta];
W1 = inv(W1);
phi1 = inv(G1'*W1*G1)*G1'*W1*h1;    

for i = 1 : N
    u((i-1)*D+1:i*D)=u((i-1)*D+1:i*D) - phi1((i-1)*(D+1)+1:i*(D+1)-1);
end;
cov_phi1 = inv(G1'*W1*G1);
cov_theta = cov_phi1(1:(D+1)*N,1:(D+1)*N);

% Stage-2 Processing.
for i = 1 : N
    h2((i-1)*(D+1)+1:i*(D+1),1) = [u((i-1)*D+1:i*D)-s(:,1); phi1(i*(D+1))].^2;
    G2((i-1)*(D+1)+1:i*(D+1),:) = [zeros(D+1,D*(i-1)),[eye(D);ones(1,D)],zeros(D+1,D*(N-i))];
    B2((i-1)*(D+1)+1:i*(D+1),1) = [-u((i-1)*D+1:i*D)+s(:,1); phi1(i*(D+1))];
end;
B2 = 2* diag(B2);
W2 = inv(B2*cov_theta*B2');
phi2 = inv(G2'*W2*G2)*G2'*W2*h2;

for i = 1 : N
    b((i-1)*D+1:i*D,1) = sign(u((i-1)*D+1:i*D)-s(:,1));
end;
B = diag(b);

% Return the source location estimate.
u = B * sqrt(abs(phi2)) + kron(ones(N,1),s(:,1));