% This program provides an example on how to call AOA3DLoc_BRPLE.m
%
% Reference: Y. Wang and K. C. Ho, “An asymptotically efficient estimator 
%  in closed-Form for 3-D AOA localization using a sensor network," IEEE 
%  Trans. Wireless Commun., vol. 14, no. 12, pp. 6524-6535, Dec. 2015.
%
% The generated figures from this code are supposed to correspond to 
% Figs. 4-5 (NseCorr=0) and Figs. 8-9 (NseCorr=1) in the reference.  
% Due to the possible difference in the initialization of the random 
% number generators in matlab, the plots produced may not be exactly 
% the same as what are shown in the paper.
%
% Yue Wang, K. C. Ho    02-18-2016
%
%       Copyright (C) 2016
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA.
%       hod@missouri.edu
%

clear all; warning off;         % program initialization.
M=10;                           % number of sensors, at least 3
L=1000;                         % number of ensemble runs
D=100;                          % number of geometries
% D=1;                            % one geometry
NseCorr=0;                      % correlated noise(1), uncorrelated noise(0)

thetao=zeros(M,1);
phio=zeros(M,1);
rand('seed',123456);            % noise initialization
for d=1:D,
    Tuo(:,d)=500*rand(3,1)-250*[1 1 1]';
    Tso(:,:,d)=100*rand(3,M)-50*[1 1 1]'*ones(1,M);
end
for d=1:D,
    fprintf('Geometry: %d/%d\n',d,D);
    uo=Tuo(:,d);
    so=Tso(:,:,d);
    randn('state',0);        % noise initialization
    rtheta=randn(M,L);       % azimuth angle noise
    rphi=randn(M,L);         % elevation angle noise
    rsAll=randn(3*M,L);      % sensor position error
    
    % obtain zero mean measurement noise
    ntheta=rtheta-mean(rtheta,2)*ones(1,L);
    nphi=rphi-mean(rphi,2)*ones(1,L);
    
    for i=1:M
        [thetao(i) phio(i) rr(i)]=cart2sph(uo(1)-so(1,i),uo(2)-so(2,i),uo(3)-so(3,i));    % true AOA
    end
    
    NsePwrVecdB =-60:5:0;                                      % noise power in log-scale
    for nseIdx = 1:length(NsePwrVecdB)
        NsePwr = 10^(NsePwrVecdB(nseIdx)/10);                      % noise power of the AOA measurement
        sigma_theta=NsePwr;
        sigma_phi=NsePwr;
        sigma_s=0.1;                                               % noise power of the sensor position error
        
        % generate the noise covariance matrices
        rand('seed',5);
        Q_s=diag(rand(3*M,1)+1/9);
        avepow_s=trace(Q_s)/3/M;
        Q_s=sigma_s/avepow_s*Q_s;
        Qx=Q_s(1:3:end-2,1:3:end-2);
        Qy=Q_s(2:3:end-1,2:3:end-1);
        Qz=Q_s(3:3:end,3:3:end);
        Q_theta=diag(rand(M,1)+1/9);
        Q_phi=diag(rand(M,1)+1/9);
        Q_theta=Q_theta/(trace(Q_theta)/M)*sigma_theta;
        Q_phi=Q_phi/(trace(Q_phi)/M)*sigma_phi;
        
        Q_m=[Q_theta,zeros(M,M);zeros(M,M),Q_phi];
        Q=[Q_m,zeros(2*M,3*M);zeros(3*M,2*M),Q_s];
        j=sqrt(diag(Q));
        Q=Q+NseCorr*j*j';               % 5Mx5M noise correlation matrix
        
        CRLB=AOA3DLocSenPosErrCRLB(Q,so,uo);
        crlb(nseIdx,d)=trace(CRLB);
        Thbias(nseIdx,d)=AOA3DLocBias(thetao,phio,so,Q,uo);
        
        % obtain zero mean sensor position error
        rs=zeros(3,M,L);
        for l=1:L
            rs(:,:,l)=reshape(sqrtm(Q_s)*rsAll(:,l),3,M);
        end
        for l=1:L
            ns(:,:,l)=rs(:,:,l)-mean(rs,3);
        end
        
        MSEBRSE=0;
        for i=1:L
            dx(:,i)=ns(1,:,i)';
            dy(:,i)=ns(2,:,i)';
            dz(:,i)=ns(3,:,i)';
            ds=[dx(:,i),dy(:,i),dz(:,i)]';
            s=so+ds;                                                    % noisy sensor position
            theta=thetao+sqrtm(Q_theta)*ntheta(:,i);                    % noisy azimuth angle
            phi=phio+sqrtm(Q_phi)*nphi(:,i);                            % noisy elevation angle
            [uBRSE(:,i)] = AOA3DLoc_BRPLE(theta,phi,s,Q);
            MSEBRSE=MSEBRSE+norm(uo-uBRSE(:,i),2)^2;
        end
        mseBRSE(nseIdx,d)=MSEBRSE/L;
        biasnormBRSE(nseIdx,d)=norm(uo-mean(uBRSE,2),2);
    end
end

% Plot the result.
figure;
plot(NsePwrVecdB,10*log10(mean(crlb,2)),'k','LineWidth',1);hold on;
plot(NsePwrVecdB,10*log10(mean(mseBRSE,2)),'ksquare','MarkerSize',8);hold on;
hold off;grid on;
ylim([-10,60]);
xlabel('10log(\sigma^2)');
ylabel('10log(mse(u))');
legend('CRLB','BR-PLE','Location','northwest');
figure;
plot(NsePwrVecdB,10*log10(mean(Thbias,2)),'k','LineWidth',1);hold on;
plot(NsePwrVecdB,10*log10(mean(biasnormBRSE,2)),'ksquare','MarkerSize',8);hold on;
hold off;grid on;
ylim([-40,30]);
xlabel('10log(\sigma^2)');
ylabel('10log(biasnorm(u))');
legend('BR-PLE, Theory','BR-PLE','Location','northwest');



