function [theta,rpn]=DTDTDOALOcRfn(m,S,Q,theta0)
% [theta,rpn]=DTDTDOALOcRfn(m,S,Q,theta0)
%
% Algebraic closed-form refinement solution for the object position by 
% differential time delay (DTD) and TDOA measurements in the absence of 
% transmitter position.
%
% 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 
%   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 max. number of repetitions

[K,M]=size(S);    % K = dimension, M = number of receivers
                   
P=[0 1;1 0];
r_do = m(1:M);
rho = 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);
H=[ones(M-1,1),-eye(M-1)];
h = [1 zeros(1,M-1)];

Qerr = [h*Q1*h' h*Q1*H'+h*Q2 h*Q2;
        H*Q1*h'+Q2'*h' H*Q1*H'+H*Q2+Q2'*H'+Q3 H*Q2+Q3;
        Q2'*h' Q2'*H'+Q3 Q3];
    
% -- define G1 and h1 as in Stage 1 of the clsoed-form solutin --
hr_d(1,:) = r_do(1);
Gr_d(1,:) = [zeros(1,K) zeros(1,K) 1 -1 1];
for i=2:M
    hr_d(i,:)=(r_do(1)+rho(i-1)-r_do(i))^2+(S(:,1)'*S(:,1))-(S(:,i)'*S(:,i));
    Gr_d(i,:)=[zeros(1,K) 2*(S(:,1)-S(:,i))' 0 -2*(r_do(1)+rho(i-1)-r_do(i)) 0];
end

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

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

theta=theta0;

% ----- repeat for better accuracy -----
for kk=1:RpnMax
    B1(1,1) = 1;
    for i=2:M
        B1(i,i)=2*norm(theta(K+1:2*K)-S(:,i));
        B1(M+i-1,M+i-1)=2*norm(theta(1:K)-S(:,i));
    end
    
    W1=B1*Qerr*B1';
    u_est=theta(1:K);
    t_est=theta(K+1:2*K);

    Gdelta=[eye(2*K);
        (theta(1:K)-S(:,1))'/norm(theta(1:K)-S(:,1)) zeros(1,K); 
        zeros(1,K) (theta(K+1:2*K)-S(:,1))'/norm(theta(K+1:2*K)-S(:,1));
        (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))];

    a_theta=[u_est;t_est;norm(u_est-S(:,1));norm(t_est-S(:,1));norm(u_est-t_est)];

    h=h1-G1*a_theta;
    G=-G1*Gdelta;
    d_theta=inv(G'*inv(W1)*G)*G'*inv(W1)*h;
    theta=theta-d_theta;
    if norm(d_theta)<1e-3
        break
    end
    
end

rpn=kk;

end









