% Wireless localization by rigid body receivers of unknown orientations without synchronizations
%
% Proposed estimators in MPR formulation
% - distributed closed-form solution in MPR: DistLocByRBRs_CFSMPR.m 
% - distributed GTRS solution in MPR: DistLocByRBRs_GTRSMPR.m 
% - joint MLE solution in MPR initialized by closed-form solution: JointLocByRBRs_MLECFSMPR.m
%
% The code can reproduce the results in Fig. 4 of the reference paper.
%
% Number of geometries (NumGeo)is set to 2 and number of ensemble runs (NumEns) 
% is set to 10 and to reduce run time of the code. To reproduce the results in
% Fig. 4, set NumGeo = 10 and NumEns = 100.
%
% Reference: X. Ke and K. C. Ho, "Wireless localization in modified polar 
% representation by rigid body receivers of unknown orientations without 
% synchronizations and transmitter positions," IEEE Trans. Wireless Commun., 2025.
%
% X. Ke, K. C. Ho     10-08-2025
%
%       Copyright (C) 2025
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA.
%       hod@missouri.edu

clear all;
%clc;
warning off;

% ----- simulation settings -----
NumGeo = 2;           % number of randomly generated geometries
NumEns = 10;           % number of ensemble runs
% NumGeo = 10;           % number of randomly generated geometries
% NumEns = 100;           % number of ensemble runs

rng('default');
rng(1387321);
sigma2_r= -25:2.5:5;  % 10*log10(noise power of distance measurement)
sigma2_b = 6;         % 10*log10(noise power of RBR position measurement)
min_dis = 45;         % minimum distance between any two RBRs
N_max = 6;            % maximum number of transmitters
N = 2;                % number of transmitters used in this simulation
M_max = 8;            % maximum number of RBRs
M = 6;                % number of RBR used in this simulation
L = 7;                % number of sensors on each RBR
C = 10*[0, 1, 1,  1,  1, -1, -1;
        0, 1, -1, 1,  -1, 1, -1;
        0, 1, 1,  -1, -1, 1, 1]; % sensor positions local to each RBR
[uu,bb,TTr,Omega] = LocByRBRs_GeoGenerator(N_max,M_max,NumGeo,min_dis); % localization geometries
OfTr  =  2000*rand(N_max,NumGeo);  % transmitter offsets caused by asynchronization
OfB  =  2000*rand(M_max,NumGeo);   % RBR offsets caused by asynchronization
for geoIdx = 1:1:NumGeo
    uu(:,geoIdx) = uu(:,geoIdx) - bb(:,1,geoIdx);
    TTr(:,:,geoIdx) =TTr(:,:,geoIdx)-bb(:,1,geoIdx);
    bb(:,:,geoIdx) = bb(:,:,geoIdx)-bb(:,1,geoIdx);
end % The global coordinate system is translated such that the first RBR is at the origin.
TTr = TTr(:,1:N,:);      % transmitter positions used in this simulation
bb =  bb(:,1:M,:);       % RBR positions used in this simulation
Omega = Omega(:,1:M,:);  % RBR orientation angles used in this simulation


% -- noise sequence --
EE_n_r = randn(N*L*M,NumEns);    
EE_n_b = randn(3*M,NumEns);      
EE_n_r = EE_n_r-mean(EE_n_r,2);  % distance measurement noise sequence
EE_n_b = EE_n_b-mean(EE_n_b,2);  % RBR position noise sequence

fprintf('progress:        ');

% -- initialize record for CRLB and estimation results --
NumAlg = 3;                                                    % number of algorithms
crlb_phio = zeros(length(sigma2_r),NumGeo);                    % CRLB for azimuth–elevation angle pairs
crlb_go = zeros(length(sigma2_r),NumGeo);                      % CRLB for inverse-range
crlb_u = zeros(length(sigma2_r),NumGeo);                       % CRLB for object position
Est_rec = zeros(6,NumAlg,NumEns,length(sigma2_r),NumGeo); % estimation results
for geoIdx = 1:1:NumGeo                 % loop through geometries
    for nsePwrIdx = 1:length(sigma2_r)  % loop through measurement noise power
        Tr_o = TTr(:,:,geoIdx);         % true transmitter positions
        u_o = uu(:,geoIdx);             % true object position  
        B_o = bb(:,:,geoIdx);           % true RBR positions
        omega_o = Omega(:,:,geoIdx);    % true RBR orientation angles
        oftr_o = OfTr(:,geoIdx);        % transmitter offsets caused by asynchronization
        ofb_o = OfB(:,geoIdx);          % RBR offsets caused by asynchronization
        
        % -- CRLBs, true measurement vectors, covariance matrices  --
        [Cm_u,Cm_phio,Cm_go,r_o,b_o,Q_r,Q_b] = TOALocByRBRs_CRLB(u_o,Tr_o,B_o,omega_o,C,oftr_o,ofb_o,sigma2_r(nsePwrIdx),sigma2_b);
        crlb_u(nsePwrIdx,geoIdx) = crlb_u(nsePwrIdx,geoIdx) + trace(Cm_u);
        crlb_phio(nsePwrIdx,geoIdx) = crlb_phio(nsePwrIdx,geoIdx) + trace(Cm_phio);
        crlb_go(nsePwrIdx,geoIdx) = crlb_go(nsePwrIdx,geoIdx) + Cm_go;
        
        est_rec = zeros(6,NumAlg,NumEns);          % initialize record for estimate values
        for nn = 1:1:NumEns                     % loop through ensemble runs
            warning('off','all');
            r = r_o+Q_r^(1/2)*EE_n_r(:,nn);        % diastance measurement
            b = b_o + Q_b^(1/2)*EE_n_b(:,nn);      % RBR position measurement
            u = 100000*ones(3,NumAlg);             % initialize record for object position (CR) estimate
            til_u = 10*ones(3,NumAlg);             % initialize record for object position (MPR) estimate

            %-- proposed CFS-MPR estimator --
            [til_u(:,1),u(:,1),tiltheta_cfs,~,~] = TOADtbLocByRBRs_CFSMPR(r,b,C,Q_r,Q_b);
            
            %-- proposed GTRS-MPR estimator --
            [til_u(:,2),u(:,2),~,~,~] = TOADtbLocByRBRs_GTRSMPR(r,b,C,Q_r,Q_b);
            
            %-- proposed MLECFS-MPR estimator --
            [til_u(:,3),u(:,3)]= TOAJntLocByRBRs_MLECFSMPR(r,b,C,Q_r,Q_b,50,0.5,tiltheta_cfs);
            
            est_rec(:,:,nn) = [til_u;u];
        end
        Est_rec(:,:,:,nsePwrIdx,geoIdx) = est_rec;
%        fprintf('progress: %2.1f%%\b\b\b',100*( (geoIdx-1)/NumGeo+(nsePwrIdx)/(NumGeo*length(sigma2_r))))
        fprintf('\b\b\b\b\b\b %3.1f%%',100*( (geoIdx-1)/NumGeo+(nsePwrIdx)/(NumGeo*length(sigma2_r))))
    end
end
fprintf('\n');

% -- compute the averaged MSEs and CRLBs --
TrueVal = [atan2(uu(2,:),uu(1,:));asin(uu(3,:)./sqrt(ones(1,3)*(uu.^2)));1./sqrt(ones(1,3)*(uu.^2));uu];
Error_Rec = Est_rec - reshape(TrueVal,[6,1,1,1,NumGeo]);
Error_Rec(1:2,:,:,:,:) = wrapToPi(Error_Rec(1:2,:,:,:,:));
MSE_Rec = reshape(mean(Error_Rec.^2,3),[6,NumAlg,length(sigma2_r),NumGeo]);
mse_phio = reshape(mean(sum(MSE_Rec(1:2,:,:,:),1),4),[NumAlg,length(sigma2_r)])'; 
mse_go = reshape(mean(MSE_Rec(3,:,:,:),4),[NumAlg,length(sigma2_r)])';
mse_u = reshape(mean(sum(MSE_Rec(4:6,:,:,:),1),4),[NumAlg,length(sigma2_r)])';
meancrlb_phio = mean(crlb_phio,2);
meancrlb_go = mean(crlb_go,2);
meancrlb_u = mean(crlb_u,2);

%% 
% -- create plot to show the results --
close all;
figure(1)
fig_width = 500;  % Width of the figure window in pixels
fig_height = 580; % Height of the figure window in pixels
set(gcf, 'Position', [100, 100, fig_width, fig_height]);
x = sigma2_r;
for i=1:1:3
    subplot(3,1,i)
    if i==1
        y = mse_phio;
        meancrlb = meancrlb_phio;
    elseif i==2
        y = mse_go;
        meancrlb = meancrlb_go;
    else
        y = mse_u;
        meancrlb = meancrlb_u;
    end
    p1 = plot(x, 10*log10(meancrlb),'-r','Linewidth',1.1); hold on; grid on;
    p2 = plot(x, 10*log10(y(:,1)),'>','Linewidth',1.0,'MarkerSize',7.0,'Color',[0 0.4470 0.7410]); hold on; grid on;
    p3 = plot(x, 10*log10(y(:,2)),'d','Linewidth',1.2,'MarkerSize',7.5,'Color',[0 0.4470 0.7410]);
    p4 = plot(x, 10*log10(y(:,3)),'x','Linewidth',1.4,'MarkerSize',7.5,'Color',[0 0.4470 0.7410]);
    xlabel('20 log($\sigma_{r}$ (m))', 'FontSize', 14,'Interpreter','latex');
    xlim([x(1),x(end)])
    if i==1
        ylim([10*log10(meancrlb(1))-2 10*log10(meancrlb(end))+8]);
        legend([p1,p2,p3,p4],'CRLB','CFS-MPR','GTRS-MPR','MLECFS-MPR','Interpreter', 'latex','location','northwest');
        ylabel('10 log(MSE(\boldmath${\phi}_o$ (rad.)))', 'FontSize', 12,'Interpreter','latex');
    elseif i==2
        ylim([10*log10(meancrlb(1))-2 10*log10(meancrlb(end))+11]);
        ylabel('10 log(MSE($g_o$ (m$^{-1}$)))', 'FontSize', 12,'Interpreter','latex');
    else
        ylim([10*log10(meancrlb(1))-2 10*log10(meancrlb(end))+7]);
        ylabel('10 log(MSE($\mathbf u$ (m)))', 'FontSize', 12,'Interpreter','latex');
    end
end

%% ================================================================================
function [uu,bb,TTr,Omega] = LocByRBRs_GeoGenerator(N,M,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
%   M:        scalar, number of RBRs
%   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
%   bb        (3 x M x NumGeo), RBR positions
%   TTr       (3 x N x NumGeo), transmitter positions
%   Omega     (3 x M x NumGeo), RBR orientation angles

TTr=zeros(3,N,NumGeo);
cu = [200,200,50]';
Au = [50,50,50]';
cTr = 1*[800;800;50];
ATr = [200;200;40];
uu = cu-Au/2+Au.*rand(3,NumGeo);
bb = zeros(3,M,NumGeo);
g=1;
while (g<=NumGeo)
    flag=0;
    TTr(:,:,g) = cTr-ATr/2+ATr.*rand(3,N);
    for i = 1:1:M
        ita = 0;
        while 1
            ita = ita+1;
            azm = 2*pi*rand(1,1);
            ele = -0*pi-0.5*pi*rand(1,1);
            dr = 50+10*rand(1,1); 
            tsso = dr*[cos(azm)*cos(ele);sin(azm)*cos(ele);sin(ele)];
            tsso = tsso+uu(:,g);
            eps = 200;
            for jj = 1:1:i-1
                eps = min(eps,norm(tsso - bb(:,jj,g)));
            end
            if eps>=min_dis
                bb(:,i,g) = tsso;
                break;
            end
            if ita>=5000
                flag=1;
                break;
            end
        end
        if flag==1
            break;
        end
    end
    if flag==0
%        fprintf('Geometry generating progress: %2.1f%% \n',100*g/NumGeo)
        g=g+1;
    end
end
Omega = 2*pi*rand(3,M,NumGeo);
end

% %% ================================================================================
% function [Cm_u,Cm_phio,Cm_go,r_o,b_o,Q_r,Q_b] = LocByRBRs_CRLB(u_o,Tr_o,B_o,omega_o,C,of_tro,of_bo,sigma2_r,sigma2_b)
% % [crlb_u,crlb_phio,crlb_go,r_o,b_o,Qr,Qb] = LocByRBRs_CRLB(u_o,Tr_o,B_o,omega_o,C,of_tro,of_bo,sigma2_r,sigma2_b)
% % This function computes the CRLBs, true measurement vectors, and covariance matrices.
% %
% % Inputs:
% %   u_o:          (3 x 1), object position
% %   Tr_o:         (3 x N), transmitter positions
% %   B_o:          (3 x M), RBR positions
% %   omega_o:      (3 x M), RBR orientation angles
% %   C:            (3 x L), sensor position local to each RBR
% %   of_tro:       (1 x N), range offset due to transmitter asynchronization
% %   of_bo:        (1 x M), range offset due to RBR asynchronization
% %   sigma2_r:     scaler, 10*log10(noise power of distance measurement)
% %   sigma2_b:     scaler, 10*log10(noise power of RBR position measurement)
% %
% % Outputs:
% %   Cm_u:         (3 x 3), CRLB for object position
% %   Cm_phio:      (2 x 2), CRLB for azimuth-elevation angles
% %   Cm_go         scaler, CRLB for inverse-range
% %   r_o           (MNL x 1), true distance measurement vector
% %   b_o           (3M x 1), true RBR position measurement vector
% %   Q_r           (MNL x MNL), covariance matrix of distance measurement
% %   Q_b           (3M x 3M), covariance matrix of RBR position measurement
% 
% [~,N] = size(Tr_o);
% [~,M] = size(B_o);
% [~,L] = size(C);
% til_uo = [atan2(u_o(2),u_o(1));asin(u_o(3)/norm(u_o));1/norm(u_o)]; 
% K_uo = -til_uo(3)*[sin(til_uo(1))/cos(til_uo(2)),-cos(til_uo(1))/cos(til_uo(2)),0;
%     cos(til_uo(1))*sin(til_uo(2)),sin(til_uo(1))*sin(til_uo(2)),-cos(til_uo(2));
%     til_uo(3)*cos(til_uo(1))*cos(til_uo(2)),til_uo(3)*sin(til_uo(1))*cos(til_uo(2)),til_uo(3)*sin(til_uo(2))];
% K_thetao = blkdiag(K_uo,eye(2*M+3*(M-M)+M+3*M+N-1));
% r_o = zeros(L*N*M,1);
% d_o = zeros(L*N*M,1);
% Jr_p = zeros(L*N*M,3*M);
% Jr_deltat = zeros(L*N*M,N-1);
% Jr_deltab = zeros(L*N*M,M);
% Jp_u = zeros(3*M,3);
% Jp_phi = zeros(3*M,2*M);
% Jp_b = zeros(3*M,3*M);
% b_o = zeros(3*M,1);
% id = 0;
% for m = 1:1:M
%     bm_o = B_o(:,m);
%     b_o((m-1)*3+1:3*m,1) = bm_o;
%     Rm_o = Angle2RotM(omega_o(:,m));
%     Sm_o = Rm_o*C+bm_o;
%     pm_o = Rm_o'*(u_o-bm_o);
%     phim_o(1,1) = atan2(pm_o(2),pm_o(1));
%     phim_o(2,1) = asin(pm_o(3)/norm(pm_o));
%     Jp_u((m-1)*3+1:3*m,1:3) = pm_o/norm(pm_o) * (u_o-bm_o)'/norm(u_o-bm_o);
%     Jp_b((m-1)*3+1:3*m,(m-1)*3+1:3*m) = -pm_o/norm(pm_o) * (u_o-bm_o)'/norm(u_o-bm_o);
%     Jp_phi((m-1)*3+1:3*m,(m-1)*2+1:2*m) = norm(u_o-bm_o)*[-sin(phim_o(1,1))*cos(phim_o(2,1)),-cos(phim_o(1,1))*sin(phim_o(2,1));
%         cos(phim_o(1,1))*cos(phim_o(2,1)),-sin(phim_o(1,1))*sin(phim_o(2,1));
%         0,cos(phim_o(2,1))];
%     for i = 1:1:L
%         for k = 1:N
%             id = id+1;
%             r_o(id,1) = norm(u_o-Tr_o(:,k)) + norm(u_o-Sm_o(:,i))+of_tro(k)+of_bo(m);
%             d_o(id,1) = norm(u_o-Sm_o(:,i));
%             Jr_p(id,(m-1)*3+1:3*m) = (pm_o-C(:,i))'/norm(pm_o-C(:,i));
%             if k>=2
%                 Jr_deltat(id,k-1) = 1;
%             end
%             Jr_deltab(id,m) = 1;
%         end
%     end
% end
% Jm_theta = [Jr_p*Jp_u,Jr_p*Jp_phi,Jr_deltat,Jr_deltab,Jr_p*Jp_b;
%             zeros(3*M,3+2*M+N+M-1),eye(3*M)];
% bar_Qr = 0.5*diag(d_o.^2)+0.5*(d_o*d_o');
% bar_Qr_temp = zeros(N*L*M);
% for mid=1:1:M
%     idm = N*L*(mid-1)+1:N*L*mid;
%     bar_Qr_temp(idm,idm) = bar_Qr(idm,idm);
% end
% bar_Qr = bar_Qr_temp;
% bar_Qr = N*L*M*bar_Qr/trace(bar_Qr);
% bar_Qb = eye(3*M);
% Q_r = 10^(sigma2_r/10)*bar_Qr;
% Q_b = 10^(sigma2_b/10)*bar_Qb;
% Cm_theta = inv(Jm_theta'/blkdiag(Q_r,Q_b)*Jm_theta);
% Cm_tiltheta = K_thetao*Cm_theta*K_thetao';
% Cm_u = Cm_theta(1:3,1:3);
% Cm_phio = Cm_tiltheta(1:2,1:2);
% Cm_go = Cm_tiltheta(3,3);
% 
% 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
% alp = omega(1); 
% beta = omega(2); 
% gamma = omega(3);
% R_alp = [1,0,0;
%          0,cos(alp),-sin(alp);
%          0,sin(alp),cos(alp)];
% R_beta = [cos(beta),0,-sin(beta);
%           0,1,0;
%           sin(beta),0,cos(beta)];
% R_gamma = [cos(gamma),-sin(gamma),0;
%            sin(gamma),cos(gamma),0,
%            0,0,1];
% R = R_gamma*R_beta*R_alp;
% end
