% This program generates the results for AOA-TDOA MLE-MPR.
% The figure generated from this code corresponds to Figs. 8 and 9 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
%

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

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

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

% -- simulation setting --
% sensor position
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_aoa = [R_theta,zeros(M,M);zeros(M,M),R_phi];
for i=1:M
    R_aoa(i,M+i)=0.5;
    R_aoa(M+i,i)=0.5;
end
R_tdoa = (eye(M-1)+ones(M-1,M-1))/2;
R=[R_tdoa, zeros(M-1,2*M); zeros(2*M,M-1),R_aoa];
NsePwrVecdB=-70:10:20;  % noise level, 10 log(sigma^2)
rVec=80;     % source range

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

fprintf('Simulation in progress ');

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

% -- ensemble runs --
for rIdx=1:length(rVec)    % loop over source range
    r=rVec(rIdx);
    uoTilde=[thetao phio 1/r]';    % source location in MPR
    uo=[0 0 0]';
    [uo(1) uo(2) uo(3)]=sph2cart(thetao,phio,r); % source location in Cartesian
    
    ro = sqrt(sum((uo*ones(1,M)-s).^2))';
    rdo = ro(2:end)-ro(1);  % true range difference
    
    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
    
    for nseIdx=1:length(NsePwrVecdB)    % loop over noise level
        fprintf('.');
        sigma2_theta = 10^(NsePwrVecdB(nseIdx)/10);
        sigma2_phi = 10^(NsePwrVecdB(nseIdx)/10);
        sigma2_tdoa = 10^(NsePwrVecdB(nseIdx)/10);
        
        Q=R*sigma2_theta;
        [HBB,CRB,HCR,HBBxy,CRBxy,HCRxy] = AOATDOALoc_HBB_3D(s,uo,Q);
        cb_invr(nseIdx)=CRB(1,1);
        cb_theta(nseIdx)=CRB(2,2);
        cb_phi(nseIdx)=CRB(3,3);
        cb_angle(nseIdx)=cb_theta(nseIdx)+cb_phi(nseIdx);
        
        SimulationMSE_MLECAR_invr = 0;
        SimulationMSE_SDRMPR_invr = 0;
        SimulationMSE_MLEMPR_invr = 0;
        SimulationMSE_MLECAR_angle = 0;
        SimulationMSE_SDRMPR_angle = 0;
        SimulationMSE_MLEMPR_angle =0;
        
        for k = 1 : L,                      % Monte Carlo Simulation
            dm=sqrtm(Q)*[ntdoa(:,k);ntheta(:,k);nphi(:,k)];
            rd=rdo+dm(1:M-1);       % measured range differences
            thetam=thetamo'+dm(M:2*M-1);
            phim=phimo'+dm(2*M:3*M-1);
            a=[thetam;phim];        % measured AOAs
            
            ui=uo+r*randn(3,1)/100;
            
            [u_MLECAR uTilde_MLECAR] = AOATDOA_MLECAR_3D(uo,s,rd,a,Q);
            uTilde_SDRMPR = AOATDOA_SDRMPR_3D(s,rd,a,Q);
            [uTilde_MLEMPR] = AOATDOA_MLEMPR(uTilde_SDRMPR,s,rd,a,Q);
            
            uTilde_MLECAR0(:,k)=uTilde_MLECAR;
            uTilde_SDRMPR0(:,k)=uTilde_SDRMPR;
            uTilde_MLEMPR0(:,k)=uTilde_MLEMPR;
            
            SimulationMSE_MLECAR_invr = SimulationMSE_MLECAR_invr + (uTilde_MLECAR(3)-uoTilde(3))^2;
            SimulationMSE_SDRMPR_invr = SimulationMSE_SDRMPR_invr + (uTilde_SDRMPR(3)-uoTilde(3))^2;
            SimulationMSE_MLEMPR_invr = SimulationMSE_MLEMPR_invr + (uTilde_MLEMPR(3)-uoTilde(3))^2;
            SimulationMSE_MLECAR_angle = SimulationMSE_MLECAR_angle+norm([uTilde_MLECAR(1) uTilde_MLECAR(2)]-[uoTilde(1) uoTilde(2)],2)^2;
            SimulationMSE_SDRMPR_angle = SimulationMSE_SDRMPR_angle+norm([uTilde_SDRMPR(1) uTilde_SDRMPR(2)]-[uoTilde(1) uoTilde(2)],2)^2;
            SimulationMSE_MLEMPR_angle = SimulationMSE_MLEMPR_angle+norm([uTilde_MLEMPR(1) uTilde_MLEMPR(2)]-[uoTilde(1) uoTilde(2)],2)^2;
            
        end;
        mse_MLECAR_invr(nseIdx) = SimulationMSE_MLECAR_invr/L;
        mse_SDRMPR_invr(nseIdx) = SimulationMSE_SDRMPR_invr/L;
        mse_MLEMPR_invr(nseIdx) = SimulationMSE_MLEMPR_invr/L;
        mse_MLECAR_angle(nseIdx) = SimulationMSE_MLECAR_angle/L;
        mse_SDRMPR_angle(nseIdx) = SimulationMSE_SDRMPR_angle/L;
        mse_MLEMPR_angle(nseIdx) = SimulationMSE_MLEMPR_angle/L;
        
        m_uTilde_MLECAR=mean(uTilde_MLECAR0,2);
        m_uTilde_SDRMPR=mean(uTilde_SDRMPR0,2);
        m_uTilde_MLEMPR=mean(uTilde_MLEMPR0,2);
        
        b_uTilde_MLECAR_g(nseIdx)=norm(m_uTilde_MLECAR(3)-uoTilde(3),2)^2;
        b_uTilde_MLECAR_a(nseIdx)=norm(m_uTilde_MLECAR(1:2)-uoTilde(1:2),2)^2;
        b_uTilde_SDRMPR_g(nseIdx)=norm(m_uTilde_SDRMPR(3)-uoTilde(3),2)^2;
        b_uTilde_SDRMPR_a(nseIdx)=norm(m_uTilde_SDRMPR(1:2)-uoTilde(1:2),2)^2;
        b_uTilde_MLEMPR_g(nseIdx)=norm(m_uTilde_MLEMPR(3)-uoTilde(3),2)^2;
        b_uTilde_MLEMPR_a(nseIdx)=norm(m_uTilde_MLEMPR(1:2)-uoTilde(1:2),2)^2;
    end;
end;

fprintf('\n');

% -- plot results --
figure;
subplot(2,1,1)
plot(NsePwrVecdB,10*log10(cb_invr),'k'); hold on;
plot(NsePwrVecdB,10*log10(mse_MLECAR_invr),'sk','MarkerSize',8); hold on;
plot(NsePwrVecdB,10*log10(mse_SDRMPR_invr),'^k','MarkerSize',8); hold on;
plot(NsePwrVecdB,10*log10(mse_MLEMPR_invr),'xk','MarkerSize',8); hold on;
hold off;grid on;
xlabel('10 log(\sigma_n^2)');
ylabel('10 log(MSE(1/r)(m^-^2))');
xlim([-70,20])
ylim([-110 0])
legend('CRLB','MLE-Cartesian','SDR-MPR','MLE-MPR','location','southeast');
subplot(2,1,2)
plot(NsePwrVecdB,10*log10(cb_angle),'k'); hold on;
plot(NsePwrVecdB,10*log10(mse_MLECAR_angle),'sk','MarkerSize',8); hold on;
plot(NsePwrVecdB,10*log10(mse_SDRMPR_angle),'^k','MarkerSize',8); hold on;
plot(NsePwrVecdB,10*log10(mse_MLEMPR_angle),'xk','MarkerSize',8); hold on;
hold off;grid on;
xlabel('10 log(\sigma_r^2)');
ylabel('10 log(MSE([\theta \phi])(rad^2))');
xlim([-70 20]);
ylim([-100 20])
legend('CRLB','MLE-Cartesian','SDR-MPR','MLE-MPR','location','southeast');

figure;
subplot(2,1,1)
plot(NsePwrVecdB,10*log10(b_uTilde_MLECAR_g),'sk','MarkerSize',8); hold on;
plot(NsePwrVecdB,10*log10(b_uTilde_SDRMPR_g),'^k','MarkerSize',8); hold on;
plot(NsePwrVecdB,10*log10(b_uTilde_MLEMPR_g),'xk','MarkerSize',8); hold on;
hold off;grid on;
xlabel('10 log(\sigma_r^2)');
ylabel('20 log(bias(1/r)(m^-^1))');
xlim([-70,20]);
ylim([-250 0]);
legend('MLE-Cartesian','SDR-MPR','MLE-MPR','Location','Southeast');
subplot(2,1,2)
plot(NsePwrVecdB,10*log10(b_uTilde_MLECAR_a),'sk','MarkerSize',8); hold on;
plot(NsePwrVecdB,10*log10(b_uTilde_SDRMPR_a),'^k','MarkerSize',8); hold on;
plot(NsePwrVecdB,10*log10(b_uTilde_MLEMPR_a),'xk','MarkerSize',8); hold on;
hold off;grid on;
xlabel('10 log(\sigma_r^2)');
ylabel('20 log(bias([\theta \phi](rad)))');
xlim([-70 20]);
ylim([-200 50])
legend('MLE-Cartesian','SDR-MPR','MLE-MPR','Location','Southeast');
