function theta=DSFLoc_MtTm_foUnkwn_2D(f,S,c,T,Qn,Qs,idc,mthd)
% theta=DSFLoc_MtTm_foUnkwn_2D(f,S,c,T,Qn,Qs,idc,mthd)
%
% Closed-form solution for the object location (position and velocity)
% in 2-D by using frequency measurements at multiple instants,
% unknown source frequency.
%
% Input:
%   f:      (M x 1), frequency measurements, M = number of sensors
%   S:      (2 x M), sensor positions
%   c:      scalar, signal propagation speed
%   T:      time interval between two consecutive measurements
%   Qn:     (MN x MN), measurement noise covariance matrix, N = number of instants
%   Qs:     (2M x 2M), sensor position error covariance matrix
%   idc:    scalar, indicator to form weighting matrix
%   mthd:   1: SCF, 2: SDP
%
% Output:
%   theta:  (4 x 1), object location ([position; velocity]) estimate
%
% Reference:
% M. Ahmed, K. C. Ho, and G. Wang, "Localization of a moving source by
% frequency measurements," IEEE Trans. Signal Process., vol. 68,
% pp. 4839-4854, 2020.
%
% Musaab Ahmed, K. C. Ho	07-18-2021
%
%       Copyright (C) 2021
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA
%       hod@missouri.edu
%

dlta=0.03;  % for gradient generation in the Newton's algorithm

M=length(S);
fa=mean(f);
fmax=fa+fa*10/c;
fmin=fa-fa*10/c;
fo_step=(fmax-fmin)/200;
fo=fmin:fo_step:fmax;

%=============================
%      Initialization
%=============================

for ll=1:4
    fo_step=fo_step/2;
    cost=1e9*ones(length(fo),1);
    for i=1:length(fo)
        cost(i)=obj(f,S,fo(i),c,T,Qn,Qs,0,idc,mthd);
    end
    cost(isnan(cost))=1e9;
    [~,idx]=min(cost);
    fo_tst=fo(idx);
    step1=fo_step/8;
    fo=fo_tst-fo_step:step1:fo_tst+fo_step;
    fo_step=step1;
end

%=============================
%     Newton's Method
%=============================

fo_tst=fo(idx);     % coarse source frequency estimate

epsilon=fo_step/2;
Iter=1;
while epsilon>1e-5&&Iter<=3
    J1=de_obj(f,S,fo_tst,c,T,Qn,Qs,0,idc,dlta,mthd);
    J1_dta=de_obj(f,S,fo_tst+dlta,c,T,Qn,Qs,0,idc,dlta,mthd);
    J2=(J1_dta-J1)/dlta;
    fo_tst=fo_tst-J1/J2;
    epsilon=abs(J1/J2);
    Iter=Iter+1;
end

fo=[fa-fa*11/c fo fa+fa*11/c];
if fo_tst>fo(idx+2)||fo_tst<fo(idx)
    fo_tst=fo(idx+1);
end

if (mthd==1)    % CFS
    theta=DSF2DLocMtTm_CFS(f,S,fo_tst,c,T,Qn,Qs,0,idc);
else            % SDP
    theta=DSF2DLocMtTm_SDP(f,S,fo_tst,c,T,Qn,Qs,0,idc);
end;

% --------------------------------------------------------------


% ==================================================================
function J=obj(f,S,fo,c,T,Qn,Qs,Qfo,idc,mthd)
% J=obj(f,S,fo,c,T,Qn,Qs,Qfo,idc,mthd)
%
% Evaluate the approximated value of the ML cost
%
% Input:
%   f:      (M x 1), frequency measurements, M = number of sensors
%   S:      (2 x M), sensor positions
%   fo:     scalar, available center frequency that is not accurate
%   c:      scalar, signal propagation speed
%   T:      time interval between two consecutive measurements
%   Qn:     (MN x MN), measurement noise covariance matrix, N = number of instants
%   Qs:     (2M x 2M), sensor position error covariance matrix
%   Qfo:    scalar, variance of available center frequency
%   idc:    scalar, indicator to form weighting matrix
%   mthd:   scalar, 1:CFS, 2: SDP
%
% Output:
% J:        scalar, approximated value of the ML cost
%
% References:
% M. Ahmed, K. C. Ho, and G. Wang, "Localization of a moving source by
% frequency measurements," IEEE Trans. Signal Process., vol. 68,
% pp. 4839-4854, 2020.
%
% Musaab Ahmed, K. C. Ho	07-18-2021
%
%       Copyright (C) 2021
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA
%       hod@missouri.edu
%

M=length(S);    % number of sensors
N=length(Qn)/M;

if mthd==1      % CFS
    theta=DSF2DLocMtTm_CFS(f,S,fo,c,T,Qn,Qs,Qfo,idc);
else            % SDP
    theta=DSF2DLocMtTm_SDP(f,S,fo,c,T,Qn,Qs,Qfo,idc);
end

% -- obtainging weighting matrix --
u=theta(1:2);v=theta(3:4);
u_all=theta(1:2)*ones(1,N)+theta(3:4)*(0:N-1)*T;
r_p=sqrt(sum((repmat(S,1,N)-kron(u_all,ones(1,M))).^2,1));
f_p=fo-fo/c*(((kron(u_all,ones(1,M))-repmat(S,1,N))./repmat(r_p,2,1))'*theta(3:4));
Df=-f/fo;
Ds1=fo/c*v(1)./r_p-(f'-fo)./r_p.^2.*(repmat(S(1,:),1,N)-kron(u_all(1,:),ones(1,M)));
Ds2=fo/c*v(2)./r_p-(f'-fo)./r_p.^2.*(repmat(S(2,:),1,N)-kron(u_all(2,:),ones(1,M)));
for n=0:N-1
    Dsa=Ds1(n*M+1:M+n*M);Dsa=diag(Dsa);
    Dsb=Ds2(n*M+1:M+n*M);Dsb=diag(Dsb);
    Dsc= Dsb(:,[1;1]*(1:size(Dsb,2)));Dsc(:,1:2:end) = Dsa;
    Ds(n*M+1:M+n*M,:)=Dsc;
end

if idc==0
    W1=eye(M);
elseif idc==1
    W1=Qn;
elseif idc==2
    W1=Ds*(Qs)*Ds'+Qn;
elseif idc==3
    W1=Df*Qfo*Df'+Qn;
else
    W1=Ds*(Qs)*Ds'+Df*Qfo*Df'+Qn;
end

J=(f-f_p)'/W1*(f-f_p);

% --------------------------------------------------------------


% ==================================================================
function J1=de_obj(f,S,fo,c,T,Qn,Qs,Qfo,idc,dlta,mthd)
% J1=de_obj(f,S,fo,c,T,Qn,Qs,Qfo,idc,dlta,mthd)
%
% Obtain the gradient of the ML cost for the Newton's algorithm
%
% Input:
%   f:      (M x 1), frequency measurements, M = number of sensors
%   S:      (2 x M), sensor positions
%   fo:     scalar, available center frequency that is not accurate
%   c:      scalar, signal propagation speed
%   T:      time interval between two consecutive measurements
%   Qn:     (MN x MN), measurement noise covariance matrix, N = number of instants
%   Qs:     (2M x 2M), sensor position error covariance matrix
%   Qfo:    scalar, variance of available center frequency
%   idc:    scalar, indicator to form weighting matrix
%   dlta:   scalar, delta value for obtaining the gradient by finite difference
%   mthd:   scalar, 1:CFS, 2: SDP
%
% Output:
% J1:       scalar, approximated gradient value of the ML cost
%
% References:
% M. Ahmed, K. C. Ho, and G. Wang, "Localization of a moving source by
% frequency measurements," IEEE Trans. Signal Process., vol. 68,
% pp. 4839-4854, 2020.
%
% Musaab Ahmed, K. C. Ho	07-18-2021
%
%       Copyright (C) 2021
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA
%       hod@missouri.edu
%

M=length(S);    % number of sensors

J=obj(f,S,fo,c,T,Qn,Qs,Qfo,idc,mthd);
J_del=obj(f,S,fo+dlta,c,T,Qn,Qs,Qfo,idc,mthd);
J1=(J_del-J)/dlta;

% --------------------------------------------------------------


