function u = TDOALocWghSenPosErrMultiDisjSrc(s,r,Q_beta,Q_alpha),
%
% This program realizes the algorithm for localizing a stationary source 
% using TDOAs in the presence of sensor position errors. The sensors are 
% stationary as well. The details of the algorithm development can be found
% in Ming Sun and K. C. Ho, "An Asymptotically Efficient Estimator for TDOA 
% and FDOA Positioning of Multiple Disjoint Sources in the Presence of 
% Sensor Location Uncertainties," IEEE Trans. Signal Process., vol. 59, 
% pp. 3434-3440, Jul. 2011.
%
% Usage: u = TDOALocWghSenPosErrMultiDisjSrc(s,r,Q_beta,Q_alpha);
% 
% Input Parameter List:
% s : 2xM or 3xM sensor position matrix.
%     M: number of sensors.
%     s(:,i) is the known position of the ith sensor.
% r = [r_1,r_2,...,r_K], r_k =[r_{21,k},r_{31,k},...{r_M1,k}]': (M-1)xK TDOA(range difference) measurement vector.
%     K : number of sources.
% Q_beta: 2Mx2M, or 3Mx3M covariance matrix of the sensor positions
%                                       reshape(s,2M,1) or reshape(s,3M,1).
% Q_alpha: K(M-1)xK(M-1) covariance matrix of r.
%
% The program returns a 2xK or 3xK source location estimate.
%
%
% The program can be used for 2D(Dim=2) or 3D(Dim=3) localization.
%
%
% Ming Sun, K. C. Ho      03-11-2011
%                        
%
%       Copyright (C) 2011
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA.
%       hod@missouri.edu
%

M = size(s,2);                   % Number of Sensors.
N = size(s,1);                   % Dimension of the localization problem.
K = length(r)/(M-1);             % Number of sources.

% Stage-1 processing. 
h1 = [];
G1 = [];
for k = 1 : K
    for j = 2 : M
        h1_k(j-1,1) = (r((k-1)*(M-1)+j-1)^2 - s(:,j)'*s(:,j) + s(:,1)'*s(:,1))/2;
        G1_k(j-1,:) = -[s(:,j)'-s(:,1)',r((k-1)*(M-1)+j-1)];
    end
    h1 = [h1;h1_k];
    G1 = blkdiag(G1,G1_k);
end;
W1 = inv(Q_alpha);
phi1 = inv(G1'*W1*G1)*G1'*W1*h1;
u = reshape(phi1,N+1,K); 
u = u(1:N,:);

for iteration = 1 : 1            % Iterate to update W1.   
    B1 = [];
    D1 = [];
    for k = 1 : K
        for j = 2 : M
            b(j-1,1) = norm(u(:,k)-s(:,j),2);
            D1_k(j-1,:)= [-(u(:,k)-s(:,1))'-r((k-1)*(M-1)+j-1)*(u(:,k)-s(:,1))'/norm(u(:,k)-s(:,1),2),...
                zeros(1,(j-2)*N),(u(:,k)-s(:,j))',zeros(1,(M-j)*N)];
        end;
        B1 = blkdiag(B1,diag(b));
        D1 = [D1;D1_k];
    end
    W1 = inv(B1*Q_alpha*B1'+D1*Q_beta*D1');
    phi1 = inv(G1'*W1*G1)*G1'*W1*h1;
    u = reshape(phi1,N+1,K); 
    u = u(1:N,:);
end;
cov_phi1 = inv(G1'*W1*G1);

% Stage-2 processing.
h2 = (phi1-repmat([s(:,1);0],K,1)).^2;
P = [eye(N); ones(1,N)];
G2 = [];
B2 = [];
for k = 1 : K
    G2 = blkdiag(G2,P);
    B2 = blkdiag(B2,diag([u(:,k)-s(:,1); norm(u(:,k)-s(:,1))]));
end

W2 = inv(B2*cov_phi1*B2');
phi2 = inv(G2'*W2*G2)*G2'*W2*h2;
phi2 = reshape(phi2,N,K);
% Return the source location estimate.
for k = 1 : K
    u(:,k) = diag(sign(u(:,k)-s(:,1)))*sqrt(phi2(:,k))+ s(:,1);
end
u=reshape(u,[],1);

