function [CRLB_Q, CRLB_Qt] = TOARBL_CRLB(A, C, q, t, Q_r)
%
%  [CRLB_Q, CRLB_Qt] = TOARBL_CRLB(A, C, q, t, Q_r)
%
% This program evaluates the CRLB for rigid body localization using TOA
% measurements.  It works for both the 2-D and 3-D cases.
% Please refer to the following paper for the details.
% B. Hao, K. C. Ho, and Z. Li, "Range based rigid body localization with 
% a calibration emitter for mitigating anchor position uncertainties," 
% IEEE Trans. Wireless Commun.
%
% Input Parameter List:
% A:        KxM, anchor positions, K is localization dimension.
% C:        KxN, sensor positions local to the rigid body.
% q:        (2K-3)x1, rotation angles.
% t:        Kx1, rigid body position.
% Q_r:      MNxMN, covariance matrix of range measurements.
%
% Output Parameter List:
% CRLB_Qt:  2Kx2K, CRLB of q and t 
% CRLB_Q:   KxK, CRLB of Q 
%
%
% 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
%

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

if (K==2)
    % 2-D case
    % rotation matrix
    theta=q(1);
    Q=[cosd(theta) -sind(theta); sind(theta) cosd(theta)];
    
    % derivative of rotation matrix
    dQ=[-sind(theta) -cosd(theta); cosd(theta) -sind(theta)];
else
    % 3-D case
    % rotation matrix
    alpha=q(1); beta=q(2); gamma=q(3);
    Q_ap = [1 0 0; 0 cosd(alpha) -sind(alpha); 0 sind(alpha) cosd(alpha)];
    Q_bt = [cosd(beta) 0  sind(beta); 0  1 0; -sind(beta)  0 cosd(beta)];
    Q_gm = [cosd(gamma) -sind(gamma) 0; sind(gamma) cosd(gamma) 0; 0 0 1];
    Q = Q_gm * Q_bt * Q_ap;
    
    % derivative of rotation matrix
    dQ = [0,-sind(beta)*cosd(gamma),-cosd(beta)*sind(gamma);
        0,-sind(beta)*sind(gamma),cosd(beta)*cosd(gamma);
        0,-cosd(beta),0;
        sind(alpha)*sind(gamma)+cosd(alpha)*sind(beta)*cosd(gamma),sind(alpha)*cosd(beta)*cosd(gamma),-cosd(alpha)*cosd(gamma)-sind(alpha)*sind(beta)*sind(gamma);
        -sind(alpha)*cosd(gamma)+cosd(alpha)*sind(beta)*sind(gamma),sind(alpha)*cosd(beta)*sind(gamma),-cosd(alpha)*sind(gamma)+sind(alpha)*sind(beta)*cosd(gamma);
        cosd(alpha)*cosd(beta),-sind(alpha)*sind(beta),0;
        cosd(alpha)*sind(gamma)-sind(alpha)*sind(beta)*cosd(gamma),cosd(alpha)*cosd(beta)*cosd(gamma),sind(alpha)*cosd(gamma)-cosd(alpha)*sind(beta)*sind(gamma);
        -cosd(alpha)*cosd(gamma)-sind(alpha)*sind(beta)*sind(gamma),cosd(alpha)*cosd(beta)*sind(gamma),sind(alpha)*sind(gamma)+cosd(alpha)*sind(beta)*cosd(gamma);
        -sind(alpha)*cosd(beta),-cosd(alpha)*sind(beta),0];
    
    dQ_ap = [0 0 0; 0 -sind(alpha) -cosd(alpha); 0 cosd(alpha) -sind(alpha)];
    dQ_bt = [-sind(beta) 0  cosd(beta); 0  0 0; -cosd(beta)  0 -sind(beta)];
    dQ_gm = [-sind(gamma) -cosd(gamma) 0; cosd(gamma) -sind(gamma) 0; 0 0 0];
end;

% -- start CRLB computation --
dr_du = zeros(M*N,(2*K-3)+K); 

if (K==2)
    % 2-D case
    for n = 1:N,
        for m = 1:M,
            dr_dtheta(M*(n-1)+m,:) =  1/norm(Q * C(:,n) + t - A(:,m)) * (Q * C(:,n) + t - A(:,m))' * dQ * C(:,n);
            dr_dt(M*(n-1)+m,:) =  1/norm(Q * C(:,n) + t - A(:,m)) * (Q * C(:,n) + t - A(:,m));
            dr_du(M*(n-1)+m,:) = [dr_dtheta(M*(n-1)+m,:), dr_dt(M*(n-1)+m,:)];
        end
    end
else  
    % 3-D case
    for n = 1:N,
        for m = 1:M,
            dr_dalpha(M*(n-1)+m,:) =  1/norm(Q * C(:,n) + t - A(:,m)) * (Q * C(:,n) + t - A(:,m))' * Q_gm * Q_bt * dQ_ap * C(:,n);
            dr_dbeta(M*(n-1)+m,:) =  1/norm(Q * C(:,n) + t - A(:,m)) * (Q * C(:,n) + t - A(:,m))' * Q_gm * dQ_bt * Q_ap * C(:,n);
            dr_dgamma(M*(n-1)+m,:) =  1/norm(Q * C(:,n) + t - A(:,m)) * (Q * C(:,n) + t - A(:,m))' * dQ_gm * Q_bt * Q_ap * C(:,n);
            dr_dt(M*(n-1)+m,:) =  1/norm(Q * C(:,n) + t - A(:,m)) * (Q * C(:,n) + t - A(:,m));
            dr_du(M*(n-1)+m,:) = [dr_dalpha(M*(n-1)+m,:), dr_dbeta(M*(n-1)+m,:), dr_dgamma(M*(n-1)+m,:), dr_dt(M*(n-1)+m,:)];
        end
    end
end;

FIM = dr_du' * inv(Q_r) * dr_du;
CRLB_Qt = inv(FIM);
CRLB_Q= dQ * CRLB_Qt(1:(2*K-3),1:(2*K-3)) * dQ';
