function [u,v,k] = TmLocObjSsrMtnEft_GN(uInit,vInit,t,vs,m,Qt0,c,err)
% [u,v,k] = TmLocObjSsrMtnEft_GN(uInit,vInit,t,vs,m,Qt0,c,err)
%
% MLE for Time Delay localization by Gauss-Newton iterations.
%
% Input:
% uInit:    (Dim x 1), initial guess of object position.
% vInit:    (Dim x 1), initial guess of object velocity.
% t:        (Dim x M), sensor positions when emitting signals, M = num. of sensors.
% vs:       (Dim x M), sensor velocities.
% m:        (M x 1), time delay measurements.
% Qt0:      (M x M), covariance matrix structure of measurement noise.
% c:        signal propagation speed.
% err:      residual error to terminate iterations.
%
% Output:
% u:        (Dim x 1), object position estimate.
% v:        (Dim x 1), object velocity estimate.
% k:        number of iterations taken.
%
% The program can be used for 2-D(Dim=2) or 3-D(Dim=3) localization directly.
%
% Reference:
% T. Jia, K. C. Ho, H. Wang and X. Shen, "Localization of a moving object
% with sensors in motion by time delays and Doppler shifts," IEEE Trans.
% Signal Process., vol. 68, pp. 5824-5841, 2020.
%
% Tianyi Jia and K. C. Ho   11-08-2020
%
%       Copyright (C) 2020
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA
%       hod@missouri.edu
%

warning off;

[K,M]=size(t);          % K=localization dimension, M=number of sensors
MAXk=200;               % maximum number of iterations

k=0;                    % iteration count
uk=uInit;
vk=vInit;
setak=[uk;vk];
rk=zeros(M,1);   % time measurements in the k-th step
flag=0;

while flag==0 & k<=MAXk
    
    %% Construct measurements in the k-th step
    rk1=zeros(1,M);   % r1 is the first part of r
    ukmt=uk-t;         % uki-t
    L=zeros(4,M);      % L(1,i)=norm(uki-ti) and L(2,i)=norm(uki-si)  L(3,i)=norm(uki-ti) and L(4,i)=norm(uki-si)
    s=zeros(K,M);      % location of sensor with signal being received
    ukimt=zeros(K,M);
    ukims=zeros(K,M);
    uki=zeros(K,M);
    pukimt=zeros(K,M);
    pukims=zeros(K,M);
    Ai=zeros(1,M);
    Bi=zeros(1,M);
    Ci=zeros(1,M);
    Di=zeros(1,M);
    dtTduv=zeros(2*K,M);
    for i=1:M
        L(1,i)=norm(ukmt(:,i));    %  L(1,i)=norm(uki-ti)
        A=1-vk'*vk/c^2;
        B=-2*ukmt(:,i)'*vk/c;
        C=-L(1,i)^2;
        rk1(i)=(-B+sqrt(B^2-4*A*C))/(2*A);
        uki(:,i)=uk+rk1(i)/c*vk;
        
        ukimt(:,i)=uki(:,i)-t(:,i);
        L(3,i)=norm(ukimt(:,i));   % L(3,i)=norm(uki-ti)
        A=1-vs(:,i)'*vs(:,i)/c^2;
        B=2*(L(3,i)-vs(:,i)'*ukimt(:,i)/c);
        
        rk(i)=B/A;
        s(:,i)=t(:,i)+rk(i)*vs(:,i)/c;
        ukims(:,i)=uki(:,i)-s(:,i);      %uki-s
        L(4,i)=norm(ukims(:,i));   % L(4,i)=norm(uki-si)
        
        pukimt(:,i)=ukimt(:,i)/L(3,i);
        pukims(:,i)=ukims(:,i)/L(4,i);
        
        Ai(i)=pukims(:,i)'*vk/c;
        Bi(i)=pukimt(:,i)'*vk/c;
        Ci(i)=pukims(:,i)'*vs(:,i)/c;
        Di(i)=pukimt(:,i)'*vs(:,i)/c;
        
        dtTduv(1:K,i)=1/(1+Ci(i))*((1+Ai(i))/(1-Bi(i))*pukimt(:,i)+pukims(:,i))/c;
        dtTduv(K+1:2*K,i)=rk1(i)/c*dtTduv(1:K,i);
    end
    
    %% Gauss-Newton iteration
    setak1=setak+(dtTduv*inv(Qt0)*dtTduv')\dtTduv*inv(Qt0)*(m-rk/c);
    if norm(setak1-setak)<=err | (isnan(setak1))
        % k
        flag=1;
        u=setak1(1:K);
        v=setak1(K+1:2*K);
    else
        if k==MAXk
            norm(setak1-setak);  % display the error when the method cannot converge
        end
        setak=setak1;
        uk=setak(1:K);
        vk=setak(K+1:2*K);
        k=k+1;
    end
    
end

u=setak1(1:K);
v=setak1(K+1:2*K);

end

