function [QHat, tHat, AHat, SEst] = TOARBLAncErrCalSrc_eDAC(r, ANsy, C, d, pNsy, Q_r, Q_a, Q_d, Q_p)
%
%  [QHat, tHat, AHat, SEst] = TOARBLAncErrCalSrc_eDAC(r, ANsy, C, d, pNsy, Q_r, Q_a, Q_d, Q_p)
%
% This program implements the DAC solution for rigid body
% localization using TOA measurements in the presence of anchor position 
% errors with a noisy calibration emitter.   The code is applicable for 2-D 
% and 3-D localizations.
% Details of the algorithm development can be found in
% Benjian Hao, K. C. Ho and Zan Li,  "Range based rigid body localization 
% with a calibration emitter for mitigating anchor position uncertainties," 
% IEEE Trans. Wireless Commun., vol. 18, no. 12, pp. 5734-5748, Dec. 2019.
%
% Shenjie Chen and K. C. Ho, "Accurate localization of a rigid body 
% using multiple sensors and landmarks,"  IEEE Trans. Signal Process., 
% vol. 63, no. 24, pp. 6559-6472, Dec. 2015.
%
% Input Parameter List:
% r:        MxN, range (TOA) measurements, M is the num. of anchors, N is the number of sensors.
% ANsy:     KxM, noisy anchor positions, K is localization dimension.
% C:        KxN, sensor positions local to the rigid body.
% d:        Mx1, range (TOA) measurements for the calibration emitter.
% pNsy:     Kx1, noisy calibration emitter position.
% Q_r:      MNxMN, covariance matrix of range measurements.
% Q_a:      KMxKM, covariance matrix of anchor positions.
% Q_d:      MxM, covariance matrix of calibration emitter range measurements.
% Q_p:      KxK, covariance matrix of calibration emitter position.
%
% Output Parameter List:
% QHat:     KxK, rigid body rotation matrix.
% tHat:     Kx1, rigid body position.
% AHat:     KxM, refined anchor positions.
% SEst:     KxN, estimated sensor positions in global frame.
%
% Benjian Hao, K. C. Ho    12-31-2019
%
%       Copyright (C) 2019
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA.
%       hod@missouri.edu
%

M = size(ANsy,2);   % number of anchors
N = size(C,2);      % number of sensors
K = size(ANsy,1);   % number of anchors
A_times = 2;
% stage1: update the anchors position using a calibration source ca
AHat = ANsy;
% AHat = a;
nnn = 1;
while nnn < A_times
    A_tep = AHat;
    AHat = TOAAncUpd_pNsy(AHat, pNsy, d, Q_d, Q_a, Q_p);
    A_error(nnn) = norm(reshape(A_tep-AHat, K*M,1),2)^2;
    if A_error(nnn) < 10e-12
       A_times = nnn;
    end
    nnn = nnn +1;
end

% stage 2 and 3: obtain the sensor position
for n = 1:N
    rr = r(:,n);
    SEst(:,n) = TOALoc_pNsy(rr, AHat, pNsy, Q_r((n-1)*M+1:n*M, (n-1)*M+1:n*M), Q_a, Q_d, Q_p);  
end


% from SEst and C, estimate Q and t according to OPP
meanC = mean(C')';
meanSEst = mean(SEst')';

C_rel= C - meanC*ones(1,N);
SEst_rel = SEst - meanSEst*ones(1,N);
[U Sigma V]= svd(C_rel * SEst_rel');

% rotation matrix Q
SigmaModified = diag( [ones(size(Sigma,1)-1,1); sign(det(V*U'))] );% ensure det Q = 1
QHat = V * SigmaModified * U';

% translation t
tHat = meanSEst - QHat * meanC;

