function theta=DSF2DLocMtTm_SDP(f,S,fo,c,T,Qn,Qs,Qfo,idc)
% theta=DSF2DLocMtTm_SDP(f,S,fo,c,T,Qn,Qs,Qfo,idc)
%
% SDP solution for the object location (position and velocity)
% in 2-D by using frequency measurements at successive instants.
%
% 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
%   T:      time interval between two consecutive measurements
%   Qn:     (MN x MN), measurement noise covariance matrix, N = number of instants
%   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);
N=length(f)/M;

try
    % -- initial solution without weighting matrix --
    theta=DSFLoc2DMtTm_SDPCore(f,S,fo,c,T,eye(N*M));
    
    % -- obtaining weighting matrix --
    if idc~=0
        d=c*(1-f/fo);
        u_all=theta(1:2)*ones(1,N)+theta(3:4)*(0:N-1)*T;
        r=sqrt(sum((kron(u_all,ones(1,M))-repmat(S,1,N)).^2,1));
        B1=2*c/fo*diag((d.*r'.^2));
        Ds1=fo/c*theta(3)./r-(f'-fo)./r.^2.*(kron(u_all(1,:),ones(1,M))-repmat(S(1,:),1,N));
        Ds2=fo/c*theta(4)./r-(f'-fo)./r.^2.*(kron(u_all(2,:),ones(1,M))-repmat(S(2,:),1,N));
        for m=0:N-1
            Dsa=Ds1(m*M+1:M+m*M);Dsa=diag(Dsa);
            Dsb=Ds2(m*M+1:M+m*M);Dsb=diag(Dsb);
            Dsc= Dsb(:,[1;1]*(1:size(Dsb,2)));Dsc(:,1:2:end) = Dsa;
            Ds(m*M+1:M+m*M,:)=Dsc;
        end
        Dfo=-f/fo;
        switch idc,
            case 1,
                Qs=0; Qfo=0;
            case 2,
                Qfo=0;
            case 3,
                Qs=0;
        end;
        iW1=B1*(Ds*Qs*Ds'+Dfo*Qfo*Dfo'+Qn)*B1';
        iW1=iW1/max(iW1(:));
        
        % -- final solution --
        theta=DSFLoc2DMtTm_SDPCore(f,S,fo,c,T,iW1);
        
    end
    
catch
    theta=ones(4,1)*nan;
end

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

% ==================================================================
function theta=DSFLoc2DMtTm_SDPCore(f,S,fo,c,T,iW)
% theta=DSFLoc2DMtTm_SDPCore(f,S,fo,c,T,iW)
%
% SDP main processing routine for the object localization (position and
% velocity) in 2-D by using frequency measurements at succesive instants.
%
% 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
%   T:      time interval between two consecutive measurements
%   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	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);
N=length(f)/M;

d=c*(1-f/fo);
sc1=1.75e-2;    % scaling factor for d
sc2=1e-3;       % scaling factor for sensor positions
S=S.*sc2;
T=T*sc2/sc1;
d=d*sc1;
%============================================================
d2=d.^2;
g=(zeros(M*N,1));
A=(zeros(N*M,16));
for k=1:M*N
    i=mod(k+M-1,M)+1;
    n=fix((k-1)/M);
    g(k,1)=d2(k)*(S(:,i)'*S(:,i));
    A(k,:)=[2*d2(k)*S(:,i)' 2*n*T*d2(k)*S(:,i)' -d2(k) -2*n*T*d2(k) S(1,i)^2-n^2*T^2*d2(k) S(2,i)^2-n^2*T^2*d2(k) 2*S(1,i)*S(2,i) -2*S(:,i)' -2*n*T*S(:,i)' 1 n^2*T^2 2*n*T];
end

%==================================
% 	CVX solution
%==================================

%  cvx_solver sedumi
cvx_solver sdpt3
cvx_precision best
cvx_begin sdp quiet;
variable Z(16,16) symmetric;
variable z(16);
minimize(trace(A'/iW*A*Z)-2*(A*z)'/iW*g)
subject to
%
[Z z;z' 1]==semidefinite(17);
%

z(5)>=0;z(7)>=0;z(8)>=0;z(14);z(15)>=0;
Z(5,7)>=0;Z(5,8)>=0;Z(5,14)>=0;Z(5,15)>=0;
Z(7,8)>=0;Z(7,14)>=0;Z(7,15)>=0;
Z(8,14)>=0;Z(8,15)>=0;
Z(14,15)>=0;

Z(3,8)==Z(4,9)
Z(3,9)==Z(4,7)
Z(3,10)==Z(6,7)
Z(3,12)==Z(7,7)+Z(7,8)
Z(3,15)==Z(7,12)+Z(8,12)
Z(3,16)==Z(7,10)+Z(8,10)
Z(4,11)==Z(6,8)
Z(4,13)==Z(7,8)+Z(8,8)
Z(4,15)== Z(7,13)+ Z(8,13)
Z(4,16)==Z(7,11)+Z(8,11)
z(5)==Z(1,1)+Z(2,2)
z(6)==Z(1,3)+Z(2,4)
Z(6,15)==Z(10,12)+Z(11,13)
Z(6,15)==Z(7,16)+Z(8,16)
Z(6,16)==Z(7,14)+Z(8,14)
z(7)==Z(3,3)
Z(7,8)==Z(9,9)
Z(7,14)==Z(10,10)
Z(7,15)==Z(12,12)
Z(7,16)==Z(10,12)
z(8)==Z(4,4)
Z(8,14)==Z(11,11)
Z(8,15)==Z(13,13)
Z(8,16)==Z(11,13)
z(9)==Z(3,4)
Z(9,14)==Z(10,11)
Z(9,15)==Z(12,13)
Z(9,16)==Z(10,13)
Z(9,16)==Z(11,12)
z(10)==Z(3,6)
z(10)==Z(1,7)+Z(2,9)
Z(10,15)==Z(12,16)
Z(10,16)==Z(12,14)
z(11)==Z(4,6)
z(11)==Z(2,8)+Z(1,9)
Z(11,15)==Z(13,16)
Z(11,16)==Z(13,14)
z(12)==Z(3,7)+Z(3,8)
z(12)==Z(3,7)+Z(4,9)
z(13)==Z(4,7)+Z(4,8)
z(13)==Z(4,8)+Z(3,9)
z(14)==Z(6,6)
z(14)==Z(1,10)+Z(2,11);
Z(14,15)==Z(16,16)
z(15)==Z(3,12)+Z(4,13)
z(15)==Z(7,7)+Z(8,8)+2*Z(9,9)
z(16)==Z(6,7)+Z(6,8)
z(16)==Z(1,12)+Z(2,13)
z(16)==Z(3,10)+Z(4,11)
Z(6,7)+Z(6,8)==Z(3,10)+Z(4,11)

cvx_end

Z=[Z z;z' 1];
[Z,D]=eig(Z);
z2=pow_p(D(end,end),.5)*Z(:,end);
theta=[z2(1:2)/sc2;z2(3:4)/sc1];
if isnan(cvx_optval)
    theta=ones(4,1)*nan;
end

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

