function [uCART uMPR] = AOATDOA_MLECAR_3D(ui,s,rd,a,Q)
% [uCART uMPR] = AOATDOA_MLECAR_3D(ui,s,rd,thetam,phim,Q)
%
% ui:   initial solution guess
% s:    (Dim x M) matrix, each column is a sensor position and
%       first column is for the reference sensor,
%       M is the number of sensors and should be at least Dim+2,
% rd:   (M-1) x 1, TDOA measurement vector
% a:    2M x 1, AOA measurement vector
% Q:    2M x 2M, covariance matrix of the AOA
%       measurement vector
% uCART:    solution in Cartesian coordinates
% uMPR:     solution in MPR
%
% The routine uses the Gauss-Newton iteration in Cartesian coorindate to obtain
% the solution, works for 3-D
%
% 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
%

M=size(s,2);
N=size(s,1);
iterN=20;
ug=ui;
W=inv(Q);

du=ones(size(ui));
SmallValue=(1e-10)*Q(1,1);   % small value to stop iteration

l=0;    % iteration count
while (norm(du)>SmallValue & l<iterN)
    l=l+1;
    for i=1:M
        mo(:,i)=(ug-s(:,i));
        [thetamg(i) phimg(i) d(i)]=cart2sph(mo(1,i),mo(2,i),mo(3,i));
    end
    for i=2:M
        rdg(i-1)=d(i)-d(1);
    end
    for i=1:M
        rr(i)=norm(ug(1:2)-s(1:2,i),2);
    end
    for i=1:M
        dthetamdx(i)=-(ug(2)-s(2,i))/rr(i)^2;
        dthetamdy(i)=(ug(1)-s(1,i))/rr(i)^2;
        dthetamdz(i)=0;
        dphimdx(i)=-(ug(1)-s(1,i))*(ug(3)-s(3,i))/(rr(i)*d(i)^2);
        dphimdy(i)=-(ug(2)-s(2,i))*(ug(3)-s(3,i))/(rr(i)*d(i)^2);
        dphimdz(i)=rr(i)^2/(rr(i)*d(i)^2);
    end
    dthetamdu=[dthetamdx',dthetamdy',dthetamdz'];
    dphimdu=[dphimdx',dphimdy',dphimdz'];
    
    for i = 2 : M
        dr_du(i-1,:) = (ug-s(:,i))'/norm(ug-s(:,i))-(ug-s(:,1))'/norm(ug-s(:,1));
    end;
    
    h=[rd;a]-[rdg';thetamg';phimg'];
    G=[dr_du;dthetamdu;dphimdu];
    du=inv(G'*W*G)*G'*W*h;
    ug=ug+du;
    
    if ug(1)<-50*max(abs(ui))+ui(1)
        ug(1)=-50*max(abs(ui))+ui(1);
    end
    if ug(1)>50*max(abs(ui))+ui(1)
        ug(1)=50*max(abs(ui))+ui(1);
    end
    if ug(2)<-50*max(abs(ui))+ui(2)
        ug(2)=-50*max(abs(ui))+ui(2);
    end
    if ug(2)>50*max(abs(ui))+ui(2)
        ug(2)=50*max(abs(ui))+ui(2);
    end
    if ug(3)<-50*max(abs(ui))+ui(3)
        ug(3)=-50*max(abs(ui))+ui(3);
    end
    if ug(3)>50*max(abs(ui))+ui(3)
        ug(3)=50*max(abs(ui))+ui(3);
    end
    
end

uCART=ug;

[thetap phip rp]=cart2sph(ug(1),ug(2),ug(3));
uMPR=[thetap phip 1/rp]';
