function [theta,thetaTilde]=DTDLoc(d,S,Q)
% [theta,thetaTilde]=DTDLoc(d,S,Q)
%
% Algebraic closed-form solution for the object and transmitter positions 
% by differential time delay (DTD) measurements in the absence of 
% transmitter position.
%
% Input:
%   d:      (M x 1), DTD measurements, M = number of sensors
%   S:      (K x M), sensor positions, K = localization dimension
%   Q:      (M x M), DTD covariance matrix 
%
% Output:
%   theta:  (2K x 1), object and transmitter position estimate
%   thetaTilde:  (2K x 1), stage-3 object and transmitter position estimate
%
% Reference:
% Y. Zhang and K. C. Ho, "Localization by signals of opportunity in the
% absence of transmitter position," IEEE Trans. Signal Process., vol. 72,
% pp. 4602-4617, 2022.
%
% Yang Zhang, K. C. Ho	10-18-2022
%
%       Copyright (C) 2022
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA
%       hod@missouri.edu
%

Rpn = 3; % iterations to update the weighting matrices

[K,M]=size(S);    % K = dimension, M = number of receivers

if (K==2)
    P=[0 1;1 0];
else
    P12=zeros(K); P12(1,2)=1; P12(2,1)=1;
    P13=zeros(K); P13(1,3)=1; P13(3,1)=1;
    P23=zeros(K); P23(2,3)=1; P23(3,2)=1;
end;

% ----- First stage setup -----
for i=1:M
    h1(i,:)=-(S(:,i)'*S(:,i))*(d(i)^2)+(1/4)*(d(i)^4);
    if (K==2)
        G1(i,:)=[-(d(i)^2)*S(:,i)' S(:,i)'*S(:,i)-(d(i)^2) 1 d(i)*((d(i)^2)-2*S(:,i)'*S(:,i)) -2*S(:,i)' ...
            2*d(i)*S(:,i)' -2*d(i) -(S(:,i).*S(:,i))' -S(1,i)*S(2,i)];
    else
        G1(i,:)=[-(d(i)^2)*S(:,i)' S(:,i)'*S(:,i)-(d(i)^2) 1 d(i)*((d(i)^2)-2*S(:,i)'*S(:,i)) -2*S(:,i)' ...
            2*d(i)*S(:,i)' -2*d(i) -(S(:,i).*S(:,i))' -S(1,i)*S(2,i) -S(1,i)*S(3,i) -S(2,i)*S(3,i)];
    end;
end

B1=eye(M);
W1=inv(B1*Q*B1');
alpha1=(G1'*W1*G1)\(G1'*W1*h1);

for k=1:Rpn
    % ----- Stage 1 -----
    if (k == 1) % use the first stage solution to update the weighting matrix W1
        for i=1:M
            B1(i,i)=(d(i)^2-2*d(i)*alpha1(K+3)-2*(alpha1(K+3)^2-alpha1(K+1))+2*S(:,i)'*alpha1(1:K)-2*S(:,i)'*S(:,i))*(d(i)-alpha1(K+3));
        end
    else % use the final stage solution to update the weighting matrix W1
        for i=1:M
            B1(i,i)=-2*norm(alpha3(1:K)-S(:,i))*norm(alpha3(K+1:2*K)-S(:,i))*(d(i)-norm(alpha3(1:K)-alpha3(K+1:2*K)));
        end
    end
    W1=inv(B1*Q*B1');
    alpha1=(G1'*W1*G1)\(G1'*W1*h1);
    
    %----- Stage 2 -----
    if (k == 1) % use the first stage solution to compute the weighting matrix W2
        B211=[eye(K) zeros(K,3); zeros(1,K) 1 0 -alpha1(K+3); zeros(1,K) 0 1 0; zeros(1,K) 0 0 1];
        B212=zeros(K+3,3*K+2);
        B221=[(alpha1(K+3)^2-alpha1(K+1))*eye(K) zeros(K,3); -alpha1(K+3)*eye(K) zeros(K,2) -alpha1(1:K);
            zeros(1,K) 0 0 -(alpha1(K+3)^2-alpha1(K+1));
            zeros(K,K+3); zeros(1,K) zeros(1,3)];
        B222=diag([ones(K,1); 2*ones(K,1); ones(K+2,1)]);
        B2=[B211 B212; B221 B222];
    else  % use the third stage solution to compute the weighting matrix W2
        B211=[eye(K) zeros(K,3); zeros(1,K) 1 0 -norm(alpha3(1:K)-alpha3(K+1:2*K)); zeros(1,K) 0 1 0; zeros(1,K) 0 0 1];
        B212=zeros(K+3,3*K+2);
        B221=[(alpha3(1:K)'*alpha3(K+1:2*K))*eye(K) zeros(K,3); -norm(alpha3(1:K)-alpha3(K+1:2*K))*eye(K) zeros(K,2) -(alpha3(1:K)+alpha3(K+1:2*K));
            zeros(1,K) 0 0 -(alpha3(1:K)'*alpha3(K+1:2*K));
            zeros(K,K+3); zeros(1,K) zeros(1,3)];
        B222=diag([ones(K,1); 2*ones(K,1); ones(K+2,1)]);
        B2=[B211 B212; B221 B222];
    end
    
    h2=[alpha1(1:2*K+3);2*alpha1(2*K+4:3*K+3);alpha1(3*K+4:end)];
    G2=[eye(K) zeros(K,2*K+4); zeros(1,2*K) alpha1(K+3) -1 zeros(1,2+K); zeros(1,2*K+2), 1, zeros(1,K+1);
        zeros(1,2*K) 1 zeros(1,K+3); zeros(K,2*K+1) -alpha1(1:K) zeros(K,1) eye(K) zeros(K,1);
        alpha1(K+3)*eye(K) zeros(K) alpha1(1:K) zeros(K,2) zeros(K) zeros(K,1);
        zeros(1,2*K+1) alpha1(K+3) zeros(1,K+2);
        zeros(K) eye(K) zeros(K,1) -ones(K,1) zeros(K,K+2);
        zeros(1,2*K+2) zeros(1,K+1) 1];
    
    if (K==3)
        B2=blkdiag(B2,eye(K-1));
        G2=blkdiag(G2,eye(K-1));
    end;
    
    %     W2=inv(B2*inv(G1'*W1*G1)*B2');
    iB2=inv(B2);
    W2=iB2'*(G1'*W1*G1)*iB2;
    alpha2=(G2'*W2*G2)\(G2'*W2*h2);
    
    % ----- Stage 3 -----
    if (K==2)   % 2D
        % -- recompute alpha2(K+1:2K) using other auxiliary variables if they are negative --
        if alpha2(3)<0 && alpha2(4)<0
            alpha2(3:4)=alpha1(11:12)+alpha2(6);
        end
        if alpha2(3)<0 && alpha2(4)>0
            alpha2(3)=(alpha2(7)/(2*sqrt(alpha2(4))))^2;
        end
        if alpha2(4)<0 && alpha2(3)>0
            alpha2(4)=(alpha2(7)/(2*sqrt(alpha2(3))))^2;
        end
    else        % 3D
        alpha2(4:6)=abs(alpha2(4:6));
    end;
    
    % -- determine sign --
    if (K==2)
        sgn=[1 1;1 -1;-1 1;-1 -1]; % account for ambiguities introduced by the square root operation
    else
        sgn=[1 1 1;1 1 -1;1 -1 1; 1 -1 -1;
            -1 1 1; -1 1 -1; -1 -1 1; -1 -1 -1];  % account for ambiguities introduced by the square root operation
    end;
    Gtheta=[eye(K) eye(K); eye(K) -eye(K)];
    
    for j=1:(2^K),
        if (k ==1) % use the second stage solution to compute the weighting matrix W3
            Bthetat(:,:,j) = [ eye(K) zeros(K) zeros(K,length(alpha2)-2*K); zeros(K) diag((0.5)./(sgn(j,:)'.*sqrt(abs(alpha2(K+1:2*K))))) zeros(K,length(alpha2)-2*K)];
        else % use the third stage solution to compute the weighting matrix W3
            Bthetat(:,:,j) = [ eye(K) zeros(K) zeros(K,length(alpha2)-2*K); zeros(K) diag((0.5)./(alpha3(1:K)-alpha3(K+1:2*K))) zeros(K,length(alpha2)-2*K)];
        end
        hthetat(:,j)=[alpha2(1:K); sgn(j,:)'.*sqrt(abs(alpha2(K+1:2*K)))];
        thetaTilde_cand(:,j)=inv(Gtheta)*hthetat(:,j);
        dRecon=sqrt(sum((repmat(thetaTilde_cand(1:K,j),1,M)-S).^2))'+norm(thetaTilde_cand(1:K,j)-thetaTilde_cand(K+1:2*K,j))-sqrt(sum((repmat(thetaTilde_cand(K+1:2*K,j),1,M)-S).^2))'; % reconstruct the measurements
        res(j)=(d-dRecon)'*inv(Q)*(d-dRecon); % compute the residual square errors
    end
    
    [~,idx]=min(res); % choose the minimum as the correct solution
    Btheta=Bthetat(:,:,idx);
    alpha3=thetaTilde_cand(:,idx);
end

alpha3 = [alpha3;alpha2(2*K+1:end)];
thetaTilde = alpha3(1:2*K); % the simple hypothesized solution

% ----- Stage 4 -----
P=ones(K,K)-eye(K);
B4=[eye(K) zeros(K,2*K+4); zeros(K,K) eye(K) zeros(K,K+4);
    (alpha3(K+1:2*K)-alpha3(1:K))' -(alpha3(K+1:2*K)-alpha3(1:K))' 2*alpha3(2*K+1) zeros(1,K+3);
    -alpha3(K+1:2*K)' -alpha3(1:K)' 0 2 zeros(1,K+2);
    -3*(norm(alpha3(K+1:2*K))^2)*alpha3(1:K)' -3*(norm(alpha3(1:K))^2)*alpha3(K+1:2*K)' 0 4*alpha3(2*K+2) 2 zeros(1,K) 0;
    -2*alpha3(K+1:2*K)*alpha3(1:K)' -2*alpha3(1:K)*alpha3(K+1:2*K)' zeros(K,3) eye(K) zeros(K,1);
    (alpha3(K+1:2*K)-alpha3(1:K))'*P -(alpha3(K+1:2*K)-alpha3(1:K))'*P 0 0 zeros(1,K+1) 1];
h4=[alpha3(1:2*K); alpha3(2*K+1)^2; 2*alpha3(2*K+2); 2*alpha3(2*K+2)^2+2*alpha3(2*K+3); alpha3(2*K+4:5*K)];
G4=[eye(K) zeros(K); zeros(K) eye(K);
    (alpha3(1:K)-alpha3(K+1:2*K))' -(alpha3(1:K)-alpha3(K+1:2*K))';
    alpha3(K+1:2*K)' alpha3(1:K)';
    (norm(alpha3(K+1:2*K))^2)*alpha3(1:K)' (norm(alpha3(1:K))^2)*alpha3(K+1:2*K)';
    (norm(alpha3(K+1:2*K))^2)*eye(K) (norm(alpha3(1:K))^2)*eye(K);
    (alpha3(1:K)-alpha3(K+1:2*K))'*P -(alpha3(1:K)-alpha3(K+1:2*K))'*P ];

if (K==3)
    [rw,cn]=size(B4);
    B4=[B4(1:rw-1,1:end), zeros(rw-1,K-1);
        (alpha3(K+1:2*K)-alpha3(1:K))'*P12*1 -(alpha3(K+1:2*K)-alpha3(1:K))'*P12*1 0 0 zeros(1,K+1) 1 0 0 ;
        (alpha3(K+1:2*K)-alpha3(1:K))'*P13*1 -(alpha3(K+1:2*K)-alpha3(1:K))'*P13*1 0 0 zeros(1,K+1) 0 1 0 ;
        (alpha3(K+1:2*K)-alpha3(1:K))'*P23*1 -(alpha3(K+1:2*K)-alpha3(1:K))'*P23*1 0 0 zeros(1,K+1) 0 0 1 ];
    G4=[ G4(1:end-1,:);
        (alpha3(1:K)-alpha3(K+1:2*K))'*P12*1  -(alpha3(1:K)-alpha3(K+1:2*K))'*P12*1;
        (alpha3(1:K)-alpha3(K+1:2*K))'*P13*1  -(alpha3(1:K)-alpha3(K+1:2*K))'*P13*1;
        (alpha3(1:K)-alpha3(K+1:2*K))'*P23*1  -(alpha3(1:K)-alpha3(K+1:2*K))'*P23*1;
        ];
end;

B22=[zeros(length(alpha3)-2*K,2*K), eye(length(alpha3)-2*K)];
B5=B4*[inv(Gtheta)*Btheta; B22];

iB5=inv(B5);
W4=iB5'*(G2'*W2*G2)*iB5;

theta=(G4'*W4*G4)\(G4'*W4*h4);  % solution

end





