function [x] = TmDpLocIgrObjSsrMtnEft_GN(uInit,vInit,t,vs,m,Q0,fc,c,err)
% [x] = TmDpLocIgrObjSsrMtnEft_GN(uInit,vInit,t,vs,m,Q0,fc,c,err)
%
% MLE for Time Delay only or with Doppler shift localization by Gauss-Newton
% iterations.
%
% Input:
% uInit:    initial guess of object position.
% vInit:    initial guess of object velocity.
% t:        (Dim x M), sensor positions when emitting signals.
% vs:       (Dim x M), sensor velocities.
% m:        (M x 1) or (2M x 1), measurements, M = num. of sensors.
% Q0:       (2M x 2M), covariance matrix structure of measurement noise.
% fc:       (M x 1), sensor carrier frequencies.
% c:        signal propagation speed.
% err:      residual error to terminate iterations.
%
% Output:
% x:        (2Dim x 1), x=[u' v']', object position estimate.
%
% 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
MAXi=200;           % maximum number of iterations

k=0;                % iteration count
flag=0;

%% only time measurement is available
if length(Q0)==M
    xk=[uInit];
    mk=zeros(M,1);   % time measurements in the k-th step
    
    while flag==0 & k<=MAXi
        % Reconstruct measurements in the k-th step
        ukmt=xk(1:K)-t;    % uk-t
        L=zeros(1,M);      % L(1,i)=norm(uki-ti) and L(2,i)=norm(uki-si)
        for i=1:M
            L(1,i)=norm(ukmt(:,i));    % L(1,i)=norm(uki-ti)
            mk(i)=(2*L(1,i))/c;
        end
        %% Compute the parameter
        Gk=zeros(K,M);
        for i=1:M
            Gk(:,i)=2*ukmt(:,i)/L(i)/c; %dt/dki
        end
        
        %% Gauss-Newton iteration
        xk1=xk+(Gk*inv(Q0)*Gk')\Gk*inv(Q0)*(m-mk);
        if norm(xk1-xk)<=err
            flag=1;
            xk=xk1;
        else
            if k==MAXi
                err=norm(xk1-xk);  % display the error when the method cannot converge
            end
            xk=xk1;
            k=k+1;
        end
    end
    
else
    
    %% hybrid time and Doppler localization
    xk=[uInit;vInit];
    mk=zeros(2*M,1);   % measurements in the k-th step
    
    while flag==0 & k<=MAXi
        % Reconstruct measurements in the k-th step
        ukmt=xk(1:K)-t;    % uk-t
        L=zeros(1,M);      % L(1,i)=norm(uki-ti) and L(2,i)=norm(uki-si)
        for i=1:M
            L(1,i)=norm(ukmt(:,i));    % L(1,i)=norm(uki-ti)
            mk(i)=(2*L(1,i))/c;
            mk(i+M)=2*ukmt(:,i)'/L(1,i)*(xk(K+1:2*K)-vs(:,i))*fc(i)/c;
        end
        %% Compute the parameter
        Gk=zeros(2*K,2*M);
        for i=1:M
            Gk(1:K,i)=2*ukmt(:,i)/L(i)/c; %dt/duki
            Gk(K+1:2*K,i)=zeros(K,1);   %dt/dvki
            Gk(1:K,i+M)=(2*(xk(K+1:2*K)-vs(:,i))/L(i)-2*ukmt(:,i)'*(xk(K+1:2*K)-vs(:,i))*ukmt(:,i)/L(i)^3)*fc(i)/c;
            Gk(K+1:2*K,i+M)=(2*ukmt(:,i)/L(i))*fc(i)/c;
        end
        %% Gauss-Newton iteration
        xk1=xk+(Gk*inv(Q0)*Gk')\Gk*inv(Q0)*(m-mk);
        if norm(xk1-xk)<=err
            flag=1;
            xk=xk1;
        else
            if k==MAXi
                err=norm(xk1-xk);  % display the error when the method cannot converge
            end
            xk=xk1;
            k=k+1;
        end
    end
end

x=xk1;

end

