function [theta,phi]=DTDTDOALoc(m,S,Q)
% [theta,phi]=DTDTDOALoc(m,S,Q)
%
% Algebraic closed-form solution for the object and transmitter positions
% by differential time delay (DTD) and TDOA measurements.
%
% Input:
%   m:      ((2M-1) x 1), DTD & TDOA 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
%   phi:    ((2K+3) x 1), first stage solution
%
% 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
%

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

Rpn = 2; % iterations to update the weighting matrices
d = m(1:M);
r = m(M+1:end);
Q1 = Q(1:M,1:M);
Q2 = Q(1:M, M+1:end);
Q3 = Q(M+1:end, M+1:end);
Hr=[ones(M-1,1),-eye(M-1)];
h = [1 zeros(1,M-1)];

P = [h zeros(1,M-1);
    Hr eye(M-1);
    zeros(M-1,M) eye(M-1)];

Qerr = P*Q*P';

% ----- Stage 1 -----
hr_d(1,:) = d(1);
Gr_d(1,:) = [zeros(1,K) zeros(1,K) 1 -1 1];
for i=2:M
    hr_d(i,:)=(d(1)+r(i-1)-d(i))^2+(S(:,1)'*S(:,1))-(S(:,i)'*S(:,i));
    Gr_d(i,:)=[zeros(1,K) 2*(S(:,1)-S(:,i))' 0 -2*(d(1)+r(i-1)-d(i)) 0];
end

for i=1:M-1
    hrh(i,:)=(r(i))^2+(S(:,1)'*S(:,1))-(S(:,i+1)'*S(:,i+1));
    Grh(i,:)=[2*(S(:,1)-S(:,i+1))' zeros(1,K) -2*(r(i)) 0 0];
end

G1 = [Gr_d;Grh];
h1 = [hr_d;hrh];

B1=eye(2*M-1);
W1=inv(B1*Qerr*B1');
phi=inv(G1'*(W1)*G1)*G1'*(W1)*h1;

for k=1:Rpn,

    B1(1,1) = 1;
    for i=2:M
        B1(i,i)=2*norm(phi(K+1:2*K)-S(:,i));
        B1(M+i-1,M+i-1)=2*norm(phi(1:K)-S(:,i));
    end

    W1=inv(B1*Qerr*B1');
    phi=(G1'*(W1)*G1)\(G1'*(W1)*h1);
end

% ----- Stage 2 -----
B2=[eye(2*K) zeros(2*K, 3);
    -phi(1:K)' zeros(1,K) 2*phi(2*K+1) 0 0;
    zeros(1,K) -phi(K+1:2*K)' 0 2*phi(2*K+2) 0;
    (phi(K+1:2*K)-phi(1:K))' -(phi(K+1:2*K)-phi(1:K))' 0 0 2*phi(2*K+3)];

h2=[phi(1:2*K);phi(2*K+1)^2-S(:,1)'*S(:,1);phi(2*K+2)^2-S(:,1)'*S(:,1);phi(2*K+3)^2];

G2=[eye(2*K);
    (phi(1:K)-2*S(:,1))' zeros(1,K);
    zeros(1,K) (phi(K+1:2*K)-2*S(:,1))';
    (phi(1:K)-phi(K+1:2*K))' -(phi(1:K)-phi(K+1:2*K))'];

W2=inv(B2*inv(G1'*W1*G1)*B2');
theta=(G2'*(W2)*G2)\(G2'*(W2)*h2);

end





