function [gamma,i]=MSLocNoTxMOST_MLE(r,fr,s,Qr,Qf,gamma0)
% [gamma,i]=MSLocNoTxMOST_MLE(r,fr,s,Qr,Qf,gamma0)
%
% This function realizes the Gauss-Newton iterative MLE for estimating
% the unknown object position in the MOST scenario, when the object 
% is the transmitter.
%
% Input parameters:
% r:    (M x 1), indirect-path range measurements.
% s:    (Dim x M), receiver position matrix, M is the number of receivers.
% Q:    (M x M), covariance matrix of the range measurements.
% gamma0: inital solution guess.
%
% Output parameters:
% gamma:(2 Dim+2 x 1), final solution,
%       [objPos;objVel;ofStTm;ofStFq]^T.
% i:    number of iterations to reach the solution.
%
% The program can be used for 2D(Dim=2) or 3D(Dim=3) localization.
%
% Reference:
% Y. Zhang and K. C. Ho, "Multistatic localization in partially dynamic 
% scenario with only sensor positions svailable," IEEE Trans. Aerosp. 
% Electron. Syst., vol. 57, no. 5, pp. 3416-3432, Oct. 2021.
% 
% Yang Zhang and K. C. Ho   10-28-2021
%
%       Copyright (C) 2021
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA.
%       hod@missouri.edu
%

[Dim,M]=size(s);    % M=number of receivers
                    % Dim=dimension

gamma=gamma0;
imax=30;
Q=blkdiag(Qr,Qf);
step=0.5;

for i=1:1:imax
    u=gamma(1:Dim);
    vu=gamma(Dim+1:2*Dim);
    offsetd=gamma(2*Dim+1);
    offsetf=gamma(2*Dim+2);
    
    ro_hat=sqrt(sum((repmat(u,1,M)-s(:,1:M)).^2))'+offsetd;
    fro_hat=(repmat(u,1,M)-s(:,1:M))'*vu./sqrt(sum((repmat(u,1,M)-s(:,1:M)).^2))'+offsetf;
    
    rho_ru=repmat(u,1,M)-s; rho_ru=rho_ru./(ones(Dim,1)*sqrt(sum(rho_ru.^2)));
    rho_u=rho_ru;
    
    rho_fru=repmat(vu,1,M)./repmat(sqrt(sum((repmat(u,1,M)-s).^2)),Dim,1)-(repmat(((repmat(u,1,M)-s)'*vu)',Dim,1).*(repmat(u,1,M)-s))./repmat((sqrt(sum((repmat(u,1,M)-s).^2)).^3),Dim,1);
    
    Jr=[rho_u' zeros(M,Dim) ones(M,1) zeros(M,1)];
    Jfr=[rho_fru' rho_u' zeros(M,1) ones(M,1)];
    J=[Jr;Jfr];
    err=[r-ro_hat;fr-fro_hat];
    
    temp=gamma;
    gamma=gamma+step*inv(J'*inv(Q)*J)*J'*inv(Q)*err;
    
    if norm((gamma-temp))<1e-6
%     if max(abs(gamma-temp))<1e-6
        break;
    end
    
end
