function [crlb_u,crlb_udot] = MultiStaticDfDlyDopplerLocCRLB(uo,uodot,T,S,c,fc,Qtau,Qf)
% [crlb_u,crlb_udot] = MultiStaticDfDlyDopplerLocCRLB(uo,uodot,T,S,c,fc,Qtau,Qf)
% 
% This function computes the CRLBs of the position and velocity of a moving 
% target using both differential delay and Doppler shift mesruements. 
%
% Input parameter list:
% uo   : (2 x 1), Target position.
% uodot: (2 x 1), Target velocity.
% T    : (2 x NumTx), Transmitter positions.
% S    : (2 x NumRx), Receiver positions.
% c    : Signal propogation speed (m/s).
% fc   : Carrier frequencies (Hz).
% Qtau : Covairance matrix of the differential delay measurements.
% Qf   : Covairance matrix of the Doppler shift measurements.
% 
% Output parameter list:
% crlb_u : The CRLB of target position.
% crlb_udot : The CRLB of target velocity.
%
% Reference:
% Liu Yang, Le Yang and K. C. Ho, "Moving target localization in multistatic
% sonar by differential delays and Doppler shifts," IEEE Signal Process. 
% Lett., vol. 23, no. 9, pp. 1160-1164, Sep. 2016.
% 
% Liu Yang, Le Yang and K. C. Ho   09-2016
% 

M = length(T(1,:));                       % Numbers of transmitters
N = length(S(1,:));                       % Numbers of receivers
Qm = blkdiag(Qtau,Qf);                    % Covariance of measurements

% Define functions for computing
unit = @(x,y) (x-y)/norm(x-y);                                   % the unit vector from y to x.
der = @(x,y,z) (norm(x-y)*z'-(x-y)'*z*unit(x,y)')/(norm(x-y))^2; % the structure used in derivatives.

tau_u = zeros(M*N,2);
f_u = zeros(M*N,2);
f_udot = zeros(M*N,2);

for i = 1:M
    for j = 1:N
        tau_u((i-1)*N+j,:) = (unit(uo,T(:,i))'+unit(uo,S(:,j))')/c;
        f_u((i-1)*N+j,:) = fc(i)*(der(uo,T(:,i),uodot) + der(uo,S(:,j),uodot))/c;
        f_udot((i-1)*N+j,:) = fc(i)*(unit(uo,T(:,i))'+unit(uo,S(:,j))')/c;
    end
end
m = [tau_u zeros(M*N,2);f_u f_udot];

crlb = inv(m'*inv(Qm)*m);
crlb_u = trace(crlb(1:2,1:2));
crlb_udot = trace(crlb(3:4,3:4));

