function [CRLB] = GeoTDOAFDOA_CRLB(u0,s0,sv,Q)
%
% CRLB = GeoTDOAFDOA_CRLB(u0,s0,sv,Q)
%
% input:
%         u0: emitter location
%         s0: 3xM,satellite locations
%         sv: satellite speeds
%         Q: measurement noise covariance matrix
% output:
%         CRLB: (3x3) CRLB matrix
%
% Evaluating the constrained CRLB for geolocating an emitter on earth
% using TDOA and FDOA measurements
%
% 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
%

M=length(s0);       % number of satellites
dd0duT=zeros(2*M-2,3);

rg0=zeros(M,1);     % true range
rgd0=zeros(M,1);    % true range rate
for i=1:M
    rg0(i)=norm(s0(:,i)-u0);
    rgd0(i)=(s0(:,i)-u0)'*sv(:,i)/rg0(i);
end

for i=1:M-1
    %% d d0/d uT
    dd0duT(i,:)=-(s0(:,i+1)-u0)'/rg0(i+1)+(s0(:,1)-u0)'/rg0(1);  % d d0/d u0
    %% d f0/d uT
    dd0duT(i+M-1,:)=(s0(:,i+1)-u0)'*rgd0(i+1)/rg0(i+1)^2-(s0(:,1)-u0)'*rgd0(1)/rg0(1)^2-sv(:,i+1)'/rg0(i+1)+sv(:,1)'/rg0(1); % d f0/d u0
end

F=-2*u0;
J=dd0duT'*inv(Q)*dd0duT;
inv_J=inv(J);
CRLB=inv_J-inv_J*F*inv(F'*inv_J*F)*F'*inv_J;
