function [theta,rpn]=DTDLocRfn(d,S,Q,theta0)
% theta=DTDLocRfn(theta0,d,S,Q)
%
% Algebraic refinement solution for the object and transmitter positions 
% by differential time delay (DTD) measurements.
%
% 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
%   theta0: (2K x 1), hypothesized value
%
% Output:
%   theta:  (2K x 1), object and transmitter position estimate
%   rpn:    number of repetitions needed
%
% 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
%

RpnMax = 3;    % refinement method iterations

[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;
    P=P12;
end;

% ----- Start -----
for i=1:M
    hd(i,:)=-(S(:,i)'*S(:,i))*(d(i)^2)+(1/4)*(d(i)^4);
    if (K==2)
        Gd(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
        Gd(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

theta=theta0;

% -- repeat several times --
for kk=1:RpnMax
    
    for i=1:M
        Bd(i,i)=-2*norm(theta(1:K)-S(:,i))*norm(theta(K+1:2*K)-S(:,i))*(d(i)-norm(theta(1:K)-theta(K+1:2*K)));
    end;
    
    iC=Bd*Q*Bd';
    u_est=theta(1:K);
    t_est=theta(K+1:2*K);
    
    Gdelta=[eye(K) eye(K);
        (2*theta(1:K)-3*theta(K+1:2*K))' (2*theta(K+1:2*K)-3*theta(1:K))';
        2*((theta(K+1:2*K)'*theta(K+1:2*K))*theta(1:K)'-(theta(1:K)'*theta(K+1:2*K))*theta(K+1:2*K)')  2*((theta(1:K)'*theta(1:K))*theta(K+1:2*K)'-(theta(1:K)'*theta(K+1:2*K))*theta(1:K)');
        (theta(1:K)-theta(K+1:2*K))'/norm(theta(1:K)-theta(K+1:2*K)) -(theta(1:K)-theta(K+1:2*K))'/norm(theta(1:K)-theta(K+1:2*K));
        eye(K)*(theta(K+1:2*K)'*theta(K+1:2*K)-theta(1:K)'*theta(K+1:2*K))+2*theta(K+1:2*K)*theta(1:K)'-(theta(1:K)+theta(K+1:2*K))*theta(K+1:2*K)' eye(K)*(theta(1:K)'*theta(1:K)-theta(1:K)'*theta(K+1:2*K))+2*theta(1:K)*theta(K+1:2*K)'-(theta(1:K)+theta(K+1:2*K))*theta(1:K)';
        eye(K)*norm(theta(1:K)-theta(K+1:2*K))+(theta(1:K)+theta(K+1:2*K))*(theta(1:K)-theta(K+1:2*K))'/norm(theta(1:K)-theta(K+1:2*K)) eye(K)*norm(theta(1:K)-theta(K+1:2*K))-(theta(1:K)+theta(K+1:2*K))*(theta(1:K)-theta(K+1:2*K))'/norm(theta(1:K)-theta(K+1:2*K));
        norm(theta(1:K)-theta(K+1:2*K))*theta(K+1:2*K)'+theta(1:K)'*theta(K+1:2*K)*(theta(1:K)-theta(K+1:2*K))'/norm(theta(1:K)-theta(K+1:2*K)) norm(theta(1:K)-theta(K+1:2*K))*theta(1:K)'-theta(1:K)'*theta(K+1:2*K)*(theta(1:K)-theta(K+1:2*K))'/norm(theta(1:K)-theta(K+1:2*K));
        diag(2*(theta(1:K)-theta(K+1:2*K)))-ones(K,1)*theta(K+1:2*K)' diag(2*(theta(K+1:2*K)-theta(1:K)))-ones(K,1)*theta(1:K)';
        2*(theta(1:K)-theta(K+1:2*K))'*P  -2*(theta(1:K)-theta(K+1:2*K))'*P];
    
    a_theta=[u_est+t_est;norm(u_est-t_est)^2-u_est'*t_est;(u_est'*u_est)*(t_est'*t_est)-(u_est'*t_est)^2;norm(u_est-t_est);(t_est'*t_est)*u_est+(u_est'*u_est)*t_est-(u_est+t_est)*(u_est'*t_est);...
        (u_est+t_est)*norm(u_est-t_est);norm(u_est-t_est)*(u_est'*t_est);(u_est-t_est).*(u_est-t_est)-ones(K,1)*(u_est'*t_est);(u_est-t_est)'*P*(u_est-t_est)];
    
    if (K==3)
        Gdelta=[Gdelta(1:end-1,:);
            2*(theta(1:K)-theta(K+1:2*K))'*P12  -2*(theta(1:K)-theta(K+1:2*K))'*P12;
            2*(theta(1:K)-theta(K+1:2*K))'*P13  -2*(theta(1:K)-theta(K+1:2*K))'*P13;
            2*(theta(1:K)-theta(K+1:2*K))'*P23  -2*(theta(1:K)-theta(K+1:2*K))'*P23];
        a_theta=[a_theta(1:end-1); (u_est-t_est)'*P12*(u_est-t_est); (u_est-t_est)'*P13*(u_est-t_est); (u_est-t_est)'*P23*(u_est-t_est)];
    end;
    
    h=hd-Gd*a_theta;
    G=-Gd*Gdelta;
%     d_theta=inv(G'*inv(W1)*G)*G'*inv(W1)*h;
    d_theta=(G'/iC*G)\(G'/iC*h);
    theta=theta-d_theta;
    if (norm(d_theta)<1e-3)
        break
    end  
end

rpn=kk;

end







