% Localization in 3-D by propagation time delays to a rigid body receiver.
%
% Performance comparison when the noisy RB position is availble by using
% - closed form solution: LocByRB_CFS_KnownSo.m
% - SDP solution: LocByRB_SDP_KnownSo.m (set SolSDP = 1)
% - MLE solution initialized by closed-form solution: LocByRB_MLE_KnownSo.m
%
% Performance comparison when the RB position is completely unknown by using
% - closed form solution: LocByRB_CFS_UnknownSo.m
% - SDP solution: LocByRB_SDP_UnknownSo.m
% - MLE solution initialized by closed-form solution: LocByRB_MLE_UnknownSo.m
%
% The code can reproduce the results in Figs. 6-7 of the reference paper.
%
% Number of ensemble runs (NumEns) is set to 5 and number of geometries (NumGeo)
% is set to 1 to reduce run time of the code. To reproduce the results in
% Figs. 6-7, set NumEns =300, and NumGeo = 10.
%
% Reference: X. Ke and K. C. Ho, "Localization of a passive object in 3-D by 
% propagation time delays to A rigid body receiver of unknown location," 
% IEEE Trans. Signal Process., vol. 72, pp. 3470-3486, 2024.
%
% X. Ke, K. C. Ho     8-28-2024
%
%       Copyright (C) 2024
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA.
%       hod@missouri.edu
%

clear all;
%clc;
%close all;
warning off;
rng(1437);

SolSDP = 0;    % generate SDP solution by CVX: Yes: 1, No: 0

% -- simulation settings --
IdxFig = 6;    % figure index: set IdxFig = 6 to generate Fig. 6, or IdxFig = 7 to generate Fig. 7
K = 3;         % localization dimension
N_max = 10;    % maximum number of transmitters
N = 5;         % number of transmitters used in this simulation
M = 9;         % number of sensors on the RB
min_dis = 40;  % minimum distance between any two transmitters
NumGeo  = 1;   % number of randomly generated geometries
NumEns = 5;    % number of ensemble runs
C = 10* ([0, 1, 0, 0, 0, 1, 1, 1,0.5;
          0, 0, 1, 0, 1, 0, 1, 1,0.5;
          0, 0, 0, 1, 1, 1, 0, 1,0.5]); % sensor positions local to the RB
[uu,sso,TTr,Omega] = LocByRB_GeoGenerator(N_max,NumGeo,min_dis); % localization geometries
TTr = TTr(:,1:N,:);  % transmitter positions used in this simulation
if IdxFig==6
    sigma2_r=-50:5:0;                        % 10*log10(noise power of distance measurement)
    sigma2_s = -20*ones(1,length(sigma2_r)); % 10*log10(noise power of RB position measurement)
elseif IdxFig ==7
    sigma2_s= -40:5:10;                      % 10*log10(noise power of RB position measurement)
    sigma2_r = -40*ones(1,length(sigma2_s)); % 10*log10(noise power of distance measurement)
end

% -- noise sequence --
rng(3781);
EE_n_r = randn(N_max*M,NumEns);
EE_n_so = randn(K,NumEns);  % RB position noise sequence
EE_n_r = EE_n_r(1:N*M,:);   % distance measurement noise sequence

% -- initialize record for CRLB and MSE --
crlbm_u = zeros(length(sigma2_r),1);            % CRLB for object position when noisy RB position is available
crlbr_u = zeros(length(sigma2_r),1);            % CRLB for object position when RB position is completely unknown
NumAlg = 6;                                     % number of algorithms
mse_u = zeros(NumAlg,length(sigma2_r),NumGeo);  % MSE for object position estimate

for g = 1:1:NumGeo                % loop through geometries
    Tr = TTr(:,:,g);              % positions of transmitters
    u_o = uu(:,g);                % true object position  
    so_o = sso(:,g);              % true RB position
    R_o = Angle2RotM(Omega(:,g)); % rotation matrix
    s_o = R_o*C+so_o;             % true sensors' positions in global coordinate
    p_o = R_o'*(u_o-so_o);        % object position in local coordinate
    
    % -- Jacobian matrices --
    r_o = zeros(M*N,1);
    Jdt_u = zeros(M*N,3);
    Jds_p = zeros(M*N,3);
    for i = 1:1:N
        for k = 1:1:M
            r_o((i-1)*M+k,1) = norm(u_o-Tr(:,i)) + norm(u_o-s_o(:,k)); % true distance
            Jdt_u((i-1)*M+k,1:K) = (u_o-Tr(:,i))'/norm(u_o-Tr(:,i));
            Jds_p((i-1)*M+k,1:K) = (p_o-C(:,k))'/norm(p_o-C(:,k));
        end
    end
    Jp_u = p_o/norm(p_o) * (u_o-so_o)'/norm(u_o-so_o);
    Jp_so = -Jp_u;
    phi_o = [atan2(p_o(2),p_o(1)); asin(p_o(3)/norm(p_o))]; % direction angles of object position in local coordinate 
    Jp_phi = norm(u_o-so_o)*[-sin(phi_o(1,1))*cos(phi_o(2,1)),-cos(phi_o(1,1))*sin(phi_o(2,1));
             cos(phi_o(1,1))*cos(phi_o(2,1)),-sin(phi_o(1,1))*sin(phi_o(2,1));
             0,cos(phi_o(2,1))];
    Jm_vartheta = [Jdt_u+Jds_p*Jp_u,Jds_p*Jp_phi,Jds_p*Jp_so;
                   zeros(3,5),eye(3)];
    Jr_theta = [Jdt_u,Jds_p];
    
    barQ_r = 0.5*diag(r_o.^2)+0.5*r_o*r_o';
    barQ_r = N*M*barQ_r/trace(barQ_r);             % normalized covariance matrix of distance measurement
    barQ_so = 0.5*diag(so_o.^2)+0.5*so_o*so_o';
    barQ_so = 3*barQ_so/trace(barQ_so);            % normalized covariance matrix of RB position measurement
    
    for nsePwrIdx=1:length(sigma2_r)               % loop through measurement noise power
        Q_r  = 10^(sigma2_r(nsePwrIdx)/10)*barQ_r; % covariance matrix of distance measurement
        Q_so = 10^(sigma2_s(nsePwrIdx)/10)*barQ_so;% covariance matrix of RB position measurement
        
        % -- CRLBs --
        Cm_vartheta = inv(Jm_vartheta'*inv(blkdiag(Q_r,Q_so))*Jm_vartheta);
        Cr_theta = inv(Jr_theta'*inv(Q_r)*Jr_theta);
        crlbm_u(nsePwrIdx) = crlbm_u(nsePwrIdx) + trace(Cm_vartheta(1:K,1:K))/NumGeo;
        crlbr_u(nsePwrIdx) = crlbr_u(nsePwrIdx) + trace(Cr_theta(1:K,1:K))/NumGeo;
        
        error2 = zeros(NumAlg,NumEns);            % initialize record for norm(position estimate error)^2
        for ii = 1:1:NumEns                       % loop through ensemble runs
            warning('off','all');
            r = r_o+Q_r^(1/2)*EE_n_r(:,ii);       % diastance measurement
            so = so_o + Q_so^(1/2)*EE_n_so(:,ii); % RB position meaurement
            hatu = 10*ones(K,NumAlg);             % initialize record for object position estimate
            
            % -- proposed closed-form estimator --
            [theta_cfs,hatu(:,1)] = TOALocByRB_CFS_UnknownSo(r,Tr,C,Q_r);          % when so is unknown 
            [vartheta_cfs,hatu(:,2)] = TOALocByRB_CFS_KnownSo(r,Tr,C,so,Q_r,Q_so); % when so is known 
            
            % -- proposed SDP estimator --
            if (SolSDP)
                hatu(:,3) = TOALocByRB_SDP_UnknownSo(r,Tr,C,Q_r);                     % when so is unknown
                hatu(:,4) = TOALocByRB_SDP_KnownSo(r,Tr,C,so,Q_r,Q_so);               % when so is known
            end;

            % -- proposed MLECFS estimator --
            hatu(:,5) = TOALocByRB_MLE_UnknownSo(r,Tr,C,Q_r,30,theta_cfs);           % when so is unknown
            hatu(:,6) = TOALocByRB_MLE_KnownSo(r,Tr,C,so,Q_r,Q_so,30,vartheta_cfs);  % when so is known

            error2(:,ii) = ((hatu - u_o).^2)'*ones(K,1); 
        end
        mse_u(:,nsePwrIdx,g) = mean(error2,2); % MSE of object position estimate
        fprintf('progress: %2.1f%% \n',100*( (g-1)/NumGeo + (nsePwrIdx)/(NumGeo*length(sigma2_r))))
    end  
end
mseAvg_u = mean(mse_u,3); % average the MSE values across all geometries

%%
% -- create plot to show the results --
close all;
mseAvg_u = mseAvg_u';
if IdxFig==6
    figure(6)
    fig_width = 500;  % Width of the figure window in pixels
    fig_height = 450; % Height of the figure window in pixels
    set(gcf, 'Position', [100, 100, fig_width, fig_height]);
    plot(sigma2_r-sigma2_s, 10*log10(crlbr_u),'-r','Linewidth',1); hold on; grid on;
    plot(sigma2_r-sigma2_s, 10*log10(mseAvg_u(:,1)),'+b','Linewidth',0.9);
    if (SolSDP)
        plot(sigma2_r-sigma2_s, 10*log10(mseAvg_u(:,3)),'sm','Linewidth',0.9);
    end;
    plot(sigma2_r-sigma2_s, 10*log10(mseAvg_u(:,5)),'^','Color',[0.4660 0.6740 0.1880],'Linewidth',1.3);
    plot(sigma2_r-sigma2_s, 10*log10(crlbm_u),'-.k','Linewidth',1); hold on; grid on;
    plot(sigma2_r-sigma2_s, 10*log10(mseAvg_u(:,2)),'x','Color',[0.4940 0.1840 0.5560],'Linewidth',1.0);
    if (SolSDP)
        plot(sigma2_r-sigma2_s, 10*log10(mseAvg_u(:,4)),'d','Color',[0.3010 0.7450 0.9330],'Linewidth',0.9);
    end;
    plot(sigma2_r-sigma2_s, 10*log10(mseAvg_u(:,6)),'>','Color',[0.9290 0.6940 0.1250],'Linewidth',1.3);
    if (SolSDP)
        legend({'CRLB-Unknown $\mathbf  s_o$','CFS-Unknown $\mathbf  s_o$','SDP-Unknown $\mathbf  s_o$','MLECFS-Unknown $\mathbf  s_o$','CRLB-Known $\mathbf  s_o$ ','CFS-Known $\mathbf  s_o$','SDP-Known $\mathbf  s_o$','MLECFS-Known $\mathbf  s_o$'}, 'Interpreter', 'latex');
    else
        legend({'CRLB-Unknown $\mathbf  s_o$','CFS-Unknown $\mathbf  s_o$','MLECFS-Unknown $\mathbf  s_o$','CRLB-Known $\mathbf  s_o$ ','CFS-Known $\mathbf  s_o$','MLECFS-Known $\mathbf  s_o$'}, 'Interpreter', 'latex');
    end;
    legend('Location', 'northwest');
    xlabel('20 log($\sigma_{r}/\sigma_{s}$)', 'FontSize', 14,'Interpreter','latex');
    ylabel('10 log(MSE($\mathbf{u}$(m)))', 'FontSize', 12,'Interpreter','latex');
    ylim([-inf 30]);
elseif IdxFig==7
    figure(7)
    fig_width = 500;  % Width of the figure window in pixels
    fig_height = 450; % Height of the figure window in pixels
    set(gcf, 'Position', [100, 100, fig_width, fig_height]);
    plot(sigma2_s-sigma2_r, 10*log10(crlbr_u),'-r','Linewidth',1); hold on; grid on;
    plot(sigma2_s-sigma2_r, 10*log10(mseAvg_u(:,1)),'+b','Linewidth',0.9);
    if (SolSDP)
        plot(sigma2_s-sigma2_r, 10*log10(mseAvg_u(:,3)),'sm','Linewidth',0.9);
    end;
    plot(sigma2_s-sigma2_r, 10*log10(mseAvg_u(:,5)),'^','Color',[0.4660 0.6740 0.1880],'Linewidth',1.3);
    plot(sigma2_s-sigma2_r, 10*log10(crlbm_u),'-.k','Linewidth',1); hold on; grid on;
    plot(sigma2_s-sigma2_r, 10*log10(mseAvg_u(:,2)),'x','Color',[0.4940 0.1840 0.5560],'Linewidth',1.0);
    if (SolSDP)
        plot(sigma2_s-sigma2_r, 10*log10(mseAvg_u(:,4)),'d','Color',[0.3010 0.7450 0.9330],'Linewidth',0.9);
    end;
    plot(sigma2_s-sigma2_r, 10*log10(mseAvg_u(:,6)),'>','Color',[0.9290 0.6940 0.1250],'Linewidth',1.3);
    if (SolSDP)
        legend({'CRLB-Unknown $\mathbf  s_o$','CFS-Unknown $\mathbf  s_o$','SDP-Unknown $\mathbf  s_o$','MLECFS-Unknown $\mathbf  s_o$','CRLB-Known $\mathbf  s_o$ ','CFS-Known $\mathbf  s_o$','SDP-Known $\mathbf  s_o$','MLECFS-Known $\mathbf  s_o$'}, 'Interpreter', 'latex');
    else
        legend({'CRLB-Unknown $\mathbf  s_o$','CFS-Unknown $\mathbf  s_o$','MLECFS-Unknown $\mathbf  s_o$','CRLB-Known $\mathbf  s_o$ ','CFS-Known $\mathbf  s_o$','MLECFS-Known $\mathbf  s_o$'}, 'Interpreter', 'latex');
    end;
    legend('Location', 'northwest');
    xlabel('20 log($\sigma_{s}/\sigma_{r}$)', 'FontSize', 14,'Interpreter','latex');
    ylabel('10 log(MSE($\mathbf{u}$(m)))', 'FontSize', 12,'Interpreter','latex');
    ylim([-Inf -12]);
end

%% ================================================================================
function [uu,sso,TTr,Omega] = LocByRB_GeoGenerator(N,NumGeo,min_dis)
% [uu,sso,TTr,Omega] = LocByRB_GeoGenerator(N,K,NumGeo,min_dis)
% This function generates localization geometries.
%
% Inputs:
%   N:        scalar, number of transmitters
%   NumGeo:   scalar, number of different geometries to generate
%   min_dis:  scalar, the minimum distance between any two transmitters
%
% Outputs:
%   uu:       (3 x NumGeo), object position
%   sso       (3 x NumGeo), RB reference position
%   TTr       (3 x N x NumGeo), transmitters' positions
%   Omega     (3 x NumGeo), Euler angles of RB orientation

K = 3; % localization dimension
cu = [40,40,50]';
Au = [40,40,25]';
cso = [20,20,5]';
Aso = [40,40,10]';
cTr = [160;160;40];
ATr = [80;80;80];

TTr=zeros(K,N,NumGeo);
Omega = zeros(K,NumGeo);
uu = cu-Au/2+Au.*rand(K,NumGeo);
sso = cso-Aso/2+Aso.*rand(K,NumGeo);

for g = 1:1:NumGeo
    TTr(:,1,g) = cTr-ATr/2+ATr.*rand(K,1);
    for m = 2:1:N
        while 1
            Tr_candidate = cTr-ATr/2+ATr.*rand(K,1);
            dis = norm(Tr_candidate-TTr(:,1,g));
            for j = 2:1:m-1
                dis = min(dis,norm(Tr_candidate-TTr(:,j,g)));
            end
            if (dis>min_dis)
                TTr(:,m,g) = Tr_candidate;
                break;
            end
        end
    end
    Omega(:,g) = 2*pi*rand(K,1);
end
end

%% ================================================================================
function R = Angle2RotM(omega)
% R = Angle2RotM(omega)
% This function generates the rotation matrix using the Euler angles.
%
% Inputs:
%   omega:    (3 x 1), Euler angles
%
% Outputs:
%   R:        (3 x 3), rotation matrix
alpha = omega(1);
beta =  omega(2);
gamma = omega(3);
R_alpha = [cos(alpha),-sin(alpha),0;
           sin(alpha),cos(alpha),0;
           0,0,1];
R_beta = [cos(beta),0,-sin(beta);
          0,1,0;
          sin(beta),0,cos(beta)];
R_gamma = [1,0,0;
           0,cos(gamma),-sin(gamma);
           0,sin(gamma),cos(gamma);];
R = R_alpha*R_beta*R_gamma;

end