function [u] = TmDpLocIgSenMtnEft_GN(m,t,v,Q,fc,c,u0,Tol)
% [u] = TmDpLocIgSenMtnEft_GN(m,t,v,Q,fc,c,u0,Tol)
%
% MLE solution by Gauss-Newton iteration for time delay and Doppler shift 
% localization when ignoring sensor motion.
%
% Input parameter list:
% m:    (2N x 1), time delay and Doppler shift measurements
% t:    (Dim x N), sensor positions when signal sending out, N = sensor num.
% v:    (Dim x N), sensor velocities.
% Q:    (2N x 2N), covariance matrix of measurements
% fc:   (N x 1), carrier frequencies
% c:    scalar, signal propagation speed
% u0:   (Dim x 1), initial guess of object position
% Tol:  scalar, tolerance level (small value) to stop iteration
%
% Output parameter list:
% u:    (Dim x 1), object location estimate
%
% 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
%

if exist('Tol')==0
    Tol=1e-6;       % tolerance level default value
end

lamda=1;            % step size in Gauss-Newton method 
[K,N]=size(t);      % number of sensors
k=0;                % iteration count

uk=u0;              % initialization
mk=zeros(2*N,1);    % time measurements in the k-th step
flag=0;             % solution reached flag
MAXi=500;           % maximum number of iterations
iQ=inv(Q);          % inverse of measurement covariance matrix

while (flag==0 & k<=MAXi)   % iterate until convergence
    
    %% Create the time measuremet in k-th step
    ukmt=repmat(uk,1,N)-t;         % uk-t
    L=zeros(2,N);      % L(1,i)=norm(uk-ti) and L(2,i)=norm(uk-si)
    s=zeros(K,N);      % sensor locations when signal being received
    ukms=zeros(K,N);
    
    for i=1:N
        L(1,i)=norm(ukmt(:,i));
        mk(i,1)=(2*L(1,i))/c;
        mk(N+i,1)=-2*fc(i)/c*(ukmt(:,i)'*v(:,i)/L(1,i));                              % inaccurate model 2
    end
    
    %% Compute the parameter
    G=zeros(K,2*N);
    for i=1:N
        G(:,i)=2/c*(ukmt(:,i)/L(1,i));               % dtao/du
        G(:,N+i)=-2*fc(i)/c*(v(:,i)/L(1,i)-(ukmt(:,i)'*v(:,i))/L(1,i)^3*ukmt(:,i));      % df/du in inaccurate model 2 
    end
    
    %% Gauss-Newton iteration
    uk1=uk+lamda*inv(G*iQ*G')*G*iQ*(m-mk);
    if norm(uk1-uk)<=Tol
        flag=1;
        uk=uk1;
    else
        uk=uk1;
        k=k+1;
    end
end

u=uk1;  % solution

end

