function [QHat, tHat] = TOARBL_DAC(A, C, r, Cov)
%
%  [QHat, tHat] = TOARBL_DAC(A, C, r, Cov)
%
% This program generates the preliminary solution for rigid body 
% localization by TOA using the DAC approach.  The code works for
% 2-D and 3-D.  Please refer to the following paper for the details.
% 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:
% A:        KxM, anchor positions, K is localization dimension.
% C:        KxN, sensor positions local to the rigid body.
% r:        MxN, distance measurements.
% Cov:      MNxMN, covariance matrix of distance measurements.
%
% Output Parameter List:
% QHat:     KxK, rotation matrix Q estimate. 
% tHat:     Kx1, position vector t estimate. 
%
%
% Shanjie Chen, Benjian Hao, K. C. Ho    11-18-2019
%
%       Copyright (C) 2019
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA.
%       hod@missouri.edu
%

M = size(A,2); % number of anchors
N = size(C,2); % number of sensors

% obtain sensor positions
for n = 1:N
    rr = r(:,n);
    Sest(:,n) = TOALoc(A, rr, Cov( (n-1)*M+1:n*M, (n-1)*M+1:n*M ));  
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 vector t
tHat = meanSest - QHat * meanC;
