function [u,u_dot] = TDOAFDOALocMvgSenPosErr(s,s_dot,r,r_dot,Q_beta,Q_alpha)
%
% This program realizes the algorithm for localizing a moving source 
% using TDOAs and FDOAs in the presence of sensor position errors. The 
% sensors are moving as well. The details of the algorithm development can 
% be found in K. C. Ho, Xiaoning Lu, L. Kovavisaruch, "Source Localization 
% Using TDOA and FDOA Measurements in the Presence of Receiver Location 
% Errors: Analysis and Solution," IEEE Trans. on Signal Processing, 
% Vol. 55, pp.684-696, 2007.
%
% Usage: [u,u_dot] = TDOAFDOALocMvgSenPosErr(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_21,r_31,...r_M1]': (M-1)x1 TDOA (range difference) measurement vector.
% r_dot = cx[f_21,f_31,...f_M1]': (M-1)x1 FDOA measurement vector.
%         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: 2(M-1)x2(M-1) covariance matrix of [r;r_dot].
%
% The program returns a 2x1 or 3x1 source location estimate u and a 2x1 or 
% 3x1 source velocity estimate u_dot.
%
% Note: W1 is updated once (RptCnt=1) in Stage-1, updating W1 more than
% once (e.g. RptCnt=2 or RptCnt=3) may provide better results for near source.
%
% 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 = 1;     % number of repetitions in Stage-1 to recompute W1

Qb = Q_beta; Qa = Q_alpha; 
M = size(s,2);           % Number of sensors.
N = size(s,1);           % Dimension of the localization problem.

% Stage-1 processing.
for j = 2 : M
    ht(j-1,1) = r(j-1)^2 - s(:,j)'*s(:,j) + s(:,1)'*s(:,1);
    hf(j-1,1) = 2*(r(j-1)*r_dot(j-1)-s_dot(:,j)'*s(:,j)+s_dot(:,1)'*s(:,1));

    Gt(j-1,:) = -2 * [(s(:,j)-s(:,1))',r(j-1),zeros(1,N+1)];
    Gf(j-1,:) = -2 * [(s_dot(:,j)-s_dot(:,1))',r_dot(j-1),(s(:,j)-s(:,1))',r(j-1)];
end;

h1 = [ht;hf];
G1 = [Gt;Gf];
W1 = inv(Qa);
phi1 = inv(G1'*W1*G1)*G1'*W1*h1;

u = phi1(1:N);
u_dot = phi1(N+2:end-1);

for m = 1 : max(1,RptCnt)           % Iterate to update W1.
    for j = 2 : M
        b(j-1,1) = norm(u-s(:,j),2);
        b_dot(j-1,1)= (u-s(:,j))'*(u_dot-s_dot(:,j))/norm(u-s(:,j));
        
        D(j-1,:) = 2*[-(u-s(:,1))',zeros(1,(j-2)*N),...
                       (u-s(:,j))',zeros(1,(M-j)*N)];
        D_dot(j-1,:)= 2*[-(u_dot-s_dot(:,1))',zeros(1,(j-2)*N),...
                          (u_dot-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];
    W1 = inv(B1*Qa*B1'+D1*Qb*D1');

    phi1 = inv(G1'*W1*G1)*G1'*W1*h1;
    u = phi1(1:N);
    u_dot = phi1(N+2:end-1);
end;
cov_phi1 = inv(G1'*W1*G1);

% Stage-2 processing.
h2 = [(u-s(:,1)).^2;                  phi1(N+1)^2;...
      (u-s(:,1)).*(u_dot-s_dot(:,1)); phi1(N+1)*phi1(end)];
G2 = [eye(N),    zeros(N);...
      ones(1,N), zeros(1,N);...
      zeros(N),  eye(N);...
      zeros(1,N),ones(1,N)];

Bt =  diag([u-s(:,1); norm(u-s(:,1))]);
Btd = diag([u_dot-s_dot(:,1); (u-s(:,1))'*(u_dot-s_dot(:,1))/norm(u-s(:,1))]);
B2 = [2*Bt,zeros(N+1); Btd, Bt];

D2 = [zeros(N,2*N*M);...
      2*(u-s(:,1))',zeros(1,N*(2*M-1));...
      zeros(N,2*N*M);...
      (u_dot-s_dot(:,1))',zeros(1,N*(M-1)),(u-s(:,1))',zeros(1,N*(M-1))];
W2 = inv(B2*cov_phi1*B2' + D2*Qb*D2' + B2*cov_phi1*G1'*W1*D1*Qb*D2' + ...
                                   D2*Qb*D1'*W1*G1*cov_phi1*B2');
phi2 = inv(G2'*W2*G2)*G2'*W2*h2;

u = diag(sign(u-s(:,1)))*sqrt(abs(phi2(1:N))) + s(:,1);
u_dot = phi2(N+1:end)./(u-s(:,1)) + s_dot(:,1);