function [u,flag] = TmLocSenMtnEft_SDR(m,t,v,Q,c,u0)
% [u,flag] = TmLocSenMtnEft_SDR(m,t,v,Q,c,u0)
%
% SDR solution for time delay localization with sensor motion taken
% into consideration.
% 
% input parameter list:  
% m:     (N x 1), range measurement vector, N = num. of measurements.
% t:     (Dim x N), sensor positions when signal sending out, N = sensor num.
% v:     (Dim x N), sensor velocities.
% Q:     (N x N), covariance matrix of range measurements.
% c:     signal propagation speed.
% u0:    initial guess for object position.
%
% Output parameter list:
% u:     (Dim x 1), object location estimate.
% flag:  scalar, flag=1 indicating valid solution.
%
% The program can be used for 2-D(Dim=2) or 3-D(Dim=3) localization directly.
%
% Reference:
% T. Jia, K. C. Ho, H. Wang, and X. Shen, "Effect of sensor motion on time 
% delay and Doppler shift localization: analysis and solution," IEEE Trans. 
% Signal Process., vol. 67, no. 22, pp. 5881-5895, Nov. 2019.
%
% Tianyi Jia and K. C. Ho   12-26-2019
%
%       Copyright (C) 2019
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA
%       hod@missouri.edu
%

[K,N]=size(t);
if (K==2) Lgth=5; else Lgth=9; end;

flag=0;                 % SDP flag

%% Construct the vectors and matrices
a=zeros(N,1);   % define as a*m*c-b
b=zeros(N,1);   % define as a*m*c-b
p=zeros(N,1);
h=zeros(N,1);
B=zeros(N,N);
G=zeros(N,Lgth);

for i=1:N
    a(i)=(1-v(:,i)'*v(:,i)/c^2)/2;
    b(i)=v(:,i)'*t(:,i)/c;
    p(i)=a(i)*m(i)*c-b(i);
    h(i)=(p(i))^2-t(:,i)'*t(:,i);
       
    if (K==2)   % 2D
        G(i,1:2)=-2*(t(:,i)+p(i)/c*v(:,i))';
        G(i,3:5)=[1 1 0]-[v(1,i)^2 v(2,i)^2 2*v(1,i)*v(2,i)]/c^2;
    else    % 3D
        G(i,1:3)=-2*(t(:,i)+p(i)/c*v(:,i))';
        G(i,4:6)=[1 1 1]-(v(:,i)').^2/c^2;
        G(i,7:9)=-2*[v(1,i)*v(2,i) v(1,i)*v(3,i) v(2,i)*v(3,i)]/c^2;
    end;
    
    if u0==0
        B(i,i)=c^2*m(i)/2;
    else
        B(i,i)=2*p(i)*a(i)*c+2*a(i)*v(:,i)'*u0;   %B(i,i) can be approximated as a constant c^2*m(i)/2; 
    end
end

W=inv(B*Q*B');      % approximated weighting matrix

%% SDP 
cvx_solver sdpt3   %
cvx_precision best %cvx_precision low %low,defaul
cvx_begin SDP
cvx_begin quiet;
variable X(Lgth,Lgth) symmetric
variable x(Lgth);  % (SDP)
if (K==2)   % 2D
    minimize(trace(G'*W*G*X)-2*h'*W*G*x);
    subject to
    x(3)==X(1,1);
    x(4)==X(2,2);
    x(5)==X(1,2);
    [X x; x' 1]==semidefinite(Lgth+1);   
else
    minimize(trace(G'*W*G*X)-2*h'*W*G*x);
    subject to
    x(4)==X(1,1);
    x(5)==X(2,2);
    x(6)==X(3,3);
    x(7)==X(1,2);
    x(8)==X(1,3);
    x(9)==X(2,3);
    [X x; x' 1]==semidefinite(Lgth+1);   
end;

cvx_end

if length(cvx_status)==6    % cvx_status='solved'
    flag(1)=1;
end

%% retrieve solution
[V,D1]=eig([X x; x' 1]);
[~,IX]=sort(diag(D1));
hat_a=V(:,IX(end));
hat_a=hat_a/hat_a(end);
u=hat_a(1:K);