function [uTilde] = AOATDOA_MLEMPR(uTildeInit,s,rd,a,Q)
% uTilde = AOATDOA_MLEMPR(uTildeInit,s,rd,a,Q)
%
% uTildeInit:   initial solution guest to start the iteration
% 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:    M x 1 (2-D case) or 2M x 1 (3-D case), AOA measurement vector
% Q:    M x M (2-D case) or 2M x 2M (3-D case) covariance matrix of the AOA
%       measurement vector
% uTilde:   solution in MPR
%
% The routine uses the Gauss-Newton iteration to obtain the solution in MPR,
% works for 2-D and 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
%

thetam=a(1:length(a)/2);
phim=a(1+length(a)/2:end);

[Dim,M]=size(s);    % Dim=dimension, M=NumSensors
iterN=100;          % maximum number of iterations

W=inv(Q/Q(1,1));
uTilde=uTildeInit;
du=ones(size(uTilde));
SmallValue=(1e-10)*Q(1,1);   % small value to stop iteration

l=0;    % iteration count
while (norm(du)>SmallValue & l<iterN)
    l=l+1;
    
    if (Dim==2) %2-D case
        thetag=uTilde(1);
        invrg=uTilde(2);
        
        % AOA gradient
        for i=1:M,
            rho=[cos(thetag)-s(1,i)*invrg   sin(thetag)-s(2,i)*invrg]';
            norm_rho=norm(rho);
            dthetamdu(i,:)=[-rho(2) rho(1)]/((norm_rho)^2);
            thetamg(i,1)=atan2(rho(2),rho(1));
            rhoAll(:,i)=rho/norm_rho;
            norm_rhoAll(i,1)=norm_rho;
        end;
        dthetamdu=dthetamdu*invrg;
        
        % range difference gradient
        dtdoamdu=(rhoAll(:,2:end)-repmat(rhoAll(:,1),1,M-1))';
        
        P=-[ ...
            sin(thetag) -cos(thetag);
            invrg*cos(thetag) invrg*sin(thetag)];
        
        hthetam=(thetam-thetamg)*invrg;
        htdoam=rd*(invrg)-norm_rhoAll(2:end)+ones(M-1,1);
        h=[htdoam;hthetam];
        G=[dtdoamdu;dthetamdu];
        
    else          % 3-D case
        thetag=uTilde(1);
        phig=uTilde(2);
        invrg=uTilde(3);
        
        % AOA gradient
        for i=1:M,
            rho=[cos(thetag)*cos(phig)-s(1,i)*invrg   sin(thetag)*cos(phig)-s(2,i)*invrg   sin(phig)-s(3,i)*invrg]';
            norm_rho=norm(rho);
            dthetamdu(i,:)=[-rho(2) rho(1) 0]/((norm_rho*cos(phig))^2);
            dphimdu(i,:)=[-rho(1)*rho(3) -rho(2)*rho(3) (norm_rho*cos(phig))^2]/(cos(phig)*norm_rho^3);
            thetamg(i,1)=atan2(rho(2),rho(1));
            phimg(i,1)=atan2(rho(3),norm(rho(1:2)));
            rhoAll(:,i)=rho/norm_rho;
            norm_rhoAll(i,1)=norm_rho;
        end;
        dthetamdu=dthetamdu*invrg;
        dphimdu=dphimdu*invrg;
        
        % range difference gradient
        dtdoamdu=(rhoAll(:,2:end)-repmat(rhoAll(:,1),1,M-1))';
        
        P=-(1/cos(phig))*[ ...
            sin(thetag) -cos(thetag) 0;
            cos(thetag)*sin(phig)*cos(phig) sin(thetag)*sin(phig)*cos(phig) -cos(phig)^2;
            invrg*cos(thetag)*cos(phig)^2 invrg*sin(thetag)*cos(phig)^2 invrg*sin(phig)*cos(phig) ];
        
        hthetam=(thetam-thetamg)*invrg;
        hphim=(phim-phimg)*invrg;
        htdoam=rd*(invrg)-norm_rhoAll(2:end)+ones(M-1,1);
        h=[htdoam;hthetam;hphim];
        G=[dtdoamdu;dthetamdu;dphimdu];
        
    end;    % Dim
    
    % -- update --
    duTilde=P*inv(G'*W*G)*G'*W*h;
    uTilde=uTilde+0.5*duTilde;
    
end

