function theta=DSF2DLocFoUnkwn(f,S,c,Qn,Qs,idc,mthd)
% theta=DSF2DLocFoUnkwn(f,S,c,Qn,Qs,idc,mthd)
%
% Closed-form solution for the object location (position and velocity)
% in 2-D by using frequency measurements at single time instant,
% 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
%   Qn:     (M x M), measurement noise covariance matrix
%   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	08-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*15/c;
fmin=fa-fa*15/c;
fo_step=(fmax-fmin)/300;
fo=fmin:fo_step:fmax;

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

cost=1e9*ones(length(fo),1);
for i=1:length(fo)
     cost(i)= obj(f,S,fo(i),c,Qn,Qs,0,idc,mthd);
end
[~,idx]=min(cost);

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

fo_tst=fo(idx);

epsilon=fo_step/2;
Iter=1;
while epsilon>1e-5&&Iter<=3
    
    J1=de_obj(f,S,fo_tst,c,Qn,Qs,0,idc,dlta,mthd);
    J1_dta=de_obj(f,S,fo_tst+dlta,c,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*20/c fo fa+fa*20/c];
if fo_tst>fo(idx+2)||fo_tst<fo(idx)
    fo_tst=fo(idx+1);
end

if (mthd==1)
    theta=DSF2DLoc_CFS(f,S,fo_tst,c,Qn,Qs,0,idc);
else
    theta=DSF2DLoc_SDP(f,S,fo_tst,c,Qn,Qs,0,idc);
end;

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


% ==================================================================
function J=obj(f,S,fo,c,Qn,Qs,Qfo,idc,mthd)
% J=obj(f,S,fo,c,Qn,Qs,Qfo,M,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
%   Qn:     (M x M), measurement noise covariance matrix
%   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

if mthd==1      % CFS
    theta=DSF2DLoc_CFS(f,S,fo,c,Qn,Qs,Qfo,idc);
else            % SDP
    theta=DSF2DLoc_SDP(f,S,fo,c,Qn,Qs,Qfo,idc);
end
r_p=sqrt(sum((repmat(theta(1:2),1,M)-S).^2));
f_p=fo-fo/c*(((repmat(theta(1:2),1,M)-S)./repmat(r_p,2,1))'*theta(3:4));

Df=-f/fo;
Ds1=(f'-fo)./r_p.^2.*(S(1,:)-theta(1))-fo/c*theta(3)./r_p;Ds1=diag(Ds1);
Ds2=(f'-fo)./r_p.^2.*(S(2,:)-theta(2))-fo/c*theta(4)./r_p;Ds2=diag(Ds2);
Ds= Ds2(:,[1;1]*(1:size(Ds2,2)));Ds(:,1:2:end) = Ds1;

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,Qn,Qs,Qfo,idc,dlta,mthd)
% J1=de_obj(f,S,fo,c,Qn,Qs,Qfo,M,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
%   Qn:     (M x M), measurement noise covariance matrix
%   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,Qn,Qs,Qfo,idc,mthd);
J_del=obj(f,S,fo+dlta,c,Qn,Qs,Qfo,idc,mthd);
J1=(J_del-J)/dlta;

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


