% This is the main program for localization of rigid body using TOA 
% measurements. It works for both the 2-D and 3-D cases. 
%
% This program will reproduce results (when setting L=1000, Num=200 in the code) 
% for Figs. 2-5 of 
% 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.
%
% To produce the results for the 2-D case, set K=2.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%% function description %%%%%%%%%%%%%%%%%%%%%%%%%%
% TOARBL_CRLB.m:                    CRLB 
% TOARBL_DAC.m:                     Rigid body localization using DAC 
% TOARBL_REF.m:                     Refining the DAC preliminary solution 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Shanjie Chen, Benjian Hao, K. C. Ho     11-18-2019
% K. C. Ho                                12-26-2022 
%   (bug fix for Refinement of the preliminary solution)
%
%       Copyright (C) 2019
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA.
%       hod@missouri.edu
%

clear;
warning off;

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

% matlabpool 4
% L = 1000; % Monte Carlo runs for each noise power
% Num = 200; % number of random anchor geometries and random calibration emitter geometries
L = 100; % Monte Carlo runs for each noise power
M = 6; % number of anchors
Num = 10; % number of random anchor geometries and random calibration emitter geometries

pi = 3.1415926;

%% generation of anchors and rigid body sensors configuration
% random anchor configurations in inertial(global) reference frame
rand('state',0);
geometry = cell(Num,1);
goodanchor = 0;
for num = 1:5*Num
    A = 100*(rand(K, M)-1/2);
    kk = 0;
    for i = 1:M-1
        for j = i+1:M
            kk = kk+1;
            r(kk) = sqrt( sum((A(:,i) - A(:,j)).^2) );
        end
    end
    if sum(r < 15) == 0 % ensure the distance between two 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];
    
    %% 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.5 1.5 1;
        0    0  0   0   1];
    
    %% 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 in deg.
    q = [alpha,beta,gamma]';
end;

% number of sensors on the rigid body
N = size(C,2);

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

%% setting of distance measurement noise 
% 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);

% distance noise power range
sigma2_r = -40 : 10 : 20;   % 20 log10(noise level)
Len = length(sigma2_r);  

% display progress 
fprintf('Noise Level Index:');

%% simulation over Num random anchor geometries at different range measurements noise power
for nseIdx = 1 : Len                 % Vary the range measurements noise power
    
    fprintf(' %d/%d.',nseIdx,Len);
    
    % distance measurement noise covariance structure
    QQ=diag([1:6]/6).^2;            % noise powers in different anchors
    Q_r = kron(eye(N),QQ);          % from the rigid body sensors to the anchors
  
    nsePwr = 10^(sigma2_r(nseIdx)/10);
    
    % for distance r from the rigid body sensors to the anchors 
    Q_r = nsePwr * Q_r;
    chol_r = chol(Q_r);

    %% simulation for Num random anchor configurations
    for num = 1:Num                     % Different geometry
        
        % anchor positions
        A = geometry{num,1};  
        
        % noiseless distance
        for n = 1:N
            for m = 1:M
                ro(m,n) = norm(A(:,m)-S(:,n));  % between anchor and rigid body sensor
            end
        end

        % CRLB
        [CRLB_Q, CRLB_Qt] = TOARBL_CRLB(A, C, q, t, Q_r);
        % CRLB for rotation matrix Q
        Q_CRLB(num) = trace(CRLB_Q);    
        % CRLB for translation vector t
        t_CRLB(num) = trace(CRLB_Qt(end-K+1:end, end-K+1:end));  
    
        ac_Q=zeros(K);      % accumulated Q over l for bias computation 
        ac_t=zeros(K,1);    % accumulated t over l for bias computation 
        
        % 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;

            %-----------------------------------------------------------------------------------%
            
            % proposed method, preliminary solution by DAC
            [Q_pre, t_pre] = TOARBL_DAC(A, C, r, Q_r);
        
            % Refinement of the preliminary solution
            Q_fnl = Q_pre; t_fnl = t_pre;
            for times = 1 : 2
               [Q_fnl, t_fnl] = TOARBL_REF(A, C, r, Q_r,  Q_fnl, t_fnl  );
            end
            se_Q(l) = norm(Q_fnl-Q,'fro')^2;
            se_t(l) = norm(t_fnl-t,2)^2;
            ac_Q=ac_Q+Q_fnl; 
            ac_t=ac_t+t_fnl;
                       
        end;    % over noise

       
        % MSE under each noisy anchors configuration and each range measurements noise power for the proposed eDAC and eSDP methods         
        mse_Q(num,nseIdx) = mean(se_Q);
        mse_t(num,nseIdx) = mean(se_t);
        bias_Q(num,nseIdx) = sum(sum((ac_Q/L-Q).^2));
        bias_t(num,nseIdx) = sum((ac_t/L-t).^2);
        
    end;    % over anchor geometry
    
    % -- average CRLB over anchor geometries --
    Q_CRLB_av(nseIdx) = mean(Q_CRLB);
    t_CRLB_av(nseIdx) = mean(t_CRLB);
   
    % -- average mse and bias over anchor geometries --
    mse_Q_av(nseIdx) = mean(mse_Q(:,nseIdx));
    mse_t_av(nseIdx) = mean(mse_t(:,nseIdx));
    bias_Q_av(nseIdx) = mean(bias_Q(:,nseIdx));
    bias_t_av(nseIdx) = mean(bias_t(:,nseIdx));
    
end     % noise level

% matlabpool close

fprintf('\n');

% -- plot results --
% Q RMSE 
figure(2); % for figure 2
loglog(10.^(sigma2_r/20),sqrt(mse_Q_av),'o','MarkerSize',8,'linewidth',1.1); hold on;
loglog(10.^(sigma2_r/20),sqrt(Q_CRLB_av),'-r','MarkerSize',8,'linewidth',1.0); hold on;
legend( 'Final solution', 'Root CRLB({\bf{Q}})','location','northwest')
xlabel('Range noise \sigma')
ylabel('RMSE(Q)');
grid on;
if (K==3) ylim([1e-3 10]); end;
hold off;
title(['RMSE of Q from ',num2str(Num),' Random Geometries, each ',num2str(L),' Ensemble Runs']);  

% translation RMSE
figure(3); % for figure 3
loglog(10.^(sigma2_r/20),sqrt(mse_t_av),'o','MarkerSize',8,'linewidth',1.1); hold on;
loglog(10.^(sigma2_r/20),sqrt(t_CRLB_av),'-r','MarkerSize',8,'linewidth',1.0); hold on;
legend( 'Final solution', 'Root CRLB({\bf{t}})','location','northwest')
xlabel('Range noise \sigma')
ylabel('RMSE(t)');
grid on;
if (K==3) ylim([1e-2 100]); end;
hold off;
title(['RMSE of t from ',num2str(Num),' Random Geometries, each ',num2str(L),' Ensemble Runs']);  

% Q bias 
figure(4); % for figure 4
loglog(10.^(sigma2_r/20),sqrt(bias_Q_av),'o','MarkerSize',8,'linewidth',1.1); hold on;
legend( 'Final solution','location','northwest')
xlabel('Range noise \sigma')
ylabel('bias(Q)');
grid on;
if (K==3) ylim([1e-6 10]); end;
hold off;
title(['Bias of Q from ',num2str(Num),' Random Geometries, each ',num2str(L),' Ensemble Runs']);  

% translation bias
figure(5); % for figure 5
loglog(10.^(sigma2_r/20),sqrt(bias_t_av),'o','MarkerSize',8,'linewidth',1.1); hold on;
legend( 'Final solution','location','northwest')
xlabel('Range noise \sigma')
ylabel('bias(t)');
grid on;
if (K==3) ylim([1e-6 100]); end;
hold off;
title(['Bias of t from ',num2str(Num),' Random Geometries, each ',num2str(L),' Ensemble Runs']);  
