function [QHat, tHat, qHat] = TOARBL_REF_aNsy(r, AHat, C, Q_r, Q_a, Qinit, tinit)
%
%  [QHat, tHat, qHat] = TOARBL_REF_aNsy(r, AHat, C, Q_r, Q_a, Qinit, tinit)
%
% This program implements the refinement 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.
% AHat:     KxM, estimated anchor positions, K is localization dimension.
% C:        KxN, sensor positions local to the rigid body.
% Q_r:      MNxMN, covariance matrix of range measurements.
% Q_a:      KMxKM, covariance matrix of anchor positions.
% Qinit:    KxK, inital rotation matrix.
% tinit:    Kx1, inital translation vector.
%
% Output Parameter List:
% QHat:     KxK, rigid body rotation matrix.
% tHat:     Kx1, rigid body position.
% qHat:     (2K-3)x1, rotation angle vector, in degrees.
%
% 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(AHat,2);   % number of anchors
N = size(C,2);      % number of sensors
K = size(C,1);      % dim of sensors

tinit2 = sum(tinit.^2);
C2 = sum(C.^2);
AHat2 = sum(AHat.^2); 

if (K == 2)
    % 2-D
    gama = [1,0,0,1]';
    lamda = [0,1,-1,0]';
else
    % 3-D
    gama = [1,0,0,0,1,0,0,0,1]';
    lamda = [0,0,0;0,0,-1;0,1,0;0,0,1;0,0,0;-1,0,0;0,-1,0;1,0,0;0,0,0];
end

for n = 1:N
    for m = 1:M
        ddot(M*(n-1)+m,1) = r(m,n)^2 - ( tinit2 - 2 * AHat(:,m)' * tinit + AHat2(m) + C2(n));
        H(M*(n-1)+m,:) = 2 * kron(C(:,n)', tinit' * Qinit - AHat(:,m)' * Qinit);
        Omega(M*(n-1)+m,:) = 2 * (kron(C(:,n)',Qinit) * gama + tinit - AHat(:,m))';
        Fai(M*(n-1)+m,:) = [zeros(1,K*(m-1)),-Omega(M*(n-1)+m,:),zeros(1,K*(M-m))];
    end
end

G = [H * lamda, Omega];
h = ddot - H * gama;

% distance weighting matrix
rr =  sqrt(r(:).^2);
B = 2 * diag(rr);
W = inv(B * Q_r * B' + Fai * Q_a * Fai');

% correcting angle and translation from LS
xx = inv(G' * W * G) * G' * W * h;

% compute the refined rotation matrix Q using correcting angle
if (K == 2)
    % 2-D
    theta = xx(1);
    costheta = cos(theta);
    sintheta = sin(theta);
else
    % 3-D
    phi = xx(1);
    theta = xx(2);
    psi = xx(3);
    cosphi = cos(phi);
    sinphi = sin(phi);
    costheta = cos(theta);
    sintheta = sin(theta);
    cospsi = cos(psi);
    sinpsi = sin(psi);
end

if (K == 2)
    % 2-D
    Qdelta = [costheta, -sintheta; sintheta, costheta];
    tHat = tinit + xx(2:3);
    QHat = Qinit * Qdelta;
    thetaHat = atan2d(QHat(2,1),QHat(1,1));
    qHat = [thetaHat]';
else
    % 3-D
    Qdelta = [costheta * cospsi, costheta * sinpsi, -sintheta;...
        sinphi * sintheta * cospsi - cosphi * sinpsi, sinphi * sintheta * sinpsi + cosphi * cospsi, costheta*sinphi;...
        cosphi * sintheta * cospsi + sinphi * sinpsi, cosphi * sintheta * sinpsi - sinphi * cospsi, costheta*cosphi];
    tHat = tinit + xx(4:6);
    QHat = Qinit * Qdelta;
    alphaHat = atan2d(QHat(3,2),QHat(3,3));
    gammaHat = atan2d(QHat(2,1),QHat(1,1));
    betaHat = atan2d(-QHat(3,1),sqrt(QHat(3,3)^2+QHat(3,2)^2));
    qHat = [alphaHat, betaHat, gammaHat]';
end

