function [u,v,k] = TmDpLocObjSsrMtnEft_GN(uInit,vInit,t,vs,m,Q0,fc,c,err)
% [u,v,k] = TmDpLocObjSsrMtnEft_GN(uInit,vInit,t,vs,m,Q0,fc,c,err)
%
% MLE for Time Delay and Doppler shift 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.
% vs:       (Dim x M), sensor velocities.
% m:        (2M x 1), time delay and Doppler 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:
% 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
MAXi=200;           % maximum number of iterations

k=0;
uk=uInit;
vk=vInit;
setak=[uk;vk];
mk=zeros(2*M,1);    % constructed measurements in the k-th step
flag=0;

while flag==0 & k<=MAXi
    
    %% Construct measurements in the k-th step
    rk1=zeros(1,M);     % r1 is the first part of r
    ukmt=uk-t;          % uk-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);
    G=zeros(2*K,2*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);
        
        %% range measurement
        mk(i)=B/A/c;
        s(:,i)=t(:,i)+mk(i)*vs(:,i);
        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;
        
        G(1:K,i)=1/(1+Ci(i))*((1+Ai(i))/(1-Bi(i))*pukimt(:,i)+pukims(:,i))/c;   % dt/du
        G(K+1:2*K,i)=rk1(i)/c*G(1:K,i);                                         % dt/dv
        
        %% Doppler measurement
        mk(i+M)=1/(1+Ci(i))*((1+Ai(i))/(1-Bi(i))*pukimt(:,i)'+pukims(:,i)')*(vk-vs(:,i))*fc(i)/c;
        
        dr1Tdu=1/(1-Bi(i))*pukimt(:,i);
        dAidu=((eye(K)+dr1Tdu*vk'/c-G(1:K,i)*vs(:,i)')*vk/L(4,i)-ukims(:,i)'*vk*(eye(K)+dr1Tdu*vk'/c-G(1:K,i)*vs(:,i)')*ukims(:,i)/L(4,i)^3)/c;
        dBidu=((eye(K)+dr1Tdu*vk'/c)*vk/L(3,i)-ukimt(:,i)'*vk*(eye(K)+dr1Tdu*vk'/c)*ukimt(:,i)/L(3,i)^3)/c;
        dCidu=((eye(K)+dr1Tdu*vk'/c-G(1:K,i)*vs(:,i)')*vs(:,i)/L(4,i)-ukims(:,i)'*vs(:,i)*(eye(K)+dr1Tdu*vk'/c-G(1:K,i)*vs(:,i)')*ukims(:,i)/L(4,i)^3)/c;
        dKidu=((eye(K)+dr1Tdu*vk'/c)*vs(:,i)/L(3,i)-ukimt(:,i)'*vs(:,i)*(eye(K)+dr1Tdu*vk'/c)*ukimt(:,i)/L(3,i)^3)/c;
        
        dAidv=(pukims(:,i)+dAidu*rk1(i))/c;
        dBidv=(pukimt(:,i)+dBidu*rk1(i))/c;
        dCidv=rk1(i)/c*dCidu;
        dKidv=rk1(i)/c*dKidu;
        
        G(1:K,M+i)=(fc(i)/((1-Bi(i))^2*(1+Ci(i))^2)*( ((Bi(i)-Di(i))*dAidu+(1+Ai(i))*(dBidu-dKidu))*(1-Bi(i))*(1+Ci(i)) - ...
            ( -dBidu*(1+Ci(i))+ dCidu*(1-Bi(i)) )*(1+Ai(i))*(Bi(i)-Di(i)) ) +  fc(i)/((1+Ci(i))^2) * ( (dAidu-dCidu)*(1+Ci(i))-dCidu*(Ai(i)-Ci(i)) ) );  % dfT / du
        G(K+1:2*K,M+i)=(fc(i)/((1-Bi(i))^2*(1+Ci(i))^2)*( ((Bi(i)-Di(i))*dAidv+(1+Ai(i))*(dBidv-dKidv))*(1-Bi(i))*(1+Ci(i)) - ...
            ( -dBidv*(1+Ci(i))+ dCidv*(1-Bi(i)) )*(1+Ai(i))*(Bi(i)-Di(i)) ) +  fc(i)/((1+Ci(i))^2) * ( (dAidv-dCidv)*(1+Ci(i))-dCidv*(Ai(i)-Ci(i)) ) );  % dfT / dv
    end
    
    %% Gauss-Newton iteration
    setak1=setak+(G*inv(Q0)*G')\G*inv(Q0)*(m-mk);
    if norm(setak1-setak)<=err | (isnan(setak1))
        flag=1;
        u=setak1(1:K);
        v=setak1(K+1:2*K);
    else
        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

