function [gamma,i]=MSLocMOST_MLE(r,d,fr,fd,s,Qr,Qd,Qfr,Qfd,gamma0)
% [gamma,i]=MSLocMOST_MLE(r,d,fr,fd,s,Qr,Qd,Qfr,Qfd,gamma0)
%
% This function realizes the iterative Gauss-Newton MLE for jointly
% estimating the unknown object and transmitter locations in the MOST scenario 
% using both indirect- and direct-path range and range-rate measurements.
%
% Input parameters:
% r:      (M x 1), indirect-path range measurements.
% d:      (M x 1), direct-path range measurements.
% fr:     (M x 1), indirect-path range-rate measurements.
% fd:     (M x 1), direct-path range-rate measurements.
% s:      (Dim x M), receiver position matrix, M is the number of receivers.
% Qr:     (M x M), covariance matrix of the indirect range measurements.
% Qd:     (M x M), covariance matrix of the direct range measurements.
% Qfr:    (M x M), covariance matrix of the indirect range-rate measurements.
% Qfd:    (M x M), covariance matrix of the direct range-rate measurements.
% gamma0: inital solution guess.
%
% Output parameters:
% gamma:  (3*Dim+2 x 1), final solution,
%         [objPos;objVel,txPos;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,Qfr,Qd,Qfd);
step=0.5;

for i=1:1:imax
    u=gamma(1:Dim);
    vu=gamma(Dim+1:2*Dim);
    t=gamma(2*Dim+1:3*Dim);
    offsetd=gamma(3*Dim+1);
    offsetf=gamma(3*Dim+2);
    
    do_hat=sqrt(sum((repmat(t,1,M)-s(:,1:M)).^2))'+offsetd;
    ro_hat=sqrt(sum((repmat(u,1,M)-s(:,1:M)).^2))'+norm(u-t)+offsetd;
    fdo_hat=ones(M,1)*offsetf;
    fro_hat=(repmat(u,1,M)-s(:,1:M))'*vu./sqrt(sum((repmat(u,1,M)-s(:,1:M)).^2))'+(u-t)'*(vu)/norm(u-t)+offsetf;
    
    rho_ru=repmat(u,1,M)-s; rho_ru=rho_ru./(ones(Dim,1)*sqrt(sum(rho_ru.^2)));
    rho_rt=(u-t)/norm(u-t)*ones(1,M);
    rho_u=rho_ru+rho_rt;
    
    rho_dt=repmat(t,1,M)-s; rho_dt=rho_dt./(ones(Dim,1)*sqrt(sum(rho_dt.^2)));
    
    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) ...
        +repmat((vu)/norm(u-t)-((u-t)'*(vu)*(u-t))/(norm(u-t)^3),1,M);
    rho_frt=repmat((-vu)/norm(u-t),1,M)-((t-u)'*(-vu)*(t-u))/(norm(u-t)^3)*ones(1,M);
    
    Jr=[rho_u' zeros(M,Dim) -rho_rt' ones(M,1) zeros(M,1)];
    Jd=[zeros(M,Dim) zeros(M,Dim) rho_dt' ones(M,1) zeros(M,1)];
    Jfr=[rho_fru' rho_u' rho_frt' zeros(M,1) ones(M,1)];
    Jfd=[zeros(M,Dim) zeros(M,Dim) zeros(M,Dim) zeros(M,1) ones(M,1)];
    J=[Jr;Jfr;Jd;Jfd];
    err=[r-ro_hat;fr-fro_hat;d-do_hat;fd-fdo_hat];
    temp=gamma;
    gamma=gamma+step*inv(J'*inv(Q)*J)*J'*inv(Q)*err;
    
    if norm(gamma-temp)<1e-6
        break;
    end
    
end
