function [CRLB,dtdu] = TmDpLocSenMtnEftCRLB(uo,t,v,Qt,Qf,f,c)
% [CRLB,dtdu] = TmDpLocSenMtnEftCRLB(uo,t,v,Qt,Qf,f,c)
%
% CRLB for localization using time delay or with Doppler shift measurements
% with sensor motion is not negligible.
%
% Input parameter list:
% uo:   (Dim x 1),unknown object position.
% t:    (Dim x N), sensor positions when signal sending out, N = sensor num.
% v:    (Dim x N), sensor velocity.
% Qt:   (N x N), covariance matrix of range.
% Qf:   (N x N), covariance matrix of Doppler shift. 
%       Qf=0 means the frequency measurement is unavailiable.
% f:    (N x 1), carrier frequencies of transmited signals.
%
% Output parameter list:
% CRLB:  (Dim x Dim), CRLB matrix for the object position.
% dtdu:  (Dim x Dim), CRLB matrix for the object position.
%
% The program can be used for 2-D(Dim=2) or 3-D(Dim=3) localization directly.
%
% Reference:
% T. Jia, K. C. Ho, H. Wang, and X. Shen, "Effect of sensor motion on time 
% delay and Doppler shift localization: analysis and solution," IEEE Trans. 
% Signal Process., vol. 67, no. 22, pp. 5881-5895, Nov. 2019.
%
% Tianyi Jia and K. C. Ho   12-26-2019
%
%       Copyright (C) 2019
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA
%       hod@missouri.edu
%

if exist('c')==0
    c=1500;            % signal propagation speed
end
[K,N]=size(t);       % number of sensors

%% Create the measuremet and range
tao0=zeros(1,N);        % true time measurements
uomt=repmat(uo,1,N)-t;	% uo-t 
L=zeros(K,N);           % L(1,i)=norm(uo-ti) and L(2,i)=norm(uo-si)
s=zeros(K,N);           % location of sensor with signal being received
uoms=zeros(K,N);
for i=1:N
    L(1,i)=norm(uomt(:,i));
    A=c^2-v(:,i)'*v(:,i);
    B=2*(L(1,i)*c-v(:,i)'*uomt(:,i));
    tao0(i)=B/A;
    s(:,i)=t(:,i)+tao0(i)*v(:,i);
    uoms(:,i)=uo-s(:,i);      %uo-s
    L(2,i)=norm(uoms(:,i));
end

%% CRLB for range and Dopppler localization
dtdu=zeros(K,N);
FIS_r=zeros(K,K);
for i=1:N
    dtdu(:,i)=1/(c+v(:,i)'*uoms(:,i)/L(2,i))*(uomt(:,i)/L(1,i)+uoms(:,i)/L(2,i));
end
FIS_r=dtdu*inv(Qt)*dtdu';
if Qf==0;
    CRLB=trace(inv(FIS_r));
else
    drdf=zeros(K,N);
    FIS_f=zeros(K,K);
    for i=1:N
        puot=uomt(:,i)/L(1,i);
        puos=uoms(:,i)/L(2,i);
        drdf(:,i)=-f(i)/((c+v(:,i)'*puos)^2)*v(:,i)'*(-puot-puos)*(v(:,i)/L(2,i)-(uoms(:,i)'*v(:,i))/L(2,i)^2*puos+(-v(:,i)'*v(:,i)/L(2,i)+...
            (uoms(:,i)'*v(:,i))^2/L(2,i)^3)*dtdu(:,i))+...
            f(i)/(c+v(:,i)'*puos)*(-v(:,i)/L(1,i)+(uomt(:,i)'*v(:,i))/L(1,i)^2*puot-v(:,i)/L(2,i)+(uoms(:,i)'*v(:,i))/L(2,i)^2*puos+...
            (v(:,i)'*v(:,i)/L(2,i)-(uoms(:,i)'*v(:,i))^2/L(2,i)^3)*dtdu(:,i));
    end
    FIS_f=drdf*inv(Qf)*drdf';
    CRLB=trace(inv(FIS_f+FIS_r));
end
