function [CRLB] = TmDpLocObjMtnEftCRLB(u0,v0,t,Qt,Qf,fc,c)
% [CRLB] = TmDpLocObjMtnEftCRLB(u0,v0,t,Qt,Qf,fc,c)
%
% CRLB for localization using time delay or with Doppler shift measurements
% when object motion is not negligible.
%
% Input: 
% u0:   (Dim x 1),unknown object position.
% v0:   (Dim x 1), object velocity.
% t:    (Dim x M), sensor positions when signal sending out, M = sensor num.
% Qt:   (M x M), covariance matrix of time delays.
% Qf:   (M x M), covariance matrix of Doppler shifts, 
%       Qf=0 means the frequency measurements are unavailable.
% fc:   (M x 1), carrier frequencies of transmitted signals.
% c:    signal propagation speed.
%
% Output: 
% CRLB:  (2Dim x 2Dim), CRLB matrix for the object location [u0 v0]'.
%
% 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, "Localization of a moving object 
% with sensors in motion by time delays and Doppler shifts," IEEE Trans. 
% Signal Process., vol. 68, pp. 5824-5841, 2020.
%
% Tianyi Jia and K. C. Ho   11-08-2020
%
%       Copyright (C) 2020
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA
%       hod@missouri.edu
%

warning off;

[K,M]=size(t);     % K=localization dimension, M=number of sensors

%% Reconstruct the measurements
u0mt=u0-t;         % u0-t 
L=zeros(1,M);      % L(1,i)=norm(u0-ti) and L(2,i)=norm(u0-si)
m0=zeros(2*M,1);
u0imt=zeros(K,M);
Lu0it=zeros(1,M);
for i=1:M
    L(1,i)=norm(u0mt(:,i));    % L(1,i)=norm(u0-ti)
    A=v0'*v0-c^2;
    B=4*u0mt(:,i)'*v0;
    C=4*L(1,i)^2;
    m0(i)=(-B-sqrt(B^2-4*A*C))/(2*A);
    u0imt(:,i)=u0mt(:,i)+m0(i)/2*v0;  % u0+dui-ti
    Lu0it(1,i)=norm(u0imt(:,i));     % norm(u0+dui-ti)
    m0(i+M)=fc(i)*2*u0imt(:,i)'/Lu0it(1,i)*v0/(c-u0imt(:,i)'/Lu0it(1,i)*v0);
end

%% CRLB for range and Doppler localization
dtdx=zeros(2*K,M);  % x=[u0; v0]
FIS_r=zeros(2*K,2*K);

for i=1:M,
    pr=1/(1-v0'*u0imt(:,i)/(c*Lu0it(i)));
    dtdx(1:K,i)=pr*2*u0imt(:,i)/Lu0it(i)/c; %dt/du0
    dtdx(K+1:2*K,i)=pr*m0(i)*u0imt(:,i)/Lu0it(i)/c; %dt/dv
end

FIS_r=dtdx*inv(Qt)*dtdx';
if Qf==0
    CRLB=inv(FIS_r);
else
    dfdx=zeros(2*K,M);
    FIS_f=zeros(2*K,2*K);
    for i=1:M
        pp=2*(u0imt(:,i)'*v0/Lu0it(1,i)/(c*(1-u0imt(:,i)'*v0/Lu0it(1,i)/c)^2)+1/(1-u0imt(:,i)'/Lu0it(1,i)*v0/c));
        dpdu=v0/Lu0it(i)-u0imt(:,i)'*v0*u0imt(:,i)/Lu0it(i)^3+(v0'*v0/(Lu0it(i))-(v0'*u0imt(:,i))^2/(Lu0it(i)^3))*dtdx(1:K,i)/2;
        dpdv=(m0(i)*v0/Lu0it(i)-m0(i)*u0imt(:,i)'*v0*u0imt(:,i)/Lu0it(i)^3+2*u0imt(:,i)/Lu0it(i)+(v0'*v0/(Lu0it(i))-(v0'*u0imt(:,i))^2/(Lu0it(i)^3))*dtdx(K+1:2*K,i))/2;
        dfdx(1:K,i)=fc(i)/c*pp*dpdu;
        dfdx(K+1:2*K,i)=fc(i)/c*pp*dpdv;
    end
    FIS_f=dfdx*inv(Qf)*dfdx';
    CRLB=inv(FIS_f+FIS_r);
end
