function [uBR]=AOA3DLoc_BRPLE(theta,phi,s,Q)
%
% This program realizes the algorithm for localizing a source in 3D
% using AoA measurements. 
% The details of the algorithm development can be found in
% 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.
%
% Usage: [uBR]=AOA3DLoc_BRPLE(theta,phi,s,Q);
%
% Input Parameter List:
% theta:    Mx1 azimuth angle measurement vector, M is the num. of sensors.
% phi:      Mx1 elevation angle measurement vector.
% s:        3xM sensor position matrix.
%           s(:,i) is the known position of the ith sensor.
% Q:        5Mx5M covariance matrix for [azimuthAng,elevationAng,senorPos]
%
% The program returns a bias-reduced 3x1 source location estimate uBR
% The program works for uncorrelated or correlated noise
%
% 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
%

M=size(s,2);        % number of sensors
N=size(s,1);        % dimension of the localization problem, should be 3
iterN=1;            % number of updates for the weighting matrix 

% ===== initialization =====
for i=1:M
    a0(i,:)=[-sin(theta(i)),cos(theta(i)),0];
    a1(i,:)=[-sin(phi(i))*cos(theta(i)),-sin(phi(i))*sin(theta(i)),cos(phi(i))];
    
    b0(i)=a0(i,:)*s(:,i);
    b1(i)=a1(i,:)*s(:,i);
    
    D0s(i,:)=[zeros(1,3*(i-1)),a0(i,:),zeros(1,3*(M-i))];
    D1s(i,:)=[zeros(1,3*(i-1)),a1(i,:),zeros(1,3*(M-i))];
    
    wtn(i,:)=[-cos(theta(i)),-sin(theta(i)),0];
    wto(i,:)=zeros(1,3);
    wpn(i,:)=[sin(phi(i))*sin(theta(i)), -sin(phi(i))*cos(theta(i)),0];
    wpo(i,:)=[-cos(phi(i))*cos(theta(i)), -cos(phi(i))*sin(theta(i)), -sin(phi(i))];
    
    u0(i,:)=[-wtn(i,:) wtn(i,:)*s(:,i)];
    v0(i,:)=[-wto(i,:) wto(i,:)*s(:,i)];
    u1(i,:)=[-wpn(i,:) wpn(i,:)*s(:,i)];
    v1(i,:)=[-wpo(i,:) wpo(i,:)*s(:,i)];
end

w0=[zeros(3*M,3),reshape(a0',3*M,1)];
w1=[zeros(3*M,3),reshape(a1',3*M,1)];

A1=[a0;a1];
B1=[b0,b1]';
A=[-A1,B1];

X0=[u0;v0;w0];
X1=[u1;v1;w1];
X=[X0;X1];

% ===== solution, iterate to improve the weighing matrix =====
for n=1:iterN+1,
    if (n==1)
        W=eye(2*M);     % initial coarse solution
    else
        d1=sqrt(sum((uBR*ones(1,M)-s).*(uBR*ones(1,M)-s)));
        D0n=diag(d1'.*cos(phi));
        D0w=zeros(M,M);
        D1n=zeros(M,M);
        D1w=diag(d1);
        D=[D0n,D0w,D0s;
            D1n,D1w,D1s];
        W=inv(D*Q*D');   % update the weighting matrix
    end;
    
    W11=W(1:M,1:M);
    W12=W(1:M,M+1:2*M);
    W21=W(M+1:2*M,1:M);
    W22=W(M+1:2*M,M+1:2*M);
    k1=N-1;                                
    k2=2*N-1;                              
    QW=kron(ones(k1,k1),Q).*[kron(ones(k2,k2),W11) kron(ones(k2,k2),W12);kron(ones(k2,k2),W21) kron(ones(k2,k2),W22)];
    R=A'*W*A;
    Omega=X'*QW*X;                                   %constraint matrix
    
    [V,D2]=eig(R,Omega);
    [d,IX]=sort(sum(D2));
    uBR=V(1:3,IX(1))/V(4,IX(1));
end