function theta=DSF3DTMA_SDP(f,fc,S,N,c,T,Qn,Qfc,Qs,wghIdc)
%theta=DSF3DTMA_SDP(f,fc,S,N,c,T,Qn,Qs,Qfc,wghIdc)
%
% SDP solution by SDR for obtaining the initial object position and
% velocity for TMA.
%
% 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.
% N:      scalar, number of time measurements.
% c:      scalar, signal propagation speed.
% T:      scalar, time measurement interval.
% Qn:     (MN x MN), 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);
[theta0,~]=sdp(f,fc,S,N,c,T,eye(N*M));

if wghIdc~=0
    iW=DSF3DTMA_Weight(f,fc,S,N,c,T,theta0(1:3),theta0(4:6),Qn,Qfc,Qs,wghIdc);
    [theta,opt_val]=sdp(f,fc,S,N,c,T,iW);
    if isnan(opt_val)
        theta=theta0;
    end
end

end

% =========================================================================
function [theta,opt_val]=sdp(f,fc,S,N,c,T,iW)
% [theta,opt_val]=sdp(f,fc,S,N,c,T,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.
% N:      scalar, number of time measurements.
% c:      scalar, signal propagation speed.
% T:      scalar, time measurement interval.
% 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-28-2022
%
%       Copyright (C) 2022
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA.
%       hod@missouri.edu
%

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

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;%2e-3;
scale2=1/norm(r,inf);%1e-1;
S=S.*scale1;
T=T*scale1/scale2;
r=r*scale2;
r2=r.^2;
g=(zeros(M*N,1));
A=(zeros(N*M,23));
for k=1:M*N
    i=mod(k+M-1,M)+1;
    n=fix((k-1)/M);
    g(k,1)=r2(k)*(S(i,:)*S(i,:)');
    A(k,:)=[2*r2(k)*S(i,:) 2*n*T*r2(k)*S(i,:) -r2(k) -2*n*T*r2(k) -((n*T)^2*r2(k)-S(i,1)^2) -((n*T)^2*r2(k)-S(i,2)^2) -((n*T)^2*r2(k)-S(i,3)^2) 2*S(i,1)*S(i,2) 2*S(i,1)*S(i,3) 2*S(i,2)*S(i,3)  -2*S(i,:) -2*n*T*S(i,:) (n*T)^2 2*n*T 1];
end

%  -- CVX solution --
%cvx_solver SeDuMi
cvx_solver sdpt3
%cvx_solver Mosek
cvx_precision best
cvx_begin sdp quiet;
variable PHI(23,23) symmetric;
variable phi(23);
minimize(trace(A'/iW*A*PHI)-2*(A*phi)'/iW*g)
subject to
%-------------------------------------------
[PHI phi;phi' 1]==semidefinite(24);
%-------------------------------------------
%{
phi(9)>=0;phi(10)>=0;phi(11)>=0;phi(21);phi(23)>=0;
PHI(9,10)>=0;PHI(9,11)>=0;PHI(9,12)>=0;PHI(9,21)>=0;PHI(9,23)>=0;
PHI(10,11)>=0;PHI(10,21)>=0;PHI(10,23)>=0;
PHI(11,21)>=0;PHI(11,23)>=0;
PHI(21,23)>=0;
%}
%-------------------------------------------
phi(9)==PHI(4,4);
phi(10)==PHI(5,5);
phi(11)==PHI(6,6);
phi(12)==PHI(4,5);
phi(13)==PHI(4,6);
phi(14)==PHI(5,6);
phi(15)==PHI(4,8);
phi(16)==PHI(5,8);
phi(17)==PHI(6,8);
phi(23)==PHI(8,8);
%-------------------------------------------
phi(7)==PHI(1,1)+PHI(2,2)+PHI(3,3);
phi(8)==PHI(1,4)+PHI(2,5)+PHI(3,6);
phi(15)==PHI(1,9)+PHI(2,12)+PHI(3,13);
phi(16)==PHI(1,12)+PHI(2,10)+PHI(3,14);
phi(17)==PHI(1,13)+PHI(2,14)+PHI(3,11);
phi(18)==PHI(4,9)+PHI(5,12)+PHI(6,13);
phi(19)==PHI(4,12)+PHI(5,10)+PHI(5,11);
phi(20)==PHI(4,13)+PHI(5,14)+PHI(6,11);
phi(21)==PHI(4,18)+PHI(5,19)+PHI(6,20);
phi(22)==PHI(1,18)+PHI(2,19)+PHI(3,20);
phi(22)==PHI(4,15)+PHI(5,16)+PHI(6,17);
phi(23)==PHI(1,15)+PHI(2,16)+PHI(3,17);
%-------------------------------------------
PHI(4,10)==PHI(5,12);
PHI(4,11)==PHI(6,13);
PHI(4,12)==PHI(5,9);
PHI(4,13)==PHI(6,9);
PHI(4,14)==PHI(5,13);
PHI(4,15)==PHI(8,9);
PHI(4,16)==PHI(5,15);
PHI(4,17)==PHI(6,15);
PHI(4,19)==PHI(5,18);
PHI(4,20)==PHI(6,18);
PHI(4,22)==PHI(8,18);
PHI(4,23)==PHI(8,15);
PHI(5,11)==PHI(6,14);
PHI(5,13)==PHI(6,12);
PHI(5,14)==PHI(6,10);
PHI(5,15)==PHI(8,12);
PHI(5,16)==PHI(8,10);
PHI(5,17)==PHI(6,16);
PHI(5,20)==PHI(6,19);
PHI(5,22)==PHI(8,19);
PHI(5,23)==PHI(8,16);
PHI(6,15)==PHI(8,13);
PHI(6,16)==PHI(8,14);
PHI(6,17)==PHI(8,11);
PHI(6,22)==PHI(8,20);
PHI(6,23)==PHI(8,17);
PHI(9,10)==PHI(12,12);
PHI(9,11)==PHI(13,13);
PHI(9,14)==PHI(12,13);
PHI(9,16)==PHI(12,15);
PHI(9,17)==PHI(13,15);
PHI(9,19)==PHI(12,18);
PHI(9,20)==PHI(13,18);
PHI(9,21)==PHI(18,18);
PHI(9,22)==PHI(15,18);
PHI(9,23)==PHI(15,15);
PHI(10,11)==PHI(14,14);
PHI(10,13)==PHI(12,14);
PHI(10,15)==PHI(12,16);
PHI(10,17)==PHI(14,16);
PHI(10,18)==PHI(12,19);
PHI(10,20)==PHI(14,19);
PHI(10,21)==PHI(19,19);
PHI(10,22)==PHI(16,19);
PHI(10,23)==PHI(16,16);
PHI(11,12)==PHI(13,14);
PHI(11,15)==PHI(13,17);
PHI(11,16)==PHI(14,17);
PHI(11,18)==PHI(13,20);
PHI(11,19)==PHI(14,20);
PHI(11,21)==PHI(20,20);
PHI(11,22)==PHI(17,20);
PHI(11,23)==PHI(17,17);
PHI(12,17)==PHI(13,16);
PHI(12,20)==PHI(13,19);
PHI(12,21)==PHI(18,19);
PHI(12,22)==PHI(15,19);
PHI(12,23)==PHI(15,16);
PHI(13,16)==PHI(14,15);
PHI(13,19)==PHI(14,18);
PHI(13,21)==PHI(18,20);
PHI(13,22)==PHI(15,20);
PHI(13,23)==PHI(15,17);
PHI(14,21)==PHI(19,20);
PHI(14,22)==PHI(16,20);
PHI(14,23)==PHI(16,17);
PHI(15,19)==PHI(16,18);
PHI(15,20)==PHI(17,18);
PHI(15,21)==PHI(18,22);
PHI(15,22)==PHI(18,23);
PHI(16,20)==PHI(17,19);
PHI(16,21)==PHI(19,22);
PHI(16,22)==PHI(19,23);
PHI(17,21)==PHI(20,22);
PHI(17,22)==PHI(20,23);
PHI(21,23)==PHI(22,22);
%-------------------------------------------
%{
PHI(4,8)==PHI(1,9)+PHI(2,12)+PHI(3,13);
PHI(4,18)==PHI(9,9)+PHI(12,12)+PHI(13,13);
PHI(4,19)==PHI(9,12)+PHI(10,12)+PHI(13,14);
PHI(4,20)==PHI(9,13)+PHI(10,13)+PHI(11,13);
PHI(4,21)==PHI(9,18)+PHI(10,18)+PHI(13,20);
PHI(4,22)==PHI(9,15)+PHI(10,15)+PHI(13,17);
PHI(5,8)==PHI(1,12)+PHI(2,10)+PHI(3,14);
PHI(5,18)==PHI(9,12)+PHI(10,12)+PHI(13,14);
PHI(5,19)==PHI(9,10)+PHI(10,10)+PHI(14,14);
PHI(5,19)==PHI(10,11)+PHI(10,10)+PHI(12,12);
PHI(5,20)==PHI(10,14)+PHI(11,14)+PHI(12,13);
PHI(5,21)==PHI(9,19)+PHI(10,19)+PHI(14,20);
PHI(5,21)==PHI(10,19)+PHI(11,19)+PHI(12,18);
PHI(5,22)==PHI(9,16)+PHI(10,16)+PHI(11,16);
PHI(6,8)==PHI(1,13)+PHI(2,14)+PHI(3,11);
PHI(6,18)==PHI(9,13)+PHI(10,13)+PHI(11,13);
PHI(6,19)==PHI(10,14)+PHI(11,14)+PHI(12,13);
PHI(6,20)==PHI(9,11)+PHI(10,11)+PHI(11,11);
PHI(6,20)==PHI(11,11)+PHI(13,13)+PHI(14,14);
PHI(6,21)==PHI(9,20)+PHI(10,20)+PHI(11,20);
PHI(6,21)==PHI(11,20)+PHI(13,18)+PHI(14,19);
PHI(6,22)==PHI(9,17)+PHI(10,17)+PHI(11,17);
PHI(6,22)==PHI(11,17)+PHI(13,15)+PHI(14,16);
PHI(8,8)==PHI(1,15)+PHI(2,16)+PHI(3,17);
PHI(8,18)==PHI(9,15)+PHI(10,15)+PHI(13,17);
PHI(8,19)==PHI(9,16)+PHI(10,16)+PHI(14,17);
PHI(8,19)==PHI(10,16)+PHI(11,16)+PHI(12,15);
PHI(8,20)==PHI(9,17)+PHI(10,17)+PHI(11,17);
PHI(8,21)==PHI(9,22)+PHI(10,22)+PHI(17,20);
PHI(8,21)==PHI(10,22)+PHI(11,22)+PHI(15,18);
PHI(8,21)==PHI(15,18)+PHI(16,19)+PHI(17,20);
PHI(8,22)==PHI(9,23)+PHI(10,23)+PHI(17,17);
PHI(8,22)==PHI(10,23)+PHI(11,23)+PHI(15,15);
PHI(8,22)==PHI(15,15)+PHI(16,16)+PHI(17,17);
%}
%-------------------------------------------
cvx_end
%-------------------------------------------
PHI=[PHI phi;phi' 1];
[PHI,D]=eig(PHI);
z2=pow_p(D(end,end),.5)*PHI(:,end);
theta=[z2(1:3)/scale1;z2(4:6)/scale2];
opt_val=cvx_optval;
if isnan(opt_val)
    theta=zeros(6,1);
end

end

