function CRLB = AOARBL3D_CRLB(Ain, Cin, Q, t, Cov, Loc)
% CRLB = AOARBL3D_CRLB(A, C, Q, t, Cov, Loc)
%
% Obtain the CRLB for the rotation matrix and the translation vector for
% the localization of a rigid body in 3D
%
% Input:
%   Ain:    (3 x M), anchor positions in global coord, M = number of anchors
%   Cin:    (3 x N), sensor positons in local coord., N = number of sensors
%   Q:      (3 x 3), true rotation matrix
%   t:      (3 x 1), true translation vector
%   Cov:    (2MN x 2MN), AOA covariance matrix
%   Loc:    scalar, Self-Localization (Loc=0), Localization(Loc=1)
%
% Output:
%   CRLB:   (12 x 12), where 12 = (length(vec(Q))+3),
%           CRLB of [vec(Q)^T,
%
% Reference:
% Y. Wang, G. Wang, S. Chen, K. C. Ho and L. Huang, "An investigation and
% solution of angle based rigid body localization," IEEE Trans. Signal
% Process., vol. 68, pp. 5457-5472, 2020.
%
% Yue Wang, K. C. Ho   07-18-2021
%
%       Copyright (C) 2020
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA
%       hod@missouri.edu
%

if Loc==0
    A=Ain;C=Cin;
else
    A=Cin;C=Ain;
end

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

for m = 1:M
    S(:,m) = Q*A(:,m) + t;
    for n = 1:N
        temp = S(:,m) - C(:,n);
        drdt((N*(m-1)+n),:) = 1/norm(temp(1:2))^2 *[temp(1)* kron(A(:,m), [0 1 0]')' - temp(2)*kron(A(:,m), [1 0 0]')',...
            -temp(2), temp(1), 0];
        drdp((N*(m-1)+n),:) = 1/norm(temp(1:2))*[kron(A(:,m), [0 0 1]')' - temp(3)/norm(temp)^2 * ( kron( A(:,m)',eye(3) )'*(S(:,m) - C(:,n)) )',...
            [0 0 1] - temp(3)/norm(temp)^2 * (S(:,m) - C(:,n))'];
    end
end

dr=[drdt;drdp];
FIM = dr'*inv(Cov)*dr;
q1 = Q(:,1); q2 = Q(:,2); q3 = Q(:,3);
zero = zeros(1,3);
G = [2*q1' zero  zero  zero;
    q2'   q1'   zero  zero;
    q3'   zero  q1'   zero;
    zero  2*q2' zero  zero;
    zero  q3'   q2'   zero;
    zero  zero  2*q3' zero];

G = [G;
    det( Q(2:3,2:3) ),  -det(Q([1 3],2:3)), det(Q(1:2,2:3)),...
    -det(Q(2:3,[1 3])), det(Q([1 3],[1 3])), -det(Q(1:2,[1 3])),...
    det(Q(2:3,1:2)),    -det(Q([1 3],1:2)), det(Q(1:2,1:2)),  zero];

% constrained CRLB
U = null(G);
CRLB = U*inv(U'*FIM*U)*U';

% -------------------------------------------------------------------
