function [f3, uf, rtsf]= lamda2_GeoTDOAFDOA(r,s0,sv,W,M,h,G1,g2,g3,C,P,V,lamda)
%
% [f3, uf, rtsf]= lamda2_GeoTDOAFDOA(r,s0,sv,W,M,h,G1,g2,g3,C,P,V,lamda)
%
%
% Finding the value of u'*u-r for a given lambda
%
% 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
%

G4=P*diag([1/(V(1,1)+lamda) 1/(V(2,2)+lamda) 1/(V(3,3)+lamda)])*P';
G5=[h,-g2,zeros(2*M-2,1),-g3,zeros(2*M-2,1)];
G6=G4*G1'*W*G5;
g7=[s0(:,1)'*s0(:,1)+r^2, 0, -1, 0, 0]';
g8=[2*sv(:,1)'*s0(:,1), 0, 0, 0, -2]';
L12=0.5*([sv(:,1)'*G4*sv(:,1) -s0(:,1)'*G4*sv(:,1); -sv(:,1)'*G4*s0(:,1) s0(:,1)'*G4*s0(:,1)])/det([s0(:,1)'*G4*s0(:,1) s0(:,1)'*G4*sv(:,1); sv(:,1)'*G4*s0(:,1) sv(:,1)'*G4*sv(:,1)])*[2*s0(:,1)'*G6-g7';2*sv(:,1)'*G6-g8'];
g9=L12(1,:)';
g10=L12(2,:)';
G11=G6-G4*(s0(:,1)*g9'+sv(:,1)*g10');
G12=G5-G1*G11;
g13=G12'*W*g3;
g14=[-g13(4),g10(4)-g13(5),g10(5),0,0]';
g15=[g13(1),g13(2)-g10(1),g13(3)-g10(2),-g10(3),0]';
G16=[g14,C*g14,C^2*g14,g15,C*g15]';
G17=g14*g9'*G16;
G18=(g15*g10'-g14*g2'*W*G12)*G16;

f=poly2coeff(G17,G18);
if isnan(f) == 1   % f has a chance to be nan
    f3=nan;
    uf=0;
    rtsf=0;
else
    rts=roots(f);
    rts(find(imag(rts)~=0))=[]; % find the real roots
    rts(find(rts<0))=[];        % find the positive roots
    Nroot=length(rts);
    err=inf;
    if Nroot==0     % Nroot has a chance to be 0
        f3=nan;
        uf=0;
        rtsf=0;
    end
    for j=1:Nroot
        rr1v=[1,rts(j),rts(j)^2,rts(j)^3,rts(j)^4]';
        rrd1=g15'*rr1v/(g14'*rr1v);
        uk=G11*[1,rts(j),rts(j)^2,rrd1,rts(j)*rrd1]';
        druk2=abs(r^2-uk'*uk);
        if druk2<err
            uf=double(uk);
            err=druk2;
            f3=r^2-uk'*uk;
            rtsf=rts(j);
        end
    end
end