% This is the main program calling routines for rigid body localization
% in the presence of anchor position uncertainties.  It is applicable for
% 2-D and 3-D localizations.
%
% This program generates the results for the figures in the paper
% 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.
%
% Please choose the performance vs different kinds of noise by setting
% lines 48-52.
%
% The code works for 2D or 3D rigid body localization, setting on 
% lines 54-57.
%
% Note: The figures generated may not be identical to those in the
% reference paper, due to the random noise seed setting.  The relative
% performance of the algorithms with respect to the CRLB should be
% consistent with the results in the paper.
%
% To reduce running time, the code only uses a single geometry, with 
% two random initializations for the MLEs only.  Please remove lines 65-66 
% for averaging 200 geometries and 10 random initializations as in the paper.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%% function description %%%%%%%%%%%%%%%%%%%%%%%%%%
% TOARBL_CRLB_aNsy_pNsy.m:          CRLB
% MLE1.m:                           proposed solution MLE1
% MLE2.m:                           proposed solution MLE2
% MLE3.m:                           proposed solution MLE3
% TOARBL_DAC_aNsy_pNsy.m:           step-1,2 of proposed solution eDAC
% TOARBL_REF_aNsy.m:                step-3 of proposed solution eDAC
% TOARBL_DAC.m:                     preliminary solution of DAC from Chen and Ho [24]
% TOARBL_REF.m:                     refinement of DAC from Chen and Ho in [24]
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% 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
%

clear, 
warning off;

% &&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
Choice = 'RngMsrNse';   % performance vs range noise power
% Choice = 'AncPosNse';   % performance vs anchor position noise power
% Choice = 'CalPosNse';   % performance vs calibration position noise power, needs large L
% &&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&

% ===== user choice for localization dimension =====
% K = 2;  % 2-D case
K = 3;  % 3-D case
% ==================================================

L = 1000;   % Monte Carlo runs for each noise power
M = 6;      % number of anchors
Num = 200;  % number of random anchor geometries and random calibration emitter geometries
KK = 10;    % number of random initializations for the MLEs
pi = 3.1415926;

Num=1;      % one geometry only
KK = 2;     % number of random initializations for the MLEs

%% generation of anchors,calibration emitter and rigid body sensors configuration
% random anchor and calibration emitter configurations in inertial(global) reference frame
rand('state',1);
geometry = cell(Num,1);
goodanchor = 0;
for num = 1:10*Num
    A = 100*(rand(K, M+1)-1/2);
    kk = 0;
    for i = 1:M
        for j = i+1:M+1
            kk = kk+1;
            r(kk) = sqrt( sum((A(:,i) - A(:,j)).^2) );
        end
    end
    if sum(r < 15) == 0 % ensure the distance between anchors and that between calibration emitter and anchors are not too small, here choose distance > 15
        goodanchor = goodanchor + 1;
        geometry{goodanchor,1} = A ;
    end
end

if (K==2)
    % rigid body sensor positions in local reference frame
    C = 3*[0.5 1.5 1.5 0.5 1;
        0    0  1.5 1.5 1];
    % number of sensors
    N = size(C,2);
    %% setting of translation vector and the orientation
    % translation
    t = [100 100]';
    % rotation matrix
    theta = 20;
    Q = [cosd(theta) -sind(theta); sind(theta) cosd(theta)];
    q=theta;
    
elseif (K==3)
    
    % rigid body sensor positions in local reference frame
    C = 3*[0.5 1.5 1.5 0.5 1;
        0    0  1.0 1.0 1;
        0    0  0   0   1];
    % number of sensors
    N = size(C,2);
    %% setting of translation vector and the orientation
    %translation
    t = [100 100 50]';
    % rotation matrix
    alpha = 20;
    beta = -25;
    gamma = 10;
    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;
    % rotation angles
    q = [alpha,beta,gamma]';
    
end;

% rigid body sensor positions in inertial(global) reference frame
S = Q * C + t * ones(1,N);

% make sure the noise is zero-mean for r, subtract the noise mean value
randn('state',17);
noise_r = randn(M*N,L);
mean_noise_r = mean(noise_r');
noise_r_0 = noise_r - mean_noise_r'*ones(1,L);

% make sure the noise is zero-mean for d, subtract the noise mean value
randn('state',19);
noise_d = randn(M*1,L);
mean_noise_d = mean(noise_d');
noise_d_0 = noise_d - mean_noise_d'*ones(1,L);

% make sure the noise is zero-mean for the anchor positions a
randn('state',11);
noise_A = randn(M*K,L);
mean_noise_A = mean(noise_A');
noise_A_0 = noise_A - mean_noise_A'*ones(1,L);

% make sure the noise is zero-mean for the calibration emitter position p
randn('state',7);
noise_p = randn(K,L);
mean_noise_p = mean(noise_p');
noise_p_0 = noise_p - mean_noise_p'*ones(1,L);

% distance noise power range (sigma2_r = sigma2_d)
sigma2_r_log = -50 : 5 : -10;
% anchor positions noise power range
sigma2_a_log = -40 : 5 : 15;
% calibration emitter position noise power range
sigma2_p_log = -40 : 5 : 10;

switch Choice,
    case 'RngMsrNse',
        sigma2_log=sigma2_r_log;
    case 'AncPosNse',
        sigma2_log=sigma2_a_log;
    case 'CalPosNse',
        sigma2_log=sigma2_p_log;
end;
Len = length(sigma2_log);

%% simulation over Num random anchor geometries at different range measurements noise power
for nseIdx = 1 : Len                 % Vary the range measurements noise power.
    
    fprintf('Noise Index %d/%d > Geometry Index: ',nseIdx,Len);
    
    % -- set up noise covariance matrices --
    switch Choice,
        case 'RngMsrNse',
            nseIdx_r=nseIdx;    % range measurement noise level
            nseIdx_a = 7;       % sigma_a = -10(logscale), anchor position noise level
            nseIdx_p = 5;       % sigma_p = -20(logscale), calibration emitter position noise level
        case 'AncPosNse',
            nseIdx_r = 3;       % sigma_r = -40(logscale), range measurement noise level
            nseIdx_a=nseIdx;    % anchor position noise level
            nseIdx_p = 7;       % set sigma_p = -10(logscale), calibration emitter position noise level
        case 'CalPosNse',
            nseIdx_r = 3;       % sigma_r = -40(logscale), range measurement noise level
            nseIdx_a = 7;       % sigma_a = -10(logscale), anchor position noise level
            nseIdx_p = nseIdx;  % calibration emitter position noise level
    end;
    
    % distance measurement noise covariances
    Qo_d = eye(M);                    % from the calibration emitter to the anchors
    Q_ro = diag([1:6]/6).^2;
    Qo_r = kron(eye(N),Q_ro);          % from the rigid body sensors to the anchors
    nsePwr_r = 10^(sigma2_r_log(nseIdx_r)/10);
    nsePwr_d = nsePwr_r;
    % for distance r from the rigid body sensors to the anchors
    Q_r = nsePwr_r * Qo_r;
    chol_r = chol(Q_r);
    % for distance d from the calibration emitter to the anchors
    Q_d = nsePwr_d * Qo_d;
    chol_d = chol(Q_d);
    
    % anchor position noise covariance
    Qo_a = eye(K * M);
    nsePwr_a = 10^(sigma2_a_log(nseIdx_a)/10);
    Q_a = Qo_a * nsePwr_a;
    chol_Q_a = chol(Q_a);
    
    % calibration emitter position noise covariance
    Qo_p = eye(K);
    nsePwr_p = 10^(sigma2_p_log(nseIdx_p)/10);
    Q_p = Qo_p * nsePwr_p;
    chol_Q_p = chol(Q_p);
    % ---------------------------------------
    
    %% simulation for Num random anchor configurations
    for num = 1:Num                     % Different geometry
        if (~mod(num,min(20,Num))) fprintf('%d/%d. ',num,Num); end;
        
        % noiseless positions
        A_p = geometry{num,1};
        A = A_p(:,1:M);    % Noiseless anchors position
        p = A_p(:,end);   % Noiseless calibration emitter position
        
        % noiseless distance
        for n = 1:N
            for m = 1:M
                ro(m,n) = norm(A(:,m)-S(:,n));  % between anchor and rigid body sensor
                do(m) = norm(A(:,m)-p);         % between anchor and calibration position
            end
        end
        
        % CRLB
        [CRLB_All,cov_a_MLE3_Theory] = TOARBLAncErrCalSrcCRLB(A, C, q, t, p, Q_r, Q_d, Q_a, Q_p);
        CRLB_Q_noAnUn1=CRLB_All.CRLB_Q_noAnUn;
        CRLB_Q_WithAnUn1=CRLB_All.CRLB_Q_WithAnUn;
        CRLB_Q_WithAnUn_p1=CRLB_All.CRLB_Q_WithAnUn_p;
        CRLB_Q_WithAnUn_p_noisy1=CRLB_All.CRLB_Q_WithAnUn_p_nsy;
        CRLB_qt_noAnUn1=CRLB_All.CRLB_noAnUn;
        CRLB_qt_WithAnUn1=CRLB_All.CRLB_WithAnUn;
        CRLB_qt_WithAnUn_p1=CRLB_All.CRLB_WithAnUn_p;
        CRLB_qt_WithAnUn_p_noisy1=CRLB_All.CRLB_WithAnUn_p_nsy;
        CRLB_A_WithAnUn1=CRLB_All.CRLB_A_WithAnUn;
        CRLB_A_WithAnUn_p1=CRLB_All.CRLB_A_WithAnUn_p;
        CRLB_A_WithAnUn_p_noisy1=CRLB_All.CRLB_A_WithAnUn_p_nsy;
        
        % CRLB for rotation matrix Q
        Q_CRLB_noAnUn(num) = trace(CRLB_Q_noAnUn1);    % when the anchors have no position uncertainties
        Q_CRLB_WithAnUn(num) = trace(CRLB_Q_WithAnUn1);       % when the anchors have position uncertainties and no calibration emitter is used
        Q_CRLB_WithAnUn_p(num) = trace(CRLB_Q_WithAnUn_p1);   % when the anchors have position uncertainties and one accurate calibration emitter is used
        Q_CRLB_WithAnUn_p_noisy(num) = trace(CRLB_Q_WithAnUn_p_noisy1); % when the anchors have position uncertainties and one noisy calibration emitter is used
        % CRLB for rotation angles q
        q_CRLB_noAnUn(num) = (sqrt(trace(CRLB_qt_noAnUn1(1:2*K-3, 1:2*K-3))) * 180 /pi)^2;      % when the anchors have no position uncertainties
        q_CRLB_WithAnUn(num) = (sqrt(trace(CRLB_qt_WithAnUn1(1:2*K-3, 1:2*K-3))) * 180 /pi)^2;  % when the anchors have position uncertainties and no calibration emitter is used
        q_CRLB_WithAnUn_p(num) = (sqrt(trace(CRLB_qt_WithAnUn_p1(1:2*K-3, 1:2*K-3))) * 180 /pi)^2; % when the anchors have position uncertainties and one accurate calibration emitter is used
        q_CRLB_WithAnUn_p_noisy(num) = (sqrt(trace(CRLB_qt_WithAnUn_p_noisy1(1:2*K-3, 1:2*K-3))) * 180 /pi)^2;  % when the anchors have position uncertainties and one noisy calibration emitter is used
        % CRLB for translation vector t
        t_CRLB_noAnUn(num) = trace(CRLB_qt_noAnUn1(end-K+1:end, end-K+1:end));  % when the anchors have no position uncertainties
        t_CRLB_WithAnUn(num) = trace(CRLB_qt_WithAnUn1(end-K+1:end, end-K+1:end)); % when the anchors have position uncertainties and no calibration emitter is used
        t_CRLB_WithAnUn_p(num) = trace(CRLB_qt_WithAnUn_p1(end-K+1:end, end-K+1:end));  % when the anchors have position uncertainties and one accurate calibration emitter is used
        t_CRLB_WithAnUn_p_noisy(num) = trace(CRLB_qt_WithAnUn_p_noisy1(end-K+1:end, end-K+1:end));  % when the anchors have position uncertainties and one noisy calibration emitter is used
        % CRLB for noisy anchors position a
        cov_A(num) = trace(Q_a);
        A_CRLB_WithAnUn(num) = trace(CRLB_A_WithAnUn1);  % when the anchors have position uncertainties and no calibration emitter is used
        A_CRLB_WithAnUn_p(num) = trace(CRLB_A_WithAnUn_p1);   % when the anchors have position uncertainties and one accurate calibration emitter is used
        A_CRLB_WithAnUn_p_noisy(num) = trace(CRLB_A_WithAnUn_p_noisy1);  % when the anchors have position uncertainties and one noisy calibration emitter is used
        A_Theory_MLE3_Ca_noi(num) = trace(cov_a_MLE3_Theory);
        
        % Monte Carlo simulation for each noise realization
        % use 'parfor' for parallel computation
        for l = 1 : L
            % distance measurement nosie and noisy distance for r
            rNse = reshape(chol_r' * noise_r_0(:,l), M, N);
            r = ro + rNse;
            
            % distance measurement nosie and noisy distance for d
            dNse = reshape(chol_d' * noise_d_0(:,l), 1, M);
            d = do' + dNse';
            
            % noisy anchor locations
            Delta_A = chol_Q_a' * noise_A_0(:, l);
            A_noisy = A + reshape(Delta_A, K, M);
            
            % noisy calibration position
            Delta_p = chol_Q_p' * noise_p_0(:, l);
            p_noisy = p + Delta_p;
            
            %% -------repeat every MLE KK times to eliminate thechance of reaching a local minimum--------%
            % define the inial values for q and t for every repeat
            randn('state',31);
            q_rand = (rand(2*K-3,KK)-0.5) * 2 * q_CRLB_WithAnUn(num);   % the difference between q_rand and q is very large for every repeat
            randn('state',37);
            t_rand = (rand(2*K-3,KK)-0.5) * 2 * t_CRLB_WithAnUn(num);   % the difference between t_rand and t is very large for every repeat
            
            % simulation with different inial q and t
            for kk = 1 : KK
                q_inial = q + q_rand(:, kk);
                t_inial = t + t_rand(:, kk);
                
                % proposed solution MLE1
                [Q_MLE1_WithAnUn_p_noisy1(:,:,kk), q_MLE1_WithAnUn_p_noisy1(:,kk), t_MLE1_WithAnUn_p_noisy1(:,kk), A_MLE1_WithAnUn_p_noisy1(:,:,kk), p_MLE1_WithAnUn_p_noisy1(:,kk)] = TOARBLAncErrCalSrc_MLE1(r, A_noisy, C, p_noisy, d, Q_r, Q_a, Q_d, Q_p, q_inial, t_inial);
                cost_MLE1(l,kk) = TOARBLAncErrCalSrc_CostEval_MLE1(r, A_noisy, d, p_noisy, C, Q_r, Q_a, Q_d, Q_p, q_MLE1_WithAnUn_p_noisy1(:,kk), t_MLE1_WithAnUn_p_noisy1(:,kk), A_MLE1_WithAnUn_p_noisy1(:,:,kk), p_MLE1_WithAnUn_p_noisy1(:,kk));
                
                % proposed solution MLE2
                [Q_MLE2_WithAnUn_p_noisy1(:,:,kk), q_MLE2_WithAnUn_p_noisy1(:,kk), t_MLE2_WithAnUn_p_noisy1(:,kk), A_MLE2_WithAnUn_p_noisy1(:,:,kk)] = TOARBLAncErrCalSrc_MLE2(r, A_noisy, C, p_noisy, d, Q_r, Q_a, Q_d, Q_p, q_inial, t_inial);
                cost_MLE2(l,kk) =TOARBLAncErrCalSrc_CostEval_MLE2(r, A_noisy, d, p_noisy, C, Q_r, Q_a, Q_d, Q_p, q_MLE2_WithAnUn_p_noisy1(:,kk), t_MLE2_WithAnUn_p_noisy1(:,kk), A_MLE2_WithAnUn_p_noisy1(:,:,kk));
                
                % proposed solution MLE3
                [Q_MLE3_WithAnUn_p_noisy1(:,:,kk), q_MLE3_WithAnUn_p_noisy1(:,kk), t_MLE3_WithAnUn_p_noisy1(:,kk), A_MLE3_WithAnUn_p_noisy1(:,:,kk)] = TOARBLAncErrCalSrc_MLE3(r, A_noisy, C, p_noisy, d, Q_r, Q_a, Q_d, Q_p, q_inial, t_inial);
                cost_MLE3(l,kk) = TOARBLAncErrCalSrc_CostEval_MLE3(r, A_noisy, p_noisy, C, Q_r, Q_a, Q_d, Q_p, q_MLE3_WithAnUn_p_noisy1(:,kk), t_MLE3_WithAnUn_p_noisy1(:,kk), A_MLE3_WithAnUn_p_noisy1(:,:,kk));
            end
            
            % find the optimal one with the smallest cost
            min_MLE_postition1 = find(cost_MLE1(l,:) == min(cost_MLE1(l,:)));
            min_MLE_postition2 = find(cost_MLE2(l,:) == min(cost_MLE2(l,:)));
            min_MLE_postition3 = find(cost_MLE3(l,:) == min(cost_MLE3(l,:)));
            
            min_MLE1(l) = min_MLE_postition1(1);
            min_MLE2(l) = min_MLE_postition2(1);
            min_MLE3(l) = min_MLE_postition3(1);
            
            % choose the best one for MLE1 and calculate the error
            Q_MLE1_WithAnUn_p_noisy_choosed = Q_MLE1_WithAnUn_p_noisy1(:,:,min_MLE1(l));
            t_MLE1_WithAnUn_p_noisy_choosed = t_MLE1_WithAnUn_p_noisy1(:,min_MLE1(l));
            q_MLE1_WithAnUn_p_noisy_choosed = q_MLE1_WithAnUn_p_noisy1(:,min_MLE1(l));
            A_MLE1_WithAnUn_p_noisy_choosed = A_MLE1_WithAnUn_p_noisy1(:,:,min_MLE1(l));
            
            Q_MLE1_WithAnUn_p_noisy_error(l) = norm(Q_MLE1_WithAnUn_p_noisy_choosed-Q,'fro')^2;
            t_MLE1_WithAnUn_p_noisy_error(l) = norm(t_MLE1_WithAnUn_p_noisy_choosed-t,2)^2;
            q_MLE1_WithAnUn_p_noisy_error(l) = norm(q_MLE1_WithAnUn_p_noisy_choosed-q,2)^2;
            A_MLE1_WithAnUn_p_noisy_error(l) = norm(A_MLE1_WithAnUn_p_noisy_choosed(:)-A(:),2)^2;
            
            % choose the best one for MLE2 and calculate the error
            Q_MLE2_WithAnUn_p_noisy_choosed = Q_MLE2_WithAnUn_p_noisy1(:,:,min_MLE2(l));
            t_MLE2_WithAnUn_p_noisy_choosed = t_MLE2_WithAnUn_p_noisy1(:,min_MLE2(l));
            q_MLE2_WithAnUn_p_noisy_choosed = q_MLE2_WithAnUn_p_noisy1(:,min_MLE2(l));
            A_MLE2_WithAnUn_p_noisy_choosed = A_MLE2_WithAnUn_p_noisy1(:,:,min_MLE2(l));
            
            Q_MLE2_WithAnUn_p_noisy_error(l) = norm(Q_MLE2_WithAnUn_p_noisy_choosed-Q,'fro')^2;
            t_MLE2_WithAnUn_p_noisy_error(l) = norm(t_MLE2_WithAnUn_p_noisy_choosed-t,2)^2;
            q_MLE2_WithAnUn_p_noisy_error(l) = norm(q_MLE2_WithAnUn_p_noisy_choosed-q,2)^2;
            A_MLE2_WithAnUn_p_noisy_error(l) = norm(A_MLE2_WithAnUn_p_noisy_choosed(:)-A(:),2)^2;
            
            % choose the best one for MLE3 and calculate the error
            Q_MLE3_WithAnUn_p_noisy_choosed = Q_MLE3_WithAnUn_p_noisy1(:,:,min_MLE3(l));
            t_MLE3_WithAnUn_p_noisy_choosed = t_MLE3_WithAnUn_p_noisy1(:,min_MLE3(l));
            q_MLE3_WithAnUn_p_noisy_choosed = q_MLE3_WithAnUn_p_noisy1(:,min_MLE3(l));
            A_MLE3_WithAnUn_p_noisy_choosed = A_MLE3_WithAnUn_p_noisy1(:,:,min_MLE3(l));
            
            Q_MLE3_WithAnUn_p_noisy_error(l) = norm(Q_MLE3_WithAnUn_p_noisy_choosed-Q,'fro')^2;
            t_MLE3_WithAnUn_p_noisy_error(l) = norm(t_MLE3_WithAnUn_p_noisy_choosed-t,2)^2;
            q_MLE3_WithAnUn_p_noisy_error(l) = norm(q_MLE3_WithAnUn_p_noisy_choosed-q,2)^2;
            A_MLE3_WithAnUn_p_noisy_error(l) = norm(A_MLE3_WithAnUn_p_noisy_choosed(:)-A(:),2)^2;
            
            
            %-------------------------------------------%
            %% for comparison with other methods
            %-------------------------------------------%
                        
            % Algebraic Method 1: step 1 and 2 for initial solution of the proposed eDAC method
            [Q_pre_WithAnUn_p_noisy1, t_pre_WithAnUn_p_noisy1, A_hat_p_noisy, S_WithAnUn_p_noisy] = TOARBLAncErrCalSrc_eDAC(r, A_noisy, C, d, p_noisy, Q_r, Q_a, Q_d, Q_p);
            
            % Algebraic Method 1: step 3 for the  final refined solution of the proposed eDAC method
            Q_eDAC_WithAnUn_p_noisy1 = Q_pre_WithAnUn_p_noisy1;
            t_eDAC_WithAnUn_p_noisy1 = t_pre_WithAnUn_p_noisy1;
            for times = 1 : 2
                [Q_eDAC_WithAnUn_p_noisy1, t_eDAC_WithAnUn_p_noisy1, q_eDAC_WithAnUn_p_noisy1] = TOARBL_REF_aNsy(r, A_hat_p_noisy, C, Q_r, Q_a, Q_eDAC_WithAnUn_p_noisy1, t_eDAC_WithAnUn_p_noisy1);
            end
            Q_eDAC_WithAnUn_p_noisy_error(l) = norm(Q_eDAC_WithAnUn_p_noisy1-Q,'fro')^2;
            t_eDAC_WithAnUn_p_noisy_error(l) = norm(t_eDAC_WithAnUn_p_noisy1-t,2)^2;
            q_eDAC_WithAnUn_p_noisy_error(l) = norm(q_eDAC_WithAnUn_p_noisy1-q,2)^2;
            A_eDAC_WithAnUn_p_noisy_error(l) = norm(A_hat_p_noisy(:)-A(:),2)^2;
            
            %             % Algebraic Method 2: the proposed eSDP method
            %             [Q_eSDP_WithAnUn_p_noisy1, t_eSDP_WithAnUn_p_noisy1, q_eSDP_WithAnUn_p_noisy1] = TOARBLAncErrCalSrc_CostEval_eSDP(A_hat_p_noisy, C, r, Q_r, p_noisy, Q_d, Q_a, Q_p);
            %             Q_eSDP_WithAnUn_p_noisy_error(l) = norm(Q_eSDP_WithAnUn_p_noisy1-Q,'fro')^2;
            %             t_eSDP_WithAnUn_p_noisy_error(l) = norm(t_eSDP_WithAnUn_p_noisy1-t,2)^2;
            %             q_eSDP_WithAnUn_p_noisy_error(l) = norm(q_eSDP_WithAnUn_p_noisy1-q',2)^2;
            %             A_eSDP_WithAnUn_p_noisy_error(l) = norm(A_hat_p_noisy(:)-A(:),2)^2;
        end
        
        % MSE under each noisy anchors configuration and each range measurements noise power for the proposed MLE1 method
        mse_Q_MLE1_WithAnUn_p_noisy(num,nseIdx) = mean(Q_MLE1_WithAnUn_p_noisy_error);
        mse_t_MLE1_WithAnUn_p_noisy(num,nseIdx) = mean(t_MLE1_WithAnUn_p_noisy_error);
        mse_q_MLE1_WithAnUn_p_noisy(num,nseIdx) = mean(q_MLE1_WithAnUn_p_noisy_error);
        mse_A_MLE1_WithAnUn_p_noisy(num,nseIdx) = mean(A_MLE1_WithAnUn_p_noisy_error);
        
        % MSE under each noisy anchors configuration and each range measurements noise power for the proposed MLE2 method
        mse_Q_MLE2_WithAnUn_p_noisy(num,nseIdx) = mean(Q_MLE2_WithAnUn_p_noisy_error);
        mse_t_MLE2_WithAnUn_p_noisy(num,nseIdx) = mean(t_MLE2_WithAnUn_p_noisy_error);
        mse_q_MLE2_WithAnUn_p_noisy(num,nseIdx) = mean(q_MLE2_WithAnUn_p_noisy_error);
        mse_A_MLE2_WithAnUn_p_noisy(num,nseIdx) = mean(A_MLE2_WithAnUn_p_noisy_error);
        
        % MSE under each noisy anchors configuration and each range measurements noise power for the proposed MLE3 method
        mse_Q_MLE3_WithAnUn_p_noisy(num,nseIdx) = mean(Q_MLE3_WithAnUn_p_noisy_error);
        mse_t_MLE3_WithAnUn_p_noisy(num,nseIdx) = mean(t_MLE3_WithAnUn_p_noisy_error);
        mse_q_MLE3_WithAnUn_p_noisy(num,nseIdx) = mean(q_MLE3_WithAnUn_p_noisy_error);
        mse_A_MLE3_WithAnUn_p_noisy(num,nseIdx) = mean(A_MLE3_WithAnUn_p_noisy_error);
        
        % MSE under each noisy anchors configuration and each range measurements noise power for the proposed eDAC method
        mse_Q_eDAC_WithAnUn_p_noisy(num,nseIdx) = mean(Q_eDAC_WithAnUn_p_noisy_error);
        mse_t_eDAC_WithAnUn_p_noisy(num,nseIdx) = mean(t_eDAC_WithAnUn_p_noisy_error);
        mse_q_eDAC_WithAnUn_p_noisy(num,nseIdx) = mean(q_eDAC_WithAnUn_p_noisy_error);
        mse_A_eDAC_WithAnUn_p_noisy(num,nseIdx) = mean(A_eDAC_WithAnUn_p_noisy_error);
        
        % MSE under each noisy anchors configuration and each range measurements noise power for the proposed eSDP method
        %         mse_Q_eSDP_WithAnUn_p_noisy(num,nseIdx) = mean(Q_eSDP_WithAnUn_p_noisy_error);
        %         mse_t_eSDP_WithAnUn_p_noisy(num,nseIdx) = mean(t_eSDP_WithAnUn_p_noisy_error);
        %         mse_q_eSDP_WithAnUn_p_noisy(num,nseIdx) = mean(q_eSDP_WithAnUn_p_noisy_error);
        %         mse_A_eSDP_WithAnUn_p_noisy(num,nseIdx) = mean(A_eSDP_WithAnUn_p_noisy_error);
        
    end     % over num
    fprintf('\n');
    
    Q_CRLB_noAnUn_av(nseIdx) = mean(Q_CRLB_noAnUn);
    Q_CRLB_WithAnUn_av(nseIdx) = mean(Q_CRLB_WithAnUn);
    Q_CRLB_WithAnUn_p_av(nseIdx) = mean(Q_CRLB_WithAnUn_p);
    Q_CRLB_WithAnUn_p_noisy_av(nseIdx) = mean(Q_CRLB_WithAnUn_p_noisy);
    
    q_CRLB_noAnUn_av(nseIdx) = mean(q_CRLB_noAnUn);
    q_CRLB_WithAnUn_av(nseIdx) = mean(q_CRLB_WithAnUn);
    q_CRLB_WithAnUn_p_av(nseIdx) = mean(q_CRLB_WithAnUn_p);
    q_CRLB_WithAnUn_p_noisy_av(nseIdx) = mean(q_CRLB_WithAnUn_p_noisy);
    
    t_CRLB_noAnUn_av(nseIdx) = mean(t_CRLB_noAnUn);
    t_CRLB_WithAnUn_av(nseIdx) = mean(t_CRLB_WithAnUn);
    t_CRLB_WithAnUn_p_av(nseIdx) = mean(t_CRLB_WithAnUn_p);
    t_CRLB_WithAnUn_p_noisy_av(nseIdx) = mean(t_CRLB_WithAnUn_p_noisy);
    
    cov_A_av(nseIdx) =  mean(cov_A);
    A_CRLB_WithAnUn_av(nseIdx) =  mean(A_CRLB_WithAnUn);
    A_CRLB_WithAnUn_p_av(nseIdx) =  mean(A_CRLB_WithAnUn_p);
    A_CRLB_WithAnUn_p_noisy_av(nseIdx) =  mean(A_CRLB_WithAnUn_p_noisy);
    A_Theory_MLE3_Ca_noi_av(nseIdx) =  mean(A_Theory_MLE3_Ca_noi);
    
    % With the noisy calibration source, MSE under each range measurements noise power for the proposed MLE1 method
    mse_Q_MLE1_WithAnUn_p_noisy_av(nseIdx) = mean(mse_Q_MLE1_WithAnUn_p_noisy(:,nseIdx));
    mse_t_MLE1_WithAnUn_p_noisy_av(nseIdx) = mean(mse_t_MLE1_WithAnUn_p_noisy(:,nseIdx));
    mse_q_MLE1_WithAnUn_p_noisy_av(nseIdx) = mean(mse_q_MLE1_WithAnUn_p_noisy(:,nseIdx));
    mse_A_MLE1_WithAnUn_p_noisy_av(nseIdx) = mean(mse_A_MLE1_WithAnUn_p_noisy(:,nseIdx));
    
    % With the noisy calibration source, MSE under each range measurements noise power for the proposed MLE1 method
    mse_Q_MLE2_WithAnUn_p_noisy_av(nseIdx) = mean(mse_Q_MLE2_WithAnUn_p_noisy(:,nseIdx));
    mse_t_MLE2_WithAnUn_p_noisy_av(nseIdx) = mean(mse_t_MLE2_WithAnUn_p_noisy(:,nseIdx));
    mse_q_MLE2_WithAnUn_p_noisy_av(nseIdx) = mean(mse_q_MLE2_WithAnUn_p_noisy(:,nseIdx));
    mse_A_MLE2_WithAnUn_p_noisy_av(nseIdx) = mean(mse_A_MLE2_WithAnUn_p_noisy(:,nseIdx));
    
    % With the noisy calibration source, MSE under each range measurements noise power for the proposed MLE1 method
    mse_Q_MLE3_WithAnUn_p_noisy_av(nseIdx) = mean(mse_Q_MLE3_WithAnUn_p_noisy(:,nseIdx));
    mse_t_MLE3_WithAnUn_p_noisy_av(nseIdx) = mean(mse_t_MLE3_WithAnUn_p_noisy(:,nseIdx));
    mse_q_MLE3_WithAnUn_p_noisy_av(nseIdx) = mean(mse_q_MLE3_WithAnUn_p_noisy(:,nseIdx));
    mse_A_MLE3_WithAnUn_p_noisy_av(nseIdx) = mean(mse_A_MLE3_WithAnUn_p_noisy(:,nseIdx));
    
%     % With the noisy calibration source, MSE under each range measurements noise power for the eDAC method
%     mse_Q_pre_WithAnUn_p_noisy_av(nseIdx) = mean(mse_Q_pre_WithAnUn_p_noisy(:,nseIdx));
%     mse_t_pre_WithAnUn_p_noisy_av(nseIdx) = mean(mse_t_pre_WithAnUn_p_noisy(:,nseIdx));
%     mse_A_pre_WithAnUn_p_noisy_av(nseIdx) = mean(mse_A_pre_WithAnUn_p_noisy(:,nseIdx));
    
    % With the noisy calibration source, MSE under each range measurements noise power for the eDAC method
    mse_Q_eDAC_WithAnUn_p_noisy_av(nseIdx) = mean(mse_Q_eDAC_WithAnUn_p_noisy(:,nseIdx));
    mse_t_eDAC_WithAnUn_p_noisy_av(nseIdx) = mean(mse_t_eDAC_WithAnUn_p_noisy(:,nseIdx));
    mse_q_eDAC_WithAnUn_p_noisy_av(nseIdx) = mean(mse_q_eDAC_WithAnUn_p_noisy(:,nseIdx));
    mse_A_eDAC_WithAnUn_p_noisy_av(nseIdx) = mean(mse_A_eDAC_WithAnUn_p_noisy(:,nseIdx));
    
    % With the noisy calibration source, MSE under each range measurements noise power for the eSDP method
    %     mse_Q_eSDP_WithAnUn_p_noisy_av(nseIdx) = mean(mse_Q_eSDP_WithAnUn_p_noisy(:,nseIdx));
    %     mse_t_eSDP_WithAnUn_p_noisy_av(nseIdx) = mean(mse_t_eSDP_WithAnUn_p_noisy(:,nseIdx));
    %     mse_q_eSDP_WithAnUn_p_noisy_av(nseIdx) = mean(mse_q_eSDP_WithAnUn_p_noisy(:,nseIdx));
    %     mse_A_eSDP_WithAnUn_p_noisy_av(nseIdx) = mean(mse_A_eSDP_WithAnUn_p_noisy(:,nseIdx));
    
end     % over nseIdx

% generate xlabel
switch Choice,
    case 'RngMsrNse',
        xStr='Range Measurements Noise \sigma_{r}^2(m^2)';
    case 'AncPosNse',
        xStr='Anchor Position Noise \sigma_{a}^2(m^2)';
    case 'CalPosNse',
        xStr='Calirbation Emitter Position Noise \sigma_{p}^2(m^2)';
end;

% Anchor positions MSE
figure(3); clf; 
loglog(10.^(sigma2_log/10),sqrt(mse_A_MLE1_WithAnUn_p_noisy_av),'^b','MarkerSize',8,'linewidth',1.1); hold on;
loglog(10.^(sigma2_log/10),sqrt(mse_A_MLE2_WithAnUn_p_noisy_av),'sb','MarkerSize',12,'linewidth',1.0); hold on;
loglog(10.^(sigma2_log/10),sqrt(mse_A_MLE3_WithAnUn_p_noisy_av),'ob','MarkerSize',10,'linewidth',1.0); hold on;
loglog(10.^(sigma2_log/10),sqrt(mse_A_eDAC_WithAnUn_p_noisy_av),'*k','MarkerSize',8,'linewidth',1.0); hold on;
loglog(10.^(sigma2_log/10),sqrt(A_CRLB_WithAnUn_p_noisy_av),'-r','MarkerSize',8,'linewidth',1.0); hold on;
loglog(10.^(sigma2_log/10),sqrt(cov_A_av),'>k','MarkerSize',8,'linewidth',1.0); hold on;
loglog(10.^(sigma2_log/10),sqrt(A_Theory_MLE3_Ca_noi_av),'--','MarkerSize',8,'linewidth',1.0); hold on;
% legend('MLE1', 'MLE2', 'MLE3', 'eDAC, eSDP', 'Root CRLB({\bf{a}})','Root {\bf{R}}_a','MLE3-Theory')
legend('MLE1', 'MLE2', 'MLE3', 'eDAC', 'Root CRLB({\bf{a}})','Root {\bf{R}}_a','MLE3-Theory','location','southeast')
xlabel(xStr);
ylabel('RMSE of Anchor Position {\bf{a}} (m)');
axis tight;

% orientation angles MSE
figure(4); clf; 
loglog(10.^(sigma2_log/10),sqrt(mse_q_MLE1_WithAnUn_p_noisy_av)*pi/180,'^b','MarkerSize',8,'linewidth',1.1); hold on;
loglog(10.^(sigma2_log/10),sqrt(mse_q_MLE2_WithAnUn_p_noisy_av)*pi/180,'sb','MarkerSize',12,'linewidth',1.0); hold on;
loglog(10.^(sigma2_log/10),sqrt(mse_q_MLE3_WithAnUn_p_noisy_av)*pi/180,'--ob','MarkerSize',10,'linewidth',1.0); hold on;
loglog(10.^(sigma2_log/10),sqrt(mse_q_eDAC_WithAnUn_p_noisy_av)*pi/180,'*k','MarkerSize',8,'linewidth',1.0); hold on;
% loglog(10.^(sigma2_r/10),sqrt(mse_q_eSDP_WithAnUn_p_noisy_av),':xk','MarkerSize',8,'linewidth',1.1); hold on;
loglog(10.^(sigma2_log/10),sqrt(q_CRLB_WithAnUn_p_noisy_av)*pi/180,'-r','MarkerSize',8,'linewidth',1.0); hold on;
legend('MLE1', 'MLE2', 'MLE3', 'eDAC', 'Root CRLB({\bf{q}})','location','northwest');
xlabel(xStr);
ylabel('RMSE of Orientation Angles {\bf{q}} (rad)');
axis tight;

% translational vector MSE
figure(5); clf; 
loglog(10.^(sigma2_log/10),sqrt(mse_t_MLE1_WithAnUn_p_noisy_av),'^b','MarkerSize',10,'linewidth',1.0); hold on;
loglog(10.^(sigma2_log/10),sqrt(mse_t_MLE2_WithAnUn_p_noisy_av),'sb','MarkerSize',12,'linewidth',1.0); hold on;
loglog(10.^(sigma2_log/10),sqrt(mse_t_MLE3_WithAnUn_p_noisy_av),'--ob','MarkerSize',10,'linewidth',1.0); hold on;
loglog(10.^(sigma2_log/10),sqrt(mse_t_eDAC_WithAnUn_p_noisy_av),'*k','MarkerSize',8,'linewidth',1.0); hold on;
% loglog(10.^(sigma2_r/10),sqrt(mse_t_eSDP_WithAnUn_p_noisy_av),':xk','MarkerSize',8,'linewidth',1.1); hold on;
loglog(10.^(sigma2_log/10),sqrt(t_CRLB_WithAnUn_p_noisy_av),'-r','MarkerSize',8,'linewidth',1.0); hold on;
legend( 'MLE1', 'MLE2', 'MLE3', 'eDAC', 'Root CRLB({\bf{t}})','location','northwest');
xlabel(xStr);
ylabel('RMSE of Translationl Vector {\bf{t}} (m)');
axis tight;


