% This program generates the results for AOA MLE-MPR.
% The figure generated from this code corresponds to Fig. 7 in the reference paper.
%
% NOTE: matlab CVX toobox is needed
% NOTE: slow processing time is mainly caused by running CVX to obtain the
% initial solution by SDR
% NOTE: can change L (number of ensemble runs) to a smaller value to obtain the results quicker 
%
% Reference: Y. Wang and K. C. Ho, “Unified near-field and far-field localization for
% AOA and hybrid AOA-TDOA positionings,” IEEE Trans. Wireless Commun., vol. 17, no. 2,
% pp. 1242-1254, Feb. 2018.
%
% Yue Wang, K. C. Ho    03-18-2019
%
%       Copyright (C) 2019
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA.
%       hod@missouri.edu
%

clear all;
%close all;
warning off;                  % Program initialization
addpath('C:\rsh\cvx')
cvx_setup

%L = 1000;                               % Number of ensemble runs
L = 200;                               % Number of ensemble runs

randn('seed',0);                        % Initialize random number generator
rand('seed',303);

% -- simulation parameters --

% sensor positons
x=[0   -4.6400   -4.9400   -5.0500   -0.2800   -5.4400   -5.4000   -5.9700   -8.1100   -3.4600];
y=[0    6.6000   -2.1300    3.8400    5.0700    7.0400    6.6300    5.8200   -0.2800    0.4100];
z=[0    1.5500    2.1500    1.8500   -4.3100    3.1000   -5.4900   -5.9700   -4.0800   -4.0800];
s=[x; y; z];

M = size(s,2);                          % Number of sensors
N = size(s,1);                          % Dimension of localization

% measurement noise covariance structure
R_theta = eye(M);
R_phi = eye(M);
R=[R_theta, zeros(M,M); zeros(M,M), R_phi];
for i=1:M
    R(i,M+i)=0.5;
    R(M+i,i)=0.5;
end

NsePwrVecdB=-20;    % noise level, 10 log(\sigma^2)

rVec=10:10:160;      % source range

% source arrival angle
thetao=(101.31/180)*pi;
phio=(30.47/180)*pi;

rtheta=randn(M,L);       % Azimuth angle noise
rphi=randn(M,L);         % Elevation angle noise
ntheta=rtheta-mean(rtheta,2)*ones(1,L);
nphi=rphi-mean(rphi,2)*ones(1,L);

fprintf('Simulation in progress ');
for rIdx=1:length(rVec)
    randn('seed',2^20);
    r=rVec(rIdx);
    uoTilde=[thetao phio 1/r]';    % true source location in MPR
    phi1o=[cos(phio)*cos(thetao) cos(phio)*sin(thetao) sin(phio) 1/r];
    phi2o=[(cos(phio)*cos(thetao))^2 cos(phio)^2];
    uo=[0 0 0]';
    [uo(1) uo(2) uo(3)]=sph2cart(thetao,phio,r);    % Cartesian Source Location
    
    ro = sqrt(sum((uo*ones(1,M)-s).^2))';
    for i=1:M                   % true AOAs
        mo(:,i)=(uo-s(:,i))/ro(i);
        [thetamo(i) phimo(i) rr(i)]=cart2sph(mo(1,i),mo(2,i),mo(3,i));
    end
    
    % -- perform simulation runs --
    for nseIdx=1:1              % over noise levels
        fprintf('.');
        nsePwr = 10^(NsePwrVecdB(nseIdx)/10);
        
        sigma2_theta=nsePwr;
        sigma2_phi=nsePwr;
        Q=R*sigma2_theta;
        
        % evaluate HBB bound
        [HBB,CRB,HCR,HBBxy,CRBxy,HCRxy] = AOALoc_HBB_3D(s,uo,Q);
        
        hbb_invr(rIdx)=HBB(1,1);
        hbb_theta(rIdx)=HBB(2,2);
        hbb_phi(rIdx)=HBB(3,3);
        hbb_angle(rIdx)=hbb_theta(rIdx)+hbb_phi(rIdx);
        
        hbb_xy(rIdx)=trace(HBBxy);
        cb_xy(rIdx)=trace(CRBxy);
        
        SimulationMSE_SDRMPR_invr = 0;
        SimulationMSE_SDRMPR_angle=0;
        SimulationMSE_MLEMPR_invr = 0;
        SimulationMSE_MLEMPR_angle =0;
        
        for k = 1 : L,                      % Monte Carlo Simulation
            dm=sqrtm(Q)*[ntheta(:,k);nphi(:,k);];
            thetam=thetamo'+dm(1:M);
            phim=phimo'+dm(M+1:2*M);
            a=[thetam;phim];                % measured AOAs
            uTildeSDR=AOA_SDRMPR_3D(s,a,Q);        % SDR solution
            [uTildeMLE] = AOA_MLEMPR(uTildeSDR,s,a,Q);   % MLE solution
            theta_uTildeMLER(k)=uTildeMLE(1);
            phi_uTildeMLER(k)=uTildeMLE(2);
            
            SimulationMSE_SDRMPR_invr = SimulationMSE_SDRMPR_invr + (uTildeSDR(3)-uoTilde(3))^2;
            SimulationMSE_SDRMPR_angle = SimulationMSE_SDRMPR_angle+norm([uTildeSDR(1) uTildeSDR(2)]-[uoTilde(1) uoTilde(2)],2)^2;
            SimulationMSE_MLEMPR_invr = SimulationMSE_MLEMPR_invr + (uTildeMLE(3)-uoTilde(3))^2;
            SimulationMSE_MLEMPR_angle = SimulationMSE_MLEMPR_angle+norm([uTildeMLE(1) uTildeMLE(2)]-[uoTilde(1) uoTilde(2)],2)^2;
        end;
        
        % mse
        mse_SDRMPR_angle(rIdx) = SimulationMSE_SDRMPR_angle/L;
        mse_SDRMPR_invr(rIdx) = SimulationMSE_SDRMPR_invr/L;
        mse_MLEMPR_invr(rIdx) = SimulationMSE_MLEMPR_invr/L;
        mse_MLEMPR_angle(rIdx) = SimulationMSE_MLEMPR_angle/L;
    end;
end;

fprintf('\n');

% -- plot results --
figure;
subplot(2,1,1)
plot(rVec,10*log10(mse_MLEMPR_invr),'xk','MarkerSize',8); hold on;
plot(rVec,10*log10(hbb_invr),'k'); hold on;grid on;
xlabel('r(m)');
ylabel('10 log(MSE(1/r)(m^2))');
ylim([-55 -40])
legend('MLE-MPR','HBB Bound','Location','southeast');
subplot(2,1,2)
plot(rVec,10*log10(mse_MLEMPR_angle),'xk','MarkerSize',8); hold on;
plot(rVec,10*log10(hbb_angle),'k'); hold on;grid on;
xlabel('r(m)');
ylabel('10 log(MSE([\theta \phi])(rad^2))');
ylim([-35 -20])
legend('MLE-MPR','HBB Bound','Location','southeast');


