function theta=DSF2DLoc_SDP(f,S,fo,c,Qn,Qs,Qfo,Idc)
% theta=DSF2DLoc_SDP(f,S,fo,c,Qn,Qs,Qfo,Idc)
%
% SDP 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);

try
    
    % ----- initial solution to form weighting matrix -----
    theta=DSFLoc2D_SDPCore(f,S,fo,c,eye(M));
    
    % -- solve for velocity sign --
    r=sqrt(sum((repmat(theta(1:2),1,M)-S).^2));
    f_est1=fo-fo/c*(((repmat(theta(1:2),1,M)-S)./repmat(r,2,1))'*[-1,0;0,-1]*theta(3:4));
    f_est2=fo-fo/c*(((repmat(theta(1:2),1,M)-S)./repmat(r,2,1))'*[1,0;0,-1]*theta(3:4));
    f_est3=fo-fo/c*(((repmat(theta(1:2),1,M)-S)./repmat(r,2,1))'*[-1,0;0,1]*theta(3:4));
    f_est4=fo-fo/c*(((repmat(theta(1:2),1,M)-S)./repmat(r,2,1))'*[1,0;0,1]*theta(3:4));
    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];
    [costm,idx]=min([cost1 cost2 cost3 cost4]);
    P=diag(code(idx,:));
    
    % ----- final solution -----
    
    % -- obtain weighting matrix --
    if (Idc~=0)
        switch Idc,
            case 1,
                Qs=0; Qfo=0;
            case 2,
                Qfo=0;
            case 3,
                Qs=0;
        end;
        Df=-f/fo;
        d=c*(1-f/fo);
        u1=theta(1:2); v1=P*theta(3:4);
        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);
        iW1=B1*(Ds*(Qs)*Ds'+Df*Qfo*Df'+Qn)*B1';
        iW1=iW1/max(iW1(:));
    end;
    
    % -- recompute solution --
    theta=DSFLoc2D_SDPCore(f,S,fo,c,iW1);
    
    theta=[theta(1:2);P*theta(3:4)];
    
catch
    theta=ones(4,1)*nan;
end

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


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function theta=DSFLoc2D_SDPCore(f,S,fo,c,iW)
% theta=DSFLoc2D_SDPCore(f,S,fo,c,iW)
%
% SDP main processing routine for the object localization (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
%   iW:      (M x M), inverse of weighting matrix
%
% Output:
%   theta:  (4 x 1), object location ([position; velocity]) estimate
%
% 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
% 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);
d=c*(1-f/fo);
scale1=1.8e-3;
scale2=1.5e-1;
S=S.*scale1;
d=d*scale2;

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

%.....................................................................
%            CVX solution
%.....................................................................
cvx_solver sedumi
%cvx_solver sdpt3
cvx_precision best
cvx_begin sdp quiet;
variable Z(9,9) symmetric;
variable z(9);
%minimize((trace(A'/iW*A*Z)-2*(A*Z(:,10))'/iW*m))
minimize((trace(A'/iW*A*Z)-2*(A*z)'/iW*m))
subject to
%
%Z==semidefinite(10);
[Z z;z' 1]==semidefinite(10);
%
z(3)>=0;z(4)>=0;z(5)>=0;z(9)>=0;
Z(3,4)>=0;Z(3,5)>=0;Z(3,9)>=0;
Z(4,5)>=0;Z(4,9)>=0;
Z(5,9)>=0;
%
z(3)==Z(1,1)+Z(2,2);
Z(6,6)==Z(4,5);
z(7)==Z(1,4)+Z(2,6);
z(8)==Z(1,6)+Z(2,5);
z(9)==Z(1,7)+Z(2,8);
%
Z(7,7)==Z(4,9);
Z(8,8)==Z(5,9);
Z(7,8)==Z(6,9);
Z(5,7)==Z(6,8);
Z(4,8)==Z(6,7);
%
cvx_end

Z=[Z z;z' 1];
[Z,D]=eig(Z);
z2=pow_p(D(end,end),.5)*Z(:,end);
theta=[z2(1:2)/scale1;sqrt(z2(4:5))/scale2];

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

