function [Theta,iter]=DTDTDOALoc_MLE(m,S,Q,Theta0)
% [Theta,i]=DTDTDOALoc_MLE(m,S,Q,Theta0)
%
% Gauss-Newton MLE solution for the object and transmitter positions 
% by differential time delay (DTD) and TDOA measurements. 
%
% Input:
%   m:      ((2M-1) x 1), DTD & TDOA measurements, M = number of sensors
%   S:      (K x M), sensor positions, K = localization dimension
%   Q:      (M x M), DTD covariance matrix 
%   theta0: (2K x 1), hypothesized value
%
% Output:
%   theta:  (2K x 1), object and transmitter position estimate
%   iter:   number of iterations needed
%
% Reference:
% Y. Zhang and K. C. Ho, "Localization by signals of opportunity in the
% absence of transmitter position," IEEE Trans. Signal Process., vol. 72,
% pp. 4602-4617, 2022.
%
% Yang Zhang, K. C. Ho	10-18-2022
%
%       Copyright (C) 2022
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA
%       hod@missouri.edu
%

IterMax=5;     % maximum number of itertaions

[K,M]=size(S);    % K = dimension, M = number of receivers
                   
H=[-ones(M-1,1),eye(M-1)];
Theta=Theta0;

for i=1:1:IterMax
    u=Theta(1:K);
    t=Theta(K+1:2*K);
    
    d=sqrt(sum((repmat(t,1,M)-S(:,1:M)).^2))';
    r=sqrt(sum((repmat(u,1,M)-S(:,1:M)).^2))'+norm(u-t);
    r_d = r - d;
    rh = H * r;
    mHat = [r_d; rh];
    
    rho_ru=repmat(u,1,M)-S; rho_ru=rho_ru./(ones(K,1)*sqrt(sum(rho_ru.^2)));
    rho_rt=(t-u)/norm(u-t)*ones(1,M);
    rho_u=rho_ru-rho_rt;
    rho_dt=repmat(t,1,M)-S; rho_dt=rho_dt./(ones(K,1)*sqrt(sum(rho_dt.^2)));
    rho_t=rho_rt-rho_dt;
    
    J=[rho_u' rho_t';
        H*rho_ru' zeros(M-1,K)];
    
    err=m-mHat;
    temp=Theta;
    Theta=Theta+inv(J'*inv(Q)*J)*J'*inv(Q)*err;
 
    if norm(Theta-temp)<1e-3
        break;
    end
end

iter=i;

end

