function [theta,cost] = XcvrTOALoc_GN_MLE(d,s,K,Q,OfstUnkn,theta0);
% function [theta,cost] = XcvrTOALoc_GN_MLE(d,s,K,Q,OfstUnkn,theta0)
%
% MLE using Gauss-Newton iterative implementation
%
% Input parameter list:
% d:  (KMN x 1), distance (time delay) measurement vector
%      M = num. of transceivers, N = num. of sensors
% s:  (Dim x N), sensor positions
% K:  K = num. of time instants
% Q:  (KMN x KMN), covariance matrix of distance measurements
% OfstUnkn:  flag, 1 if time offsets known and 0 if offsets unknown
% theta0:    initial solution guess
%
% Output parameter list:
% theta:    solution
% cost:     minimum optimization cost value
%
% The code can work with 2-D(Dim=2) or 3-D(Dim=3) localization.
%
% Reference:
% K. C. Ho, "Localization through transceivers in unknown constant velocity
% trajectories," IEEE Trans. Signal Process., vol. 70, pp. 3011-3028, 2022.
%
% K. C. Ho     08-28-2022
%
%       Copyright (C) 2022
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA
%       hod@missouri.edu
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

theta=theta0*1e3; cost=1e9;
theta0org=theta0;

[L,N]=size(s);
M=(length(theta0)-L)/(2*L+1*OfstUnkn);

tiParaLgth=2*L;

k=0:K-1;

MxItn=100;
W=inv(Q);
delta=zeros(M,1);
for itn=1:MxItn,
    %     fprintf('%d ',itn);
    dHat=zeros(K,M,N);
    u=theta0(1:L);
    for i=1:M,
        ti=repmat(theta0(L+(i-1)*tiParaLgth+1:L+(i-1)*tiParaLgth+L),1,K)+theta0(L+(i-1)*tiParaLgth+L+1:L+(i-1)*tiParaLgth+2*L)*k;
        liu(:,i)=sqrt(sum((ti-repmat(u,1,K)).^2));
        rhoiu=-(ti-repmat(u,1,K))./(repmat(liu(:,i)',L,1)+eps);
        if (OfstUnkn)
            delta(i)=theta0(end-M+i);
        else
            delta(i)=0;
        end;
        
        Gi=[];
        for j=1:N,
            lij(:,i,j)=sqrt(sum((ti-repmat(s(:,j),1,K)).^2));
            rhoij=(ti-repmat(s(:,j),1,K))./(repmat(lij(:,i,j)',L,1)+eps);
            if (OfstUnkn)
                Gij=[rhoiu; (-rhoiu+rhoij); repmat(k,L,1).*(-rhoiu+rhoij); ones(1,length(rhoiu))]';
            else
                Gij=[rhoiu; (-rhoiu+rhoij); repmat(k,L,1).*(-rhoiu+rhoij)]';
            end;
            Gi=[Gi; Gij];
            dHat(:,i,j)=liu(:,i)+lij(:,i,j)+delta(i)*OfstUnkn;
        end;
        
        if (i>1)
            G=[G, zeros((i-1)*N*K,size(Gi,2)-L); Gi(:,1:L), zeros(N*K,size(G,2)-L), Gi(:,L+1:end)];
        else
            G=Gi;
        end;
    end;
    dHat=permute(dHat,[1 3 2]);
    
    % -- rearrange G and H with all time offsets at the end --
    if (OfstUnkn)
        tpG=G;
        for i=1:M,
            G(:,L+(2*L)*(i-1)+1:L+(2*L)*(i))=tpG(:,L+(2*L+1)*(i-1)+1:L+(2*L+1)*(i-1)+2*L);
            G(:,end-M+i)=tpG(:,L+(2*L+1)*(i-1)+2*L+1);
        end;
    end;
    
    dx=(G'*W*G)\G'*W*(d-dHat(:));
    if (norm(dx)>1e-10)
        theta0=theta0+0.125*dx;
    else
        continue;
    end;
end;

theta=theta0;

cost=(d-dHat(:))'*W*(d-dHat(:));
