function [u,u_dot] = TDOAFDOALocMvgSenPosErrImp(s,s_dot,r,r_dot,Q_beta,Q_alpha)
%
% This program realizes the algorithm for localizing multiple disjoint moving  
% sources using TDOAs and FDOAs in the presence of sensor location errors.  
% The sensors are moving 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,u_dot] = TDOAFDOALocMvgSenPosErrImp(s,s_dot,r,r_dot,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.
% s_dot: 2xM or 3xM sensor velocity matrix.
%        s_dot(:,i) is the known velocity of the ith sensor.
% r = [r_1,r_2,...,r_K]: (M-1)xK TDOA (range difference) measurement vector.
%     K: number of sources
%     r_i = [r_{21,i},r_{31,i},...r_{M1,}]'
% r_dot = [r_dot_1,r_dot_2,...,r_dot_K]: (M-1)xK FDOA (range rate difference) measurement vector.
%         r_dot_i = c x [f_{21,i},f_{31,i},...f_{M1,i}]'
%         c is the signal propagation speed.
% Q_beta: 4Mx4M, or 6Mx6M covariance matrix of the sensor position and velocity
%                       reshape([s,s_dot],4M,1) or reshape([s,s_dot],6M,1).
% Q_alpha: 2K(M-1)x2K(M-1) covariance matrix of [r_1;r_dot_1;...;r_K;r_dot_K].
%
% The program returns a 2xK or 3xK source position estimate u and a 2xK or 
% 3xK source velocity estimate u_dot.
%
% The program can be used for 2D(Dim=2) or 3D(Dim=3) localization.
%
%
% Ming Sun and K. C. Ho      03-05-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.
h1c = []; G1c = [];
for k = 1 : K
    for j = 2 : M
        ht(j-1,1) = r((k-1)*(M-1)+j-1)^2 - s(:,j)'*s(:,j) + s(:,1)'*s(:,1);
        hf(j-1,1) = 2*(r((k-1)*(M-1)+j-1)*r_dot((k-1)*(M-1)+j-1)-s_dot(:,j)'*s(:,j)+s_dot(:,1)'*s(:,1));
        
        Gt(j-1,:) = -2 * [(s(:,j)-s(:,1))',r((k-1)*(M-1)+j-1),zeros(1,N+1)];
        Gf(j-1,:) = -2 * [(s_dot(:,j)-s_dot(:,1))',r_dot((k-1)*(M-1)+j-1),(s(:,j)-s(:,1))',r((k-1)*(M-1)+j-1)];
    end;
    h1 = [ht;hf];
    G1 = [Gt;Gf];
    
    h1c = [h1c;h1];
    G1c = blkdiag(G1c,G1);
end
W1 = inv(Q_alpha);
phi1 = inv(G1c'*W1*G1c)*G1c'*W1*h1c;
phi1 = reshape(phi1,2*(N+1),K);

for k = 1 : K
   u(N*(k-1)+1:N*k) = phi1(1:N,k);
   u_dot(N*(k-1)+1:N*k) = phi1(N+2:end-1,k);
end
u=u';
u_dot=u_dot';

for m = 1 : 1           % Iterate to update W1.
    B1c = []; D1c = [];
    for k = 1 : K
        for j = 2 : M
            b(j-1,1) = norm(u(N*(k-1)+1:N*k)-s(:,j),2);
            b_dot(j-1,1)= (u(N*(k-1)+1:N*k)-s(:,j))'*(u_dot(N*(k-1)+1:N*k)-s_dot(:,j))/norm(u(N*(k-1)+1:N*k)-s(:,j));
            
            D(j-1,:) = 2*[-(u(N*(k-1)+1:N*k)-s(:,1))'-(r((k-1)*(M-1)+j-1)/norm(u(N*(k-1)+1:N*k)-s(:,1)))*(u(N*(k-1)+1:N*k)-s(:,1))',zeros(1,(j-2)*N),...
                (u(N*(k-1)+1:N*k)-s(:,j))',zeros(1,(M-j)*N)];
            tmp = -(u_dot(N*(k-1)+1:N*k)-s_dot(:,1))'-r((k-1)*(M-1)+j-1)*...
                (1/norm(u(N*(k-1)+1:N*k)-s(:,1))*(u_dot(N*(k-1)+1:N*k)-s_dot(:,1))'...
                -(u(N*(k-1)+1:N*k)-s(:,1))'*(u_dot(N*(k-1)+1:N*k)-s_dot(:,1))/norm(u(N*(k-1)+1:N*k)-s(:,1))^3*(u(N*(k-1)+1:N*k)-s(:,1))')...
                -r_dot((k-1)*(M-1)+j-1)/norm(u(N*(k-1)+1:N*k)-s(:,1))*(u(N*(k-1)+1:N*k)-s(:,1))';           
            D_dot(j-1,:)= 2*[tmp,zeros(1,(j-2)*N),...
                (u_dot(N*(k-1)+1:N*k)-s_dot(:,j))',zeros(1,(M-j)*N)];
        end;
        B = 2 * diag(b);
        B_dot= 2 * diag(b_dot);
        
        B1 = [B,zeros(size(B));B_dot,B];
        D1 = [D,zeros(size(D));D_dot,D];
        
        B1c = blkdiag(B1c,B1);
        D1c = [D1c;D1];
    end
    
    W1 = inv(B1c*Q_alpha*B1c'+D1c*Q_beta*D1c');

    phi1 = inv(G1c'*W1*G1c)*G1c'*W1*h1c;
    phi1 = reshape(phi1,2*(N+1),K);
    for k = 1 : K
       u(N*(k-1)+1:N*k) = phi1(1:N,k);
       u_dot(N*(k-1)+1:N*k) = phi1(N+2:end-1,k);
    end
end;
cov_phi1 = inv(G1c'*W1*G1c);

% Stage-2 processing.
h2c = []; G2c = [];
B2c = [];
for k = 1 : K
    h2 = [(u(N*(k-1)+1:N*k)-s(:,1)).^2;                  phi1(N+1,k)^2;...
        (u(N*(k-1)+1:N*k)-s(:,1)).*(u_dot(N*(k-1)+1:N*k)-s_dot(:,1)); phi1(N+1,k)*phi1(end,k)];
    G2 = [eye(N),    zeros(N);...
        ones(1,N), zeros(1,N);...
        zeros(N),  eye(N);...
        zeros(1,N),ones(1,N)];
    
    Bt =  diag([u(N*(k-1)+1:N*k)-s(:,1); norm(u(N*(k-1)+1:N*k)-s(:,1))]);
    Btd = diag([u_dot(N*(k-1)+1:N*k)-s_dot(:,1); (u(N*(k-1)+1:N*k)-s(:,1))'*(u_dot(N*(k-1)+1:N*k)-s_dot(:,1))/norm(u(N*(k-1)+1:N*k)-s(:,1))]);
    B2 = [2*Bt,zeros(N+1); Btd, Bt];
    
    h2c = [h2c;h2];
    G2c = blkdiag(G2c,G2);
    B2c = blkdiag(B2c,B2);
end

W2 = inv(B2c*cov_phi1*B2c');
phi2 = inv(G2c'*W2*G2c)*G2c'*W2*h2c;
phi2 = reshape(phi2,2*N,K);

% Return the source position and velocity estimates.
for k = 1 : K
   u(N*(k-1)+1:N*k) = diag(sign(u(N*(k-1)+1:N*k)-s(:,1)))*sqrt(abs(phi2(1:N,k))) + s(:,1);
   u_dot(N*(k-1)+1:N*k) = phi2(N+1:end,k)./(u(N*(k-1)+1:N*k)-s(:,1)) + s_dot(:,1);
end



