function [u,u0] = TmLocSenMtnEft(m,t,v,Q,c)
% [uwls,u0] = TmLocSenMtnEft(m,t,v,Q,c)
%
% Algebraic closed-form solution for time delay localization with sensor
% motion taken into consideration.
%
% input parameter list:  
% m:    (N x 1), time measurement vector, N = num. of measurements.
% t:    (Dim x N), sensor positions when signal sending out, N = sensor num.
% v:    (Dim x N), sensor velocities.
% Q:    (N x N), covariance matrix of time delay measurements.
% c:    signal propagation speed.
%
% Output parameter list:
% u:    (Dim x 1), object location estimate.
% u0:   (Dim x 1), coarse object location estimate from first stage.
%
% The program can be used for 2D(Dim=2) or 3D(Dim=3) localization directly.
%
% Reference:
% T. Jia, K. C. Ho, H. Wang, and X. Shen, "Effect of sensor motion on time 
% delay and Doppler shift localization: analysis and solution," IEEE Trans. 
% Signal Process., vol. 67, no. 22, pp. 5881-5895, Nov. 2019.
%
% Tianyi Jia and K. C. Ho   12-26-2019
%
%       Copyright (C) 2019
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA
%       hod@missouri.edu
%

[K,N]=size(t);
if (K==2) Lgth=5; else Lgth=9; end;
    
%% stage1: WLS localization with auxiliary variables
a=zeros(N,1);   % define as a*m*c-b
b=zeros(N,1);   % define as a*m*c-b
p=zeros(N,1);
h1=zeros(N,1);
G1=zeros(N,Lgth);
B1=zeros(N,N);
W1=zeros(N,N);

for i=1:N,
    a(i)=(1-v(:,i)'*v(:,i)/c^2)/2;
    b(i)=v(:,i)'*t(:,i)/c;
    p(i)=a(i)*m(i)*c-b(i);
    h1(i)=(p(i))^2-t(:,i)'*t(:,i);
        
    if (K==2)   % 2D
        G1(i,1:2)=-2*(t(:,i)+p(i)/c*v(:,i))';
        G1(i,3:5)=[1 1 0]-[v(1,i)^2 v(2,i)^2 2*v(1,i)*v(2,i)]/c^2;
    else        % 3D
        G1(i,1:3)=-2*(t(:,i)+p(i)/c*v(:,i))';
        G1(i,4:6)=[1 1 1]-(v(:,i)').^2/c^2;
        G1(i,7:9)=-2*[v(1,i)*v(2,i) v(1,i)*v(3,i) v(2,i)*v(3,i)]/c^2;
    end;
    
    B1(i,i)=c^2*m(i)/2; %B(i,i) can be approximated by a constant;
end

W1=inv(B1*Q*B1');
x0=(G1'*W1*G1)\G1'*W1*h1; 
u0=x0(1:K);     % initial solution to obtain better weighting matrix

for i=1:N
    B1(i,i)=2*p(i)*a(i)*c+2*a(i)*v(:,i)'*u0; %B(i,i)=-c^2*m(i);
end

W1=inv(B1*Q*B1');
cov1=G1'*W1*G1;

x1=(cov1)\G1'*W1*h1;
u1=x1(1:K);     % solution from stage1

%% stage2: refine the estimate
if (K==2)   % 2D
    h2=zeros(5,1);
    G2=zeros(5,2);
    B2=zeros(5,5);
    W2=zeros(5,5);
    
    h2(1:2)=x1(1:2).^2;
    h2(3:4)=x1(3:4);
    h2(5)=(x1(1)+x1(2))^2-2*x1(5); %h2(5)=x1(5)^2*(1/x1(3)+1/x1(4));  % h2(5)=x1(5)^2*(1/x1(1)^2+1/x1(2)^2);
    G2(1:2,1:2)=eye(2);
    G2(3:4,1:2)=eye(2);
    G2(5,1:2)=[1 1];
    B2(1:2,1:2)=2*diag(x1(1:2));
    B2(3:4,3:4)=eye(2);
    B2(5,1:2)=2*(x1(1)+x1(2))*[1 1]; 
    B2(5,5)=-2;       
else    % 3D  
    h2=zeros(9,1);
    G2=zeros(9,3);
    B2=zeros(9,9);
    W2=zeros(9,9);
    
    h2(1:3)=x1(1:3).^2;
    h2(4:6)=x1(4:6);
    h2(7)=(x1(1)+x1(2))^2-2*x1(7);
    h2(8)=(x1(1)+x1(3))^2-2*x1(8);
    h2(9)=(x1(2)+x1(3))^2-2*x1(9);
    G2(1:3,:)=eye(3);
    G2(4:6,:)=eye(3);
    G2(7:9,:)=[1 1 0;1 0 1;0 1 1];
    B2(1:3,1:3)=2*diag(x1(1:3));
    B2(4:6,4:6)=eye(3);
    B2(7:9,7:9)=-2*eye(3);
    B2(7:9,1:3)=diag([x1(1)+x1(2) x1(1)+x1(3) x1(2)+x1(3)])*[1 1 0;1 0 1;0 1 1]*2;
end;
    
W2=inv(B2')*(cov1)*inv(B2); 
x2=(G2'*W2*G2)\G2'*W2*h2;

u=sign(x1(1:K)).*sqrt(x2(1:K));     % final solution
