function [QHat, tHat, qHat] = TOARBL_REF(A, C, r, Cov, Qinit, tinit)
%
%  [QHat, tHat, qHat] = TOARBL_REF(A, C, r, Cov, Qinit, tinit)
%
% This program refines 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.
% Qinit:    KxK, inital rotation matrix.
% tinit:    Kx1, inital translation vector.
%
% Output Parameter List:
% QHat:    KxK, refined rotation matrix Q.
% tHat:    Kx1, refined translation vector t.
%
%
% 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
dim = size(C,1);    %% dim of sensors

tinit2 = sum(tinit.^2);
C2 = sum(C.^2);
A2 = sum(A.^2); 

if dim == 2
    gama = [1,0,0,1]';
    lamda = [0,1,-1,0]';
else
    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 * A(:,m)' * tinit + A2(m) + C2(n));
        H(M*(n-1)+m,:) = 2 * kron(C(:,n)', tinit' * Qinit - A(:,m)' * Qinit);
        Omega(M*(n-1)+m,:) = 2 *( Qinit * C(:,n) + tinit - A(:,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 * Cov * B');

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

% compute the refined rotation matrix Q using corrected angle
if dim == 2
    % 2-D case
    theta = xx(1);
    costheta = cos(theta);
    sintheta = sin(theta);
else
    % 3-D case
    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 dim == 2
    % 2-D case
    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 case
    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

