% This program gives an example of how to call the localization routine for 
% AOA rigid body localization for generating Figs. 3-10 in the paper
% Y. Wang, G. Wang, S. Chen, K. C. Ho and L. Huang, "An investigation and
% solution of angle based rigid body localization," IEEE Trans. Signal
% Process., vol. 68, pp. 5457-5472, 2020.
%
% NOTE: the code may not reproduce the exact same figures in the paper,
%       especially Figs. 3-6, due to variation in inialization of the
%       random number generator.
%
% NOTE: only 20 ensemble runs (L=10) are used in the current code setting
%       to reduce the running time, the results in the paper use L=200.
%
% Yue Wang, K. C. Ho   04-18-2021
%
%       Copyright (C) 2020
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA
%       hod@missouri.edu
%

clear all;
warning off;
rng(123456);

% ==============================================================
%   choosing figures to generate
%       fig='3-4';  % single sensor self-localization
%       fig='5-6';  % multiple sensor self-localization
%       fig='7-8';  % single anchor localization
%       fig='9-10'; % multiple anchor localization
% ==============================================================
fig='3-4';

% -- cases for different figures --
switch fig,
    case '3-4',
        Loc=0; M=6; N=1; ar=100;   % single sensor self-localization
    case '5-6',
        Loc=0; M=6; N=5; ar=100; 	% multiple sensor self-localization
    case '7-8',
        Loc=1; M=1; N=5; ar=20;     % single anchor localization
    case '9-10',
        Loc=1; M=6; N=5; ar=100;    % multiple anchor localization
end;

% -- simulation settings --
L=20;   %200;      % number of ensemble runs
K=3;        % localization dimension
geoNum=50;	% number of geometries
if (M==1 | N==1)
    NsePwrVecLg=-60:5:0;   % noise level in log scale, i.e. 20log(noisePower)
else
    NsePwrVecLg=-40:5:20;   % noise level in log scale, i.e. 20log(noisePower)
end;

% -- generating randomly geoNum geometries --
geonum=0;
while geonum<geoNum
    A = ar*(rand(3, M)-1/2);
    kk = 0;
    if M>1
        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
            geonum = geonum + 1;
            at(:,:,geonum) = A ;
        end
    else
        geonum = geonum + 1;
        at(:,:,geonum) = A;
    end
end

% -- sensor locations w.r.t. to the rigid body --
c=3*[0.5,1.5,1.5,0.5,1;
    0,0,1.5,1.5,1;
    0,0,0,0,1];
c=c(:,1:N);
c=c-c(:,1)*ones(1,N);

% -- true location of the rigid body --
thetao=20*pi/180;phio=-25*pi/180;psio=10*pi/180;    % rotation angles
if M>1         % translation vector
    to=0.5*[100,100,50]';
else
    to=[8,8,4]';
end
Qo=[cos(thetao)*cos(psio) cos(thetao)*sin(psio) -sin(thetao);
    sin(phio)*sin(thetao)*cos(psio)-cos(phio)*sin(psio) sin(phio)*sin(thetao)*sin(psio)+cos(phio)*cos(psio) cos(thetao)*sin(phio);
    cos(phio)*sin(thetao)*cos(psio)+sin(phio)*sin(psio) cos(phio)*sin(thetao)*sin(psio)-sin(phio)*cos(psio) cos(thetao)*cos(phio)];

% -- try all geometries --
for geoIdx=1:geoNum
    a=at(:,:,geoIdx);
    
    % -- true measurement values --
    if (Loc==0)
        a_beta=Qo*a+to*ones(1,M);
        for i=1:M
            for j=1:N
                temp0=a_beta(:,i)-c(:,j);
                thetamo(i,j)=atan2(temp0(2),temp0(1));
                phimo(i,j)=atan2(temp0(3),norm(temp0(1:2),2));
            end
        end
    else
        c_beta=Qo*c+to*ones(1,N);
        for i=1:M
            for j=1:N
                temp0=a(:,i)-c_beta(:,j);
                if (M>1) temp0=-temp0; end; % true measurement to work with the positioning function            
                thetamo(j,i)=atan2(temp0(2),temp0(1));
                phimo(j,i)=atan2(temp0(3),norm(temp0(1:2),2));
            end
        end
    end;
    
    % -- noise for ensemble runs --
    R=eye(2*M*N);
    for j=1:N
        for i=1:M
            R(M*(j-1)+i,M*N+M*(j-1)+i)=0.5;
            R(M*N+M*(j-1)+i,M*(j-1)+i)=0.5;
        end
    end
    rn=randn(2*M*N,L);
    mn=mean(rn,2);
    zmn=rn-mn*ones(1,L);
    
    fprintf('Geometry: %d/%d, 20log10(NsePwr):',geoIdx,geoNum);
    
    % -- go over several noise levels --
    for nseIdx=1:length(NsePwrVecLg)
        fprintf(' %d.',NsePwrVecLg(nseIdx));
        nsePwr(nseIdx) = 10^(NsePwrVecLg(nseIdx)/10);
        Qn = (sqrt(nsePwr(nseIdx))/180*pi)^2 * R;   % AOA noise covariance matrix
        
        % obtain CRLB
        CRLB = AOARBL3D_CRLB(a, c, Qo, to, Qn, Loc);
        tcrlbt(nseIdx,geoIdx) = trace(CRLB(end-K+1:end, end-K+1:end));
        Qcrlbt(nseIdx,geoIdx) = trace(CRLB(1:K^2, 1:K^2));
        
        SimulationMSEQ_new=0;
        SimulationMSEt_new=0;
        SimulationMSEQ_BRPLE=0;
        SimulationMSEt_BRPLE=0;
        
        uQ_new=zeros(K,K,L);
        ut_new=zeros(K,L);
        uQ_BRPLE=zeros(K,K,L);
        ut_BRPLE=zeros(K,L);
        
        % perform ensemble runs
        parfor k = 1 : L,
            %          for k = 1 : L,
            % AOA measurements
            nzmn=sqrtm(Qn)*zmn(:,k);
            thetam=thetamo+reshape(nzmn(1:M*N),size(thetamo,1),size(thetamo,2));
            phim=phimo+reshape(nzmn(M*N+1:2*M*N),size(phimo,1),size(phimo,2));
            
            if (M==1 | N==1)
                % proposed method
                [uQ_new(:,:,k),ut_new(:,k)] = AOARBL3D_SDR(a,c,thetam,phim,Qn,Loc);
            else
                % proposed method
                [uQ_new(:,:,k),ut_new(:,k)] = AOARBL3D_SDR(a,c,thetam,phim,Qn,Loc);
                % BRPLE method
                [uQ_BRPLE(:,:,k),ut_BRPLE(:,k)] = AOARBL3D_BRPLE(a,c,thetam,phim,Qn,Loc);
            end;
            
            SimulationMSEQ_new = SimulationMSEQ_new + norm(uQ_new(:,:,k)-Qo,'fro')^2;
            SimulationMSEt_new = SimulationMSEt_new + norm(ut_new(:,k)-to,2)^2;
            SimulationMSEQ_BRPLE = SimulationMSEQ_BRPLE + norm(uQ_BRPLE(:,:,k)-Qo,'fro')^2;
            SimulationMSEt_BRPLE = SimulationMSEt_BRPLE + norm(ut_BRPLE(:,k)-to,2)^2;
        end
        
        % average over ensemble runs
        mseQ_newt(nseIdx,geoIdx) = SimulationMSEQ_new/L;
        mset_newt(nseIdx,geoIdx) = SimulationMSEt_new/L;
        mseQ_BRPLEt(nseIdx,geoIdx) = SimulationMSEQ_BRPLE/L;
        mset_BRPLEt(nseIdx,geoIdx) = SimulationMSEt_BRPLE/L;
    end
    fprintf('\n');
end;

% average over geometries
mseQ_new=mean(mseQ_newt,2);
mset_new=mean(mset_newt,2);
mseQ_BRPLE=mean(mseQ_BRPLEt,2);
mset_BRPLE=mean(mset_BRPLEt,2);
Qcrlb = mean(Qcrlbt,2);
tcrlb = mean(tcrlbt,2);

% plot results
AOARBL3D_PlotFig(NsePwrVecLg,Qcrlb,mseQ_new,mseQ_BRPLE,tcrlb,mset_new,mset_BRPLE,M,N,Loc);

% -------------------------------------------------------------------
