function [CRLBu] = AOA3DLocSenPosErrCRLB(Q,so,uo)
%
% This program generates the CRLB for the 3D source location where the 
% measurements are AoAs with Gaussian additive noise. The sensor positions
% have additive Gaussian noise as well.  The details 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: [CRLBu] = AOA3DLocSenPosErrCRLB(Q,so,uo);
%
% Input Parameter List:
% Q:        5Mx5M covariance matrix for [azimuthAng,elevationAng,senorPos]
%           M is the num. of sensors.
% so:       3xM true sensor position matrix.
%           so(:,i) is the true position of the ith sensor.
% uo:       3x1 is the true source location.
%
% The program returns the 3x3 CRLB matrix for the source location. 
%
% 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
r=zeros(M,1);
d=zeros(M,1);
dthetadx=zeros(M,1);
dthetady=zeros(M,1);
dthetadz=zeros(M,1);
dphidx=zeros(M,1);
dphidy=zeros(M,1);
dphidz=zeros(M,1);

for i=1:M
    r(i)=norm(uo(1:2)-so(1:2,i),2);
    d(i)=norm(uo-so(:,i),2);
end

% generate the partial derivatives
for i=1:M
    dthetadx(i)=-(uo(2)-so(2,i))/r(i)^2;
    dthetady(i)=(uo(1)-so(1,i))/r(i)^2;
    dthetadz(i)=0;
    dphidx(i)=-(uo(1)-so(1,i))*(uo(3)-so(3,i))/(r(i)*d(i)^2);
    dphidy(i)=-(uo(2)-so(2,i))*(uo(3)-so(3,i))/(r(i)*d(i)^2);
    dphidz(i)=r(i)/d(i)^2;
    dthetads(i,:)=[zeros(1,3*(i-1)),dthetadx(i),dthetady(i),dthetadz(i),zeros(1,3*(M-i))];
    dphids(i,:)=[zeros(1,3*(i-1)),dphidx(i),dphidy(i),dphidz(i),zeros(1,3*(M-i))];
end
dthetadu=[dthetadx,dthetady,dthetadz];
dphidu=[dphidx,dphidy,dphidz];
dmdu=[dthetadu;dphidu];
dmds=[dthetads;dphids];
dm=[dmdu,dmds;
    zeros(3*M,3),eye(3*M)];
FIM=dm'*inv(Q)*dm;
CRLB=inv(FIM);
CRLBu=CRLB(1:3,1:3);
