function theta=DSF3DLoc_SDP(f,fc,S,c,Qn,Qfc,Qs,wghIdc)
%theta=DSF3DLoc_SDP(f,fc,S,c,Qn,Qs,Qfc,wghIdc)
%
% SDP solution by SDR 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
%

M=length(S);
try
    theta0=sdp(f,fc,S,c,eye(M));    % initial estimate without weighting

    % -- resolve velocity sign --
    S=S';
    dn=sqrt(sum((repmat(theta0(1:3)',M,1)-S).^2,2));

    f_est1=fc-fc/c*(((repmat(theta0(1:3)',M,1)-S)./repmat(dn,1,3))*[-1 0 0;0 -1 0;0 0 -1]*theta0(4:6));
    f_est2=fc-fc/c*(((repmat(theta0(1:3)',M,1)-S)./repmat(dn,1,3))*[1 0 0;0 -1 0;0 0 -1]*theta0(4:6));
    f_est3=fc-fc/c*(((repmat(theta0(1:3)',M,1)-S)./repmat(dn,1,3))*[-1 0 0;0 1 0;0 0 -1]*theta0(4:6));
    f_est4=fc-fc/c*(((repmat(theta0(1:3)',M,1)-S)./repmat(dn,1,3))*[1 0 0;0 1 0;0 0 -1]*theta0(4:6));
    f_est5=fc-fc/c*(((repmat(theta0(1:3)',M,1)-S)./repmat(dn,1,3))*[-1 0 0;0 -1 0;0 0 1]*theta0(4:6));
    f_est6=fc-fc/c*(((repmat(theta0(1:3)',M,1)-S)./repmat(dn,1,3))*[1 0 0;0 -1 0;0 0 1]*theta0(4:6));
    f_est7=fc-fc/c*(((repmat(theta0(1:3)',M,1)-S)./repmat(dn,1,3))*[-1 0 0;0 1 0;0 0 1]*theta0(4:6));
    f_est8=fc-fc/c*(((repmat(theta0(1:3)',M,1)-S)./repmat(dn,1,3))*[1 0 0;0 1 0;0 0 1]*theta0(4:6));

    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);
    cost5=(f-f_est5)'/Qn*(f-f_est5);
    cost6=(f-f_est6)'/Qn*(f-f_est6);
    cost7=(f-f_est7)'/Qn*(f-f_est7);
    cost8=(f-f_est8)'/Qn*(f-f_est8);

    code=[-1 -1 -1;1 -1 -1;-1 1 -1;1 1 -1;-1 -1 1;1 -1 1;-1 1 1;1 1 1];

    [~,idx]=min([cost1 cost2 cost3 cost4 cost5 cost6 cost7 cost8]);
    L=diag(code(idx,:));

    S=S';
    if wghIdc~=0
        iW=DSF3DLoc_Weight(f,fc,S,c,theta0(1:3),L*theta0(4:6),Qn,Qfc,Qs,wghIdc);
        theta=sdp(f,fc,S,c,iW);     % solution with weighting
    end
    if ~isnan(theta(1))
        theta=[theta(1:3);L*theta(4:6)];
    end

catch
    theta=zeros(6,1);
end

end

% =========================================================================
function theta=sdp(f,fc,S,c,iW)
% [theta,opt_val]=sdp(f,fc,S,c,iW)
%
% SDP solution after SDR using CVX.
%
% Input:
% f:      (MN x 1), DSF measurement vector, 
%         (M=num. of sensors, N=num. of time measurements).
% fc:     scalar, center frequency.
% S:      (3 x M), sensor poition matrix.
% c:      scalar, signal propagation speed.
% iW:     (MN x MN), inverse of the 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.
% 
% K. C. Ho     10-18-2022
%
%       Copyright (C) 2022
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA.
%       hod@missouri.edu
%

S=S';
M=length(S);
r=c*(1-f/fc);

% -- scaling
a=sqrt((S(:,1)-S(:,1)').^2+(S(:,2)-S(:,2)').^2+(S(:,3)-S(:,3)').^2)+eps*eye(M);a=max(a(:));
scale1=1/a;%1.5e-3;
scale2=1/norm(r,inf);%1.5e-1;
S=S.*scale1;
r=r*scale2;

r2=r.^2;
h=zeros(M,1);A=zeros(M,14);
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(i,1)*S(i,2) -2*S(i,:) 1 0];
    %A(i,:)=[2*d2(i)*S(i,:) -d2(i) S(i,:).^2 2*S(i,1)*S(i,2) -2*S(i,:) 1];
    h(i,1)=r2(i)*(S(i,:)*S(i,:)');
    A(i,:)=[2*r2(i)*S(i,:) S(i,:).^2 -r2(i) 2*S(i,1)*S(i,2) 2*S(i,1)*S(i,3) 2*S(i,2)*S(i,3) -2*S(i,:) 1];
end

% -- solution by cvx --
%cvx_solver sedumi
cvx_solver sdpt3
cvx_precision best
cvx_begin sdp quiet;
variable PHI(14,14) symmetric;
variable phi(14);
%minimize((trace(A'/iW*A*PHI)-2*(A*PHI(:,10))'/iW*m))
minimize((trace(A'/iW*A*PHI)-2*(A*phi)'/iW*h))
subject to
%-------------------------------------------
[PHI phi;phi' 1]==semidefinite(15);
%-------------------------------------------
phi(4)>=0;phi(5)>=0;phi(6)>=0;phi(7)>=0;phi(14)>=0;
%   PHI(4,5)>=0;PHI(4,6)>=0;PHI(4,7)>=0;PHI(4,14)>=0;
%  PHI(5,6)>=0;PHI(5,7)>=0;PHI(5,14)>=0;
%   PHI(6,7)>=0;PHI(6,14)>=0;
%   PHI(7,14)>=0;
%-------------------------------------------
phi(7)==PHI(1,1)+PHI(2,2)+PHI(3,3);
phi(11)==PHI(1,4)+PHI(2,8)+PHI(3,9);
phi(12)==PHI(1,8)+PHI(2,5)+PHI(3,10);
phi(13)==PHI(1,9)+PHI(2,10)+PHI(3,6);
phi(14)==PHI(1,11)+PHI(2,12)+PHI(3,13);
%-------------------------------------------
PHI(4,5)==PHI(8,8);
PHI(4,6)==PHI(9,9);
PHI(5,6)==PHI(10,10);
%-------------------------------------------
PHI(11,11)==PHI(4,14);
PHI(12,12)==PHI(5,14);
PHI(13,13)==PHI(6,14);
%-------------------------------------------
PHI(11,12)==PHI(8,14);
PHI(11,13)==PHI(9,14);
PHI(12,13)==PHI(10,14);
%-------------------------------------------
PHI(10,11)==PHI(9,12);
PHI(9,12)==PHI(8,13);
%-------------------------------------------
PHI(4,10)==PHI(8,9);
PHI(5,9)==PHI(8,10);
PHI(6,8)==PHI(9,10);
%-------------------------------------------
%PHI(4,14)+PHI(5,14)+PHI(6,14)==PHI(11,11)+PHI(12,12)+PHI(13,13);
%-------------------------------------------
PHI(5,11)==PHI(8,12);
PHI(4,12)==PHI(8,11);
PHI(6,11)==PHI(9,13);
PHI(4,13)==PHI(9,11);
PHI(6,12)==PHI(10,13);
PHI(5,13)==PHI(10,12);

cvx_end

PHI=[PHI phi;phi' 1];
[PHI,D]=eig(PHI);
phi2=pow_p(D(end,end),.5)*PHI(:,end);

theta=[phi2(1:3)/scale1;sqrt(phi2(4:6))/scale2];

end

