function [CRLB_All,cov_a_MLE3_thy] = TOARBLAncErrCalSrcCRLB(A, C, q, t, p, Q_r, Q_d, Q_a, Q_p)
%
%  [CRLB_All,cov_a_MLE3_thy] = TOARBLAncErrCalSrcCRLB(A, C, q, t, p, Q_r, Q_d, Q_a, Q_p)
%
% This program evaluates the CRLB for rigid body localization using TOA
% measurements, in the presence of anchor and calibration emitter position
% errors.  The program 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.
%
% Input Parameter List:
% A:        KxM, anchor positions, K is localization dimension.
% C:        KxN, sensor positions local to the rigid body.
% q:        Kx1, rotation angles.
% t:        Kx1, rigid body position.
% p:        Kx1, calibration emitter position.
% Q_r:      MNxMN, covariance matrix of range measurements.
% Q_d:      MxM, covariance matrix of calibration emitter range measurements.
% Q_a:      KMxKM, covariance matrix of anchor positions.
% Q_p:      KxK, covariance matrix of calibration emitter position.
%
% Output Parameter List inside the structure CRLB_All:
% CRLB_Q_noAnUn:            CRLB of Q without the anchors position uncertainties
% CRLB_Q_WithAnUn:          CRLB of Q in the presense of anchors position uncertainties but without using any calibration emitter
% CRLB_Q_WithAnUn_ca:       CRLB of Q in the presense of anchors position uncertainties using the true calibration emitter position
% CRLB_Q_WithAnUn_ca_nsy:   CRLB of Q in the presense of anchors position uncertainties using the noisy calibration emitter position
% CRLB_noAnUn:              CRLB of q and t without the anchors position uncertainties
% CRLB_WithAnUn:            CRLB of q and t in the presense of anchors position uncertainties but without using any calibration emitter
% CRLB_WithAnUn_ca:         CRLB of q and t in the presense of anchors position uncertainties using the true calibration emitter position
% CRLB_WithAnUn_ca_nsy:     CRLB of q and t in the presense of anchors position uncertainties using the noisy calibration emitter position
% CRLB_A_WithAnUn:          CRLB of A in the presense of anchors position uncertainties but without using any calibration emitter
% CRLB_A_WithAnUn_ca:       CRLB of A in the presense of anchors position uncertainties using the true calibration emitter position
% CRLB_A_WithAnUn_ca_nsy:   CRLB of A in the presense of anchors position uncertainties using the noisy calibration emitter position
%
% cov_a_MLE3_thy:           theoretical covariance matrix of MLE3
%
% 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(A,2);  %% number of anchors
N = size(C,2);  %% number of sensors

K = size(A,1);  %% localization dimension
% -------------------------------------------------

if (K==2)
    % 2-D
    % 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)];
    Lambda=dQ;

else
    % 3-D
alpha=q(1); beta=q(2); gamma=q(3);

% rotation matrix
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,-sin(beta)*cos(gamma),-cos(beta)*sin(gamma);
          0,-sin(beta)*sin(gamma),cos(beta)*cos(gamma);
          0,-cos(beta),0;
          sin(alpha)*sin(gamma)+cos(alpha)*sin(beta)*cos(gamma),sin(alpha)*cos(beta)*cos(gamma),-cos(alpha)*cos(gamma)-sin(alpha)*sin(beta)*sin(gamma);
          -sin(alpha)*cos(gamma)+cos(alpha)*sin(beta)*sin(gamma),sin(alpha)*cos(beta)*sin(gamma),-cos(alpha)*sin(gamma)+sin(alpha)*sin(beta)*cos(gamma);
          cos(alpha)*cos(beta),-sin(alpha)*sin(beta),0;
          cos(alpha)*sin(gamma)-sin(alpha)*sin(beta)*cos(gamma),cos(alpha)*cos(beta)*cos(gamma),sin(alpha)*cos(gamma)-cos(alpha)*sin(beta)*sin(gamma);
          -cos(alpha)*cos(gamma)-sin(alpha)*sin(beta)*sin(gamma),cos(alpha)*cos(beta)*sin(gamma),sin(alpha)*sin(gamma)+cos(alpha)*sin(beta)*cos(gamma);
          -sin(alpha)*cos(beta),-cos(alpha)*sin(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];

Lambda=dQ;
end;
% -------------------------------------------------
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_X = dr_du' * inv(Q_r) * dr_du;
CRLB_noAnUn = inv(FIM_X);
CRLB_Q_noAnUn= Lambda * CRLB_noAnUn(1:(2*K-3),1:(2*K-3)) * Lambda';

dr_da = zeros(M*N,K*M);
for n = 1:N
    for m = 1:M
        dr_da(M*(n-1)+m,:) = 1/norm(A(:,m)-Q*C(:,n)-t)*[zeros(1,K*(m-1)),(A(:,m)-Q*C(:,n)-t)',zeros(1,K*(M-m))];
    end
end
FIM_Y = dr_du' * inv(Q_r) * dr_da;

da_da = eye(K*M,K*M);

dd_da = zeros(M*K,M);
for m = 1:M
    dd_da(:,m) = 1/norm(A(:,m)-p)*[zeros(1,K*(m-1)),(A(:,m)-p)',zeros(1,K*(M-m))]';
end
FIM_Z_nop = da_da' * inv(Q_a) * da_da + dr_da' * inv(Q_r) * dr_da;
FIM_Z = FIM_Z_nop + dd_da * inv(Q_d) * dd_da' ;

dp_dp = eye(K,K);
for m = 1:M
    dd_dp(:,m) = 1/norm(A(:,m)-p)*(p-A(:,m));
end

FIM_E = dd_dp * inv(Q_d) * dd_da';

FIM_F = dd_dp * inv(Q_d) * dd_dp' + dp_dp' * inv(Q_p) * dp_dp;

FIM_Z_nsy = FIM_Z - FIM_E' * inv(FIM_F) * FIM_E;

CRLB_A_WithAnUn = inv(FIM_Z_nop-FIM_Y' * inv(FIM_X) * FIM_Y);
CRLB_A_WithAnUn_p = inv(FIM_Z-FIM_Y' * inv(FIM_X) * FIM_Y);
CRLB_A_WithAnUn_p_nsy = inv(FIM_Z_nsy-FIM_Y' * inv(FIM_X) * FIM_Y);

CRLB_WithAnUn = inv(FIM_X) + inv(FIM_X) * FIM_Y * CRLB_A_WithAnUn * FIM_Y' * inv(FIM_X);
CRLB_WithAnUn_p = inv(FIM_X) + inv(FIM_X) * FIM_Y * CRLB_A_WithAnUn_p * FIM_Y' * inv(FIM_X);
CRLB_WithAnUn_p_nsy = inv(FIM_X) + inv(FIM_X) * FIM_Y * CRLB_A_WithAnUn_p_nsy * FIM_Y' * inv(FIM_X);

CRLB_Q_WithAnUn= Lambda * CRLB_WithAnUn(1:(2*K-3),1:(2*K-3)) * Lambda';
CRLB_Q_WithAnUn_p= Lambda * CRLB_WithAnUn_p(1:(2*K-3),1:(2*K-3)) * Lambda';
CRLB_Q_WithAnUn_p_nsy= Lambda * CRLB_WithAnUn_p_nsy(1:(2*K-3),1:(2*K-3)) * Lambda';

Ra = [Q_a, zeros(K*M,M); zeros(M,K*M), Q_d + dd_dp' * Q_p * dd_dp];
Ga = [da_da; dd_da'];
cov_a_MLE3_thy = inv(Ga' * inv(Ra) * Ga);

% -- pack outputs and return --
CRLB_All.CRLB_Q_noAnUn=CRLB_Q_noAnUn;
CRLB_All.CRLB_Q_WithAnUn=CRLB_Q_WithAnUn; 
CRLB_All.CRLB_Q_WithAnUn_p=CRLB_Q_WithAnUn_p;
CRLB_All.CRLB_Q_WithAnUn_p_nsy=CRLB_Q_WithAnUn_p_nsy;
CRLB_All.CRLB_noAnUn=CRLB_noAnUn;
CRLB_All.CRLB_WithAnUn=CRLB_WithAnUn;
CRLB_All.CRLB_WithAnUn_p=CRLB_WithAnUn_p;
CRLB_All.CRLB_WithAnUn_p_nsy=CRLB_WithAnUn_p_nsy;
CRLB_All.CRLB_A_WithAnUn=CRLB_A_WithAnUn;
CRLB_All.CRLB_A_WithAnUn_p=CRLB_A_WithAnUn_p;
CRLB_All.CRLB_A_WithAnUn_p_nsy=CRLB_A_WithAnUn_p_nsy;

