function [biasnorm]=AOA3DLocBias(thetao,phio,so,Q,uo)
%
% This program generates the theoretical bias in the proposed source 
% location estimate. 
% The details of the bias derivation 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: [biasnorm]=AOA3DLocBias(thetao,phio,so,Q,uo);
%
% Input Parameter List:
% thetao:   Mx1 true azimuth angle measurement vector, M is num. of sensors.
% phio:     Mx1 true elevation angle measurement vectors.
% so:       3xM true sensor position matrix.
%           so(:,i) is the true position of the ith sensor.
% Q:        5Mx5M covariance matrix for [azimuthAng,elevationAng,senorPos]
% uo:       3x1 is the true source location.
%
% The program returns the Euclidean norm of the theoretical bias in the 
% proposed source location estimate
%
% 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(so,2);               % number of sensors
Q_n=Q(1:M,1:M);
Q_w=Q(M+1:2*M,M+1:2*M);
Q_nw=Q(1:M,M+1:2*M);
Q_wn=Q_nw';

for i=1:M
    g1(i,:)=[sin(thetao(i)),-cos(thetao(i)),0];
    g2(i,:)=[sin(phio(i))*cos(thetao(i)),sin(phio(i))*sin(thetao(i)),-cos(phio(i))];
    h1(i)=g1(i,:)*so(:,i);
    h2(i)=g2(i,:)*so(:,i);
    B1s(i,:)=[zeros(1,3*(i-1)),g1(i,:),zeros(1,3*(M-i))];
    B2s(i,:)=[zeros(1,3*(i-1)),g2(i,:),zeros(1,3*(M-i))];
    w1n(i,:)=[cos(thetao(i)),sin(thetao(i)),0];
    w1w(i,:)=zeros(1,3);
    w2n(i,:)=[-sin(phio(i))*sin(thetao(i)), sin(phio(i))*cos(thetao(i)),0];
    w2w(i,:)=[cos(phio(i))*cos(thetao(i)), cos(phio(i))*sin(thetao(i)), sin(phio(i))];
end
G=[g1;g2];
h=[h1;h2];
do=sqrt(sum((uo*ones(1,M)-so).*(uo*ones(1,M)-so)));
B1n=-diag(do'.*cos(phio));
B1w=zeros(M,M);
B2n=zeros(M,M);
B2w=-diag(do);
B=[B1n,B1w,B1s;
    B2n,B2w,B2s];
W=inv(B*Q*B');
H=inv(G'*W*G)*G'*W;
qn=[];
qw=[];
for i=1:M
    d1n(i)=w1n(i,:)*(so(:,i)-uo);
    d2n(i)=w2n(i,:)*(so(:,i)-uo);
    d1w(i)=w1w(i,:)*(so(:,i)-uo);
    d2w(i)=w2w(i,:)*(so(:,i)-uo);
end
D1n=diag(d1n);
D2n=diag(d2n);
D1w=diag(d1w);
D2w=diag(d2w);
Dn=[D1n;D2n];
wn=[w1n;w2n];
Dw=[D1w;D2w];
ww=[w1w;w2w];
tn=wn*H*Dn;
tw=ww*H*Dw;
tnw=wn*H*Dw;
twn=ww*H*Dn;
k=[1;1];
E_delta_u=-[H,H]*([tn,tnw;twn,tw].*[kron(k,Q_n),kron(k,Q_nw);kron(k,Q_wn),kron(k,Q_w)])*ones(2*M,1);
biasnorm=sqrt(sum(E_delta_u.^2));      % Return the theoretical bias
