function [u] = TmLocIgSenMtnEft_GN(tao,t,Qt,c,u0,Tol)
% [u] = TmLocIgSenMtnEft_GN(tao,t,Qt,c,u0,Tol)
%
% MLE solution by Gauss-Newton iteration for time delay localization 
% when ignoring sensor motion.
%
% Input parameter list:
% tao:  (N x 1), time delay measurements
% t:    (Dim x N), sensor positions when signal sending out, N = sensor num.
% Qt:   (N x N), covariance matrix of time delay measurements
% 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
v=zeros(K,N);       % ignore the sensor motion for this algorithm: v=0
k=0;                % iteration count

uk=u0;              % initialization
taok=zeros(N,1);    % time measurements in the k-th step
flag=0;             % solution reached flag
MAXi=500;           % maximum number of iterations
iQt=inv(Qt);        % 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));
        taok(i)=(2*L(1,i))/c;
        s(:,i)=t(:,i);
        ukms(:,i)=ukmt(:,i);      %uk-s
        L(2,i)=L(1,i);
    end
    
    %% Compute the parameter
    dtdu=zeros(K,N);
    for i=1:N
        dtdu(:,i)=1/(c+v(:,i)'*ukms(:,i)/L(2,i))*(ukmt(:,i)/L(1,i)+ukms(:,i)/L(2,i));    
    end
    
    %% Gauss-Newton iteration
    uk1=uk+lamda*inv(dtdu*iQt*dtdu')*dtdu*iQt*(tao-taok);
    if norm(uk1-uk)<=Tol
        flag=1;
        uk=uk1;
    else
        uk=uk1;
        k=k+1;
    end
end

u=uk1;  % solution

end

