function theta=DSF3DLoc_CFS_MLE(f,fc,S,c,Qn,Qfc,Qs,wghIdc)
% theta=DSF3DLoc_CFS_MLE(f,fc,S,c,Qn,Qfc,Qs,wghIdc)
%
% MLE with CFS as initialization for obtaining the object position
% and velocity.
%
% Input:
% f:      (M x 1), DSF measurement vector, M=num. of sensors.
% fc:     scalar, center frequency.
% S:      (3 x M), sensor poition matrix.
% c:      scalar, signal propagation speed.
% Qn:     (M x M), 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
%

% -- initial guess --
theta0=DSF3DLoc_CFS(f,fc,S,c,Qn,Qfc,Qs,wghIdc);

% -- Gauss-Newton MLE --
MaxItn=50;  % maximum number of iterations

S=S';
M=length(S);

dvf=[theta0' fc]';
a=[zeros(3*M,7) eye(3*M)];
b=[zeros(1,6) 1 zeros(1,3*M)]';
theta=[dvf;reshape(S',3*M,1)];

gc=1;
n=0;

while (gc>1e-3)&&gc<1e3
    Sne=reshape(theta(8:end),3,M)';
    foe=theta(7);
    u=theta(1:3);
    udot=theta(4:6);

    d=sqrt(sum((repmat(u',M,1)-Sne).^2,2));
    g1=(((repmat(u',M,1)-Sne)./repmat(d,1,3))*udot);
    g=foe-foe/c*g1;
    g_u=-foe/c*(repmat(udot',M,1)./repmat(d,1,3)-repmat(g1./d.^2,1,3).*(repmat(u',M,1)-Sne));
    g_udot=-foe/c*((repmat(u',M,1)-Sne)./repmat(d,1,3));
    g_fo=1-1/c*g1;
    G=[g_u g_udot g_fo de2bi(7*8.^(0:M-1),3*M)*diag(reshape(-g_u',3*M,1))];
    gc=norm(G'/Qn*(f-g)+a'/Qs*(reshape(S',3*M,1)-a*theta)+b/Qfc*(fc-foe));
    theta=(b/Qfc*b'+a'/Qs*a+G'/Qn*G)\(G'/Qn*(f-g+G*theta)+a'/Qs*reshape(S',3*M,1)+b/Qfc*fc);
    n=n+1;
    if n==MaxItn, break; end;
end

theta=theta(1:6);

end

