function theta=DSF3DTMA_CFS_MLE(f,fc,S,N,c,T,Qn,Qfc,Qs,wghIdc)
% theta=DSF3DTMA_CFS_MLE(f,fc,S,N,c,T,Qn,Qfc,Qs,wghIdc)
%
% MLE with CFS as initialization for obtaining the initial object position
% and velocity for TMA.
%
% Input:
% f:      (MN x 1), DSF measurement vector,
%         (M=num. of sensors, N=num. of time measurements).
% fc:     scalar, center frequency.
% S:      (3 x M), sensor poition matrix.
% N:      scalar, number of time measurements.
% c:      scalar, signal propagation speed.
% T:      scalar, time measurement interval.
% Qn:     (MN x MN), covariance matrix of DSF measurements.
% Qfc:    scalar, variance of center frequency.
% Qs:     (3M x 3M), covariance matrix of sensor positions.
% wghIdc: scalar, weighting matrix indicator for different mix of errors
%         wghIdc=0   No weighting matrix
%         wghIdc=1   weighting matrix for measurement errors only
%         wghIdc=2   weighting matrix for measurement and sensor pos. errors
%         wghIdc=3   weighting matrix for measurement and source freq. errors
%         otherwise  Full weighting matrix
%
% Output:
% theta:  (6 x 1), solution, [objPos;objVel].
%
% Reference: M. H. Ahmed, K. C. Ho, and G. Wang, "3-D target localization
% and motion analysis based on Doppler shifted frequencies," IEEE Trans.
% Aerosp. Electron. Syst., vol. 58, no. 2, pp. 815-833, Apr. 2022.
%
% M. H. Ahmed, K. C. Ho     10-28-2022
%
%       Copyright (C) 2022
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA.
%       hod@missouri.edu
%

MxItn=50;    % maximum number of iterations

M=length(S);
theta0=DSF3DTMA_CFS(f,fc,S,N,c,T,Qn,Qfc,Qs,wghIdc);

S=S';
dvf=[theta0' fc]';
G=zeros(N*M,7+3*M);
alpha=[zeros(3*M,7) eye(3*M)];
beta=zeros(3*M+7,1);beta(7)=1;

theta=[dvf;reshape(S',3*M,1)];

gc=1;
n=0;
theta1=theta0;
while gc>1e-5&&gc<1e2
    %.{
    u_all_est=theta(1:3)*ones(1,N)+theta(4:6)*(0:N-1)*T;
    fo_est=theta(7);
    S_est=reshape(theta(8:end),3,M);
    d_est=sqrt(sum((repelem(u_all_est',M,1)-repmat(S_est',N,1)).^2,2));
    f_est=fo_est-fo_est/c*(((repelem(u_all_est',M,1)-repmat(S_est',N,1))./repmat(d_est,1,3))*theta(4:6));
    for k=0:N-1
        for i=1:M
            l=i+k*M;
            ums=theta(1:3)+k*T*theta(4:6)-S_est(:,i);
            a=-fo_est/c/d_est(l)*(theta(4:6)-(ums'*theta(4:6)*ums)/d_est(l)^2)';
            b=-fo_est/c/d_est(l)*(ums+k*T*theta(4:6)-k*T*ums'*theta(4:6)*ums/d_est(l)^2)';
            d=1-1/c*ums'*theta(4:6)/d_est(l);
            C=[zeros(1,3*(i-1)) -a zeros(1,3*(M-i))];
            G(l,:)=[a b d C];
        end
    end

    theta=(beta/Qfc*beta'+alpha'/Qs*alpha+G'/Qn*G)\(G'/Qn*(f-f_est+G*theta)+alpha'/Qs*reshape(S',3*M,1)+beta/Qfc*fc);
    theta2=theta(1:6);
    gc=norm(theta2-theta1);
    theta1=theta2;
    n=n+1;
    if n==MxItn, break; end
end

theta=theta(1:6);

end

