function [u,lambda3,rt] = GeoTDOAFDOA_EmitterOnEarth(m,r,s0,sv,Q,thrshd)
%
% [u,lambda3,rt] = GeoTDOAFDOA_EmitterOnEarth(m,r,s0,sv,Q,thrshd)
%
% input: 
%         m: (2M-2)x1, vector containing range(TODA) and range rate (FDOA) measurements
%         r: earth radius at emitter location, r=norm(u'*u)
%         s0: 3xM,satellite locations
%         sv: satellite speeds
%         Q: measurement noise covariance matrix
%         thrshd: stopping threshold for finding lambda_3
% output:
%         u: the emitter location estimate
%         lambda3: lambda_3
%         rt: roots of the polynomial
%
% Gelolcating an emitter on earth using TDOA and FDOA measurements
% when the number of satellites is at least 3
%
% Reference: K. C. Ho and Y. T. Chan, “Geolocation of a known altitude object from
%  TDOA and FDOA measurements," IEEE Trans. Aerosp. Electon. Eng., vol. 33, no. 3, 
%  pp. 770-783, Jul. 1997.
%
% Tianyi Jia, K. C. Ho    02-28-2019
%
%       Copyright (C) 2019
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA.
%       hod@missouri.edu
%

if (nargin==5) thrshd=1e-10; end;

NumUpdateW=0+1;   % number of times to update the weighting matrix W

N=3;    % geolocation dimension, 3D

M=length(s0);       % number of satellites
h=zeros(2*M-2,1);
G1=zeros(2*M-2,N);
g2=zeros(2*M-2,1);
g3=zeros(2*M-2,1);
C=zeros(5);
C(1,5)=1;
C(2:5,1:4)=eye(4);

iQ=inv(Q);
W=iQ;

% for i=1:M-1
%     h(i)= m(i)^2-s0(:,i+1)'*s0(:,i+1)+s0(:,1)'*s0(:,1);
%     h(i+M-1)=2*m(i)*m(i+M-1)-2*s0(:,i+1)'*sv(:,i+1)+2*s0(:,1)'*sv(:,1);
%     G1(i,:)=-2*(s0(:,i+1)'-s0(:,1)');
%     G1(i+M-1,:)=-2*(sv(:,i+1)'-sv(:,1)');
%     g2(i)=-2*m(i);
%     g2(i+M-1)=-2*m(i+M-1);
%     g3(i+M-1)=-2*m(i);
% end

satAltSq = sum(s0.^2)';
satAltVel = sum(s0.*sv)';
h(1:M-1) = m(1:M-1).^2 - satAltSq(2:end) + satAltSq(1);
h(M:end) = 2*( m(1:M-1).*m(M:end) - satAltVel(2:end) + satAltVel(1) );
G1 = -2*[ (s0(:,2:end)-repmat(s0(:,1),1,M-1))'; (sv(:,2:end)-repmat(sv(:,1),1,M-1))' ];
g2 = -2*m;
g3=-2*[ zeros(M-1,1); m(1:M-1) ];

% -- repeat for updating the weighting matrix W --
for ii=1:(NumUpdateW+1),
    
    [P,V]=eig(G1'*W*G1);
    
    % -- using the Newton's method to find lambda_3 --
    MAXk=60;
    Lk=1e-6; % the initial value of the Newton's method
    Lk1=0;   % the initial value of the Newton's method
    f3k = lamda2_GeoTDOAFDOA(r,s0,sv,W,M,h,G1,g2,g3,C,P,V,Lk);
    [f3k1, u0, rts0] = lamda2_GeoTDOAFDOA(r,s0,sv,W,M,h,G1,g2,g3,C,P,V,Lk1);
    uk1=u0;
    rts1=rts0;
    
    for k=1:MAXk,
        Lk2=(Lk*f3k1-Lk1*f3k)/(f3k1-f3k);  % Newton's method to update lamda_3
        [f3k2, uk2, rts2]=lamda2_GeoTDOAFDOA(r,s0,sv,W,M,h,G1,g2,g3,C,P,V,Lk2);
        if isnan(f3k2) == 1  % f3k2 has a chance to be nan
            u=uk1;
            rt=rts1;
            lambda3=Lk1;
            break;
        end
        if  abs(f3k2)<=thrshd  % lambda3 found
            u=uk2;
            rt=rts2;
            lambda3=Lk2;
            break;
        end
        Lk=Lk1;
        Lk1=Lk2;
        f3k=f3k1;
        f3k1=f3k2;
        uk1=uk2;
        rts1=rts2;
    end
    
    % -- extreme condition --
    if exist('u')==0 % if the Newton's method cannot converge within the threshold
        if abs(r^2-u0'*u0)<= abs(r^2-uk2'*uk2)
            u=u0;
            rt=rts0;
            lambda3=0;
        else
            u=uk2;
            rt=rts2;
            lambda3=Lk2;
        end
    end
    
    % -- update weighting matrix W --
    rgHat = sqrt(sum((s0 - repmat(u,1,M)).^2));
    rgdHat = sum((s0 - repmat(u,1,M)) .* sv)./rgHat;
    B=zeros(2*M-2);
    B(1:M-1,1:M-1)=diag(2*rgHat(2:M));
    B(1:M-1,M:2*M-2)=diag(2*rgdHat(2:M));
    B(M:2*M-2,M:2*M-2)=B(1:M-1,1:M-1);
    iB=inv(B);
    W=iB*iQ*iB';
    
end;    %NumUpdateW


