function theta=DSF2DLoc_CFS(f,S,fo,c,Qn,Qs,Qfo,idc)
% theta=DSF2DLoc_CFS(f,S,fo,c,Qn,Qs,Qfo,idc)
%
% Closed-form solution for the object location (position and velocity)
% in 2-D by using frequency measurements at single time instant.
%
% 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 for weighting matrix generation
%           idc =0, identity weighting matrix
%           idc =1, for measurement noise only
%           idc=2,  for measurement noise and sensor pos. errors
%           idc=3,  for measurement noise and source freq. errors
%           otherwise,    for measurement noise, sensor position and source freq. errors
%
% 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
% Musaab Ahmed, K. C. Ho	11-26-2021, changed symbols for weighting matrix
%
%       Copyright (C) 2021
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA
%       hod@missouri.edu
%

M=length(S);        % number of sensors
d=c*(1-f/fo);

%====================================
%   	stage 1
%====================================

h1=zeros(M,1);A1=zeros(M,9);
d2=d.^2;
for i=1:M
    h1(i,1)=d2(i)*(S(:,i)'*S(:,i));
    A1(i,:)=[2*d2(i)*S(:,i)' -d2(i) S(:,i)'.^2 2*S(1,i)*S(2,i) -2*S(:,i)' 1];
end

% -- initial estimate --
psi1=(A1'*A1)\A1'*h1;
psi1(4:5)=real(sqrt(psi1(4:5))).^2;

% -- resolve velocity sign --
[P]=VelSign(f,psi1(1:2),psi1(4:5),S,fo,c,Qn);

% -- generate weighting matrix --
if (idc==0)
    iW1=eye(M);
else
    Df=-f/fo;
    u1=psi1(1:2); v1=P*sqrt(psi1(4:5));
    r=sqrt(sum((repmat(u1,1,M)-S).^2));
    Ds1=-fo/c*v1(1)./r'+(f-fo)./r'.^2.*(S(1,:)'-u1(1));Ds1=diag(Ds1);
    Ds2=-fo/c*v1(2)./r'+(f-fo)./r'.^2.*(S(2,:)'-u1(2));Ds2=diag(Ds2);
    Ds= Ds2(:,[1;1]*(1:size(Ds2,2)));Ds(:,1:2:end) = Ds1;
    B1=2*c/fo*diag(d.*r'.^2);
    switch idc,
        case 1,
            Qs=0; Qfo=0;
        case 2,
            Qfo=0;
        case 3,
            Qs=0;
    end;
    iW1=B1*(Ds*(Qs)*Ds'+Df*Qfo*Df'+Qn)*B1';
    iW1=iW1/max(iW1(:));
end;

% -- improved stage one solution --
psi1=(A1'/iW1*A1)\A1'/iW1*h1;

%====================================
%	Stage 2
%====================================

M2=[psi1(1:5);psi1(6)^2;psi1(7:9)];

A2=zeros(9,4);
A2(1:2,1:2)=eye(2);
A2(3,1:2)=psi1(1:2)';
A2(4:5,3:4)=eye(2);
A2(6,3:4)=0.5*[psi1(5) psi1(4)];
A2(7,1:2)=[psi1(4) psi1(6)];
A2(8,1:2)=[psi1(6) psi1(5)];
A2(9,1:2)=[psi1(7) psi1(8)];

% -- generate weighting matrix --
psi2=[psi1(1:2);psi1(4:5)];
%        for l=1:3,
B2=eye(9);
B2(3,1:2)=-psi2(1:2)';
B2(6,4:6)=[-0.5*psi2(4) -0.5*psi2(3) 2*psi1(6)];
B2(7,4:6)=[-psi2(1) 0 -psi2(2)];
B2(8,5:6)=[-psi2(2) -psi2(1)];
B2(9,7:8)=-[psi2(1) psi2(2)];
iW2=B2/(A1'/iW1*A1)*B2';iW2=iW2/max(iW2(:));

% -- stage two solution --
psi2=(A2'/iW2*A2)\(A2'/iW2*M2);
%       end

psi2org=psi2;
psi2(3:4)=(real(sqrt(psi2(3:4)))).^2;

% -- resolve velocity sign --
[P,costm]=VelSign(f,psi2(1:2),psi2(3:4),S,fo,c,Qn);


% -- final solution --
theta=[psi2(1:2);P*sqrt(psi2(3:4))];
thetac=theta;

% -- alternative velocity estimation from measurement model --
r=sqrt(sum((repmat(psi2(1:2),1,M)-S).^2));
B=[ones(M,1) -1/c*(((repmat(psi2(1:2)',M,1)-S')./repmat(r',1,2)))];
fo_udot=(B'*B)\B'*f;
thetax=[psi2(1:2);fo_udot(2:3)/fo_udot(1)];

% -- select the better solution --
f_estx=fo-fo/c*(((repmat(thetax(1:2),1,M)-S)./repmat(r,2,1))'*thetax(3:4));
costx=(f-f_estx)'/Qn*(f-f_estx);
if (costx<costm) theta=thetax; end;

% -- CFS velocity estimate is not reliable if it has small x-velocity --
% -- or y-velocity values.  If it happens,use the frequency measurement --
% -- model to obtain the velocity --

if (psi2(3)<4 || psi2(4)<4)  theta=thetax;  end;

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


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [P,costMin]=VelSign(f,pos,vel2,S,fo,c,Qn)
% [P,costMin]=VelSign(f,pos,vel2,S,fo,c,Qn)
%
% Determing the signs of the velocity components based on the ML cost
% function
%
% Input:
%   f:      (M x 1), frequency measurements, M = number of sensors
%   pos:    (2 x 1), postion vector
%   vel2:   (2 x 1), velocity square vector
%   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
%
% Output:
%   P:      (2 x 2), velocity sign matrix
%   costMin: scalar, minimum 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
%

% -- Solve for velocity sign --
M=length(S);

r=sqrt(sum((repmat(pos,1,M)-S).^2));
f_est1=fo-fo/c*(((repmat(pos,1,M)-S)./repmat(r,2,1))'*[-1,0;0,-1]*sqrt(vel2));
f_est2=fo-fo/c*(((repmat(pos,1,M)-S)./repmat(r,2,1))'*[1,0;0,-1]*sqrt(vel2));
f_est3=fo-fo/c*(((repmat(pos,1,M)-S)./repmat(r,2,1))'*[-1,0;0,1]*sqrt(vel2));
f_est4=fo-fo/c*(((repmat(pos,1,M)-S)./repmat(r,2,1))'*[1,0;0,1]*sqrt(vel2));
cost1=(f-f_est1)'/Qn*(f-f_est1);
cost2=(f-f_est2)'/Qn*(f-f_est2);
cost3=(f-f_est3)'/Qn*(f-f_est3);
cost4=(f-f_est4)'/Qn*(f-f_est4);
code=[-1 -1;1 -1;-1 1;1 1];
[costMin,idx]=min([cost1 cost2 cost3 cost4]);
P=diag(code(idx,:));

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



