function [CRLB,drTduv] = TmDpLocObjSsrMtnEftCRLB(u0,v0,t,vs,Qt,Qf,fc,c)
% [CRLB,drTduv] = TmDpLocObjSsrMtnEftCRLB(u0,v0,t,vs,Qt,Qf,fc,c)
%
% CRLB for localization using time delay or with Doppler shift measurements
% when the sensor and object motions are 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]'.
% drTduv: partial derivative of the measurements with respect to u and v
%
% 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

%% construct measurements and ranges
r01=zeros(1,M);     % r1 is the first part of r
r0=zeros(1,M);      % r
u0mt=u0-t;          % u0-t
L=zeros(4,M);       % L(1,i)=norm(u0-ti) and L(2,i)=norm(u0-si)  L(3,i)=norm(u0i-ti) and L(4,i)=norm(u0i-si)
s=zeros(K,M);       % location of sensor with signal being received
u0imt=zeros(K,M);
u0ims=zeros(K,M);
u0i=zeros(K,M);
pu0imt=zeros(K,M);
pu0ims=zeros(K,M);
Ai=zeros(1,M);
Bi=zeros(1,M);
Ci=zeros(1,M);
Di=zeros(1,M);

for i=1:M
    L(1,i)=norm(u0mt(:,i));    %  L(1,i)=norm(u0-ti)
    A=1-v0'*v0/c^2;
    B=-2*u0mt(:,i)'*v0/c;
    C=-L(1,i)^2;
    r01(i)=(-B+sqrt(B^2-4*A*C))/(2*A);
    u0i(:,i)=u0+r01(i)/c*v0;
    
    u0imt(:,i)=u0i(:,i)-t(:,i);
    L(3,i)=norm(u0imt(:,i));   % L(3,i)=norm(u0i-ti)
    A=1-vs(:,i)'*vs(:,i)/c^2;
    B=2*(L(3,i)-vs(:,i)'*u0imt(:,i)/c);
    
    r0(i)=B/A;
    s(:,i)=t(:,i)+r0(i)*vs(:,i)/c;
    u0ims(:,i)=u0i(:,i)-s(:,i);      %u0-s
    L(4,i)=norm(u0ims(:,i));   % L(4,i)=norm(u0i-si)
    
    pu0imt(:,i)=u0imt(:,i)/L(3,i);
    pu0ims(:,i)=u0ims(:,i)/L(4,i);
    
    Ai(i)=pu0ims(:,i)'*v0/c;
    Bi(i)=pu0imt(:,i)'*v0/c;
    Ci(i)=pu0ims(:,i)'*vs(:,i)/c;
    Di(i)=pu0imt(:,i)'*vs(:,i)/c;
end

%% CRLB for range and Doppler localization
drTduv=zeros(2*K,M);
dtTduv=zeros(2*K,M);
FIS_t=zeros(2*K,2*K);
for i=1:M
    drTduv(1:K,i)=1/(1+Ci(i))*((1+Ai(i))/(1-Bi(i))*pu0imt(:,i)+pu0ims(:,i));   % dt/du
    drTduv(K+1:2*K,i)=r01(i)/c*drTduv(1:K,i);
end
dtTduv=drTduv/c;
FIS_t=dtTduv*inv(Qt)*dtTduv';

if Qf==0
    CRLB=(inv(FIS_t)); %CRLB=(inv(FIS_r));
else
    dr1Tdu=zeros(K,1);
    dr1Tdv=zeros(K,1);
    dAidu=zeros(K,1);
    dBidu=zeros(K,1);
    dCidu=zeros(K,1);
    dDidu=zeros(K,1);
    dAidv=zeros(K,1);
    dBidv=zeros(K,1);
    dCidv=zeros(K,1);
    dDidv=zeros(K,1);
    
    drdTduv=zeros(2*K,M);
    dfTduv=zeros(2*K,M);
    FIS_f=zeros(2*K,2*K);
    for i=1:M
        
        %% dfT / du
        dr1Tdu=1/(1-Bi(i))*pu0imt(:,i);
        dAidu=((eye(K)+dr1Tdu*v0'/c-drTduv(1:K,i)*vs(:,i)'/c)*v0/L(4,i)-u0ims(:,i)'*v0*(eye(K)+dr1Tdu*v0'/c-drTduv(1:K,i)*vs(:,i)'/c)*u0ims(:,i)/L(4,i)^3)/c;
        dBidu=((eye(K)+dr1Tdu*v0'/c)*v0/L(3,i)-u0imt(:,i)'*v0*(eye(K)+dr1Tdu*v0'/c)*u0imt(:,i)/L(3,i)^3)/c;
        dCidu=((eye(K)+dr1Tdu*v0'/c-drTduv(1:K,i)*vs(:,i)'/c)*vs(:,i)/L(4,i)-u0ims(:,i)'*vs(:,i)*(eye(K)+dr1Tdu*v0'/c-drTduv(1:K,i)*vs(:,i)'/c)*u0ims(:,i)/L(4,i)^3)/c;
        dDidu=((eye(K)+dr1Tdu*v0'/c)*vs(:,i)/L(3,i)-u0imt(:,i)'*vs(:,i)*(eye(K)+dr1Tdu*v0'/c)*u0imt(:,i)/L(3,i)^3)/c;
        
        drdTduv(1:K,i)=c/((1-Bi(i))^2*(1+Ci(i))^2)*( ((Bi(i)-Di(i))*dAidu+(1+Ai(i))*(dBidu-dDidu))*(1-Bi(i))*(1+Ci(i)) - ...
            ( -dBidu*(1+Ci(i))+ dCidu*(1-Bi(i)) )*(1+Ai(i))*(Bi(i)-Di(i)) ) +  c/((1+Ci(i))^2) * ( (dAidu-dCidu)*(1+Ci(i))-dCidu*(Ai(i)-Ci(i)) );  % dfT / du
        
        
        %% dfT / dv
        dAidv=(pu0ims(:,i)+dAidu*r01(i))/c;
        dBidv=(pu0imt(:,i)+dBidu*r01(i))/c;
        dCidv=r01(i)/c*dCidu;
        dDidv=r01(i)/c*dDidu;
        drdTduv(K+1:2*K,i)=c/((1-Bi(i))^2*(1+Ci(i))^2)*( ((Bi(i)-Di(i))*dAidv+(1+Ai(i))*(dBidv-dDidv))*(1-Bi(i))*(1+Ci(i)) - ...
            ( -dBidv*(1+Ci(i))+ dCidv*(1-Bi(i)) )*(1+Ai(i))*(Bi(i)-Di(i)) ) +  c/((1+Ci(i))^2) * ( (dAidv-dCidv)*(1+Ci(i))-dCidv*(Ai(i)-Ci(i)) );  % dfT / dv
        
        dfTduv(1:2*K,i)=drdTduv(1:2*K,i)*fc(i)/c;
    end
    FIS_f=dfTduv*inv(Qf)*dfTduv'; % FIS_f=drdTduv*inv(Qf)*drdTduv';
    
    CRLB=(inv(FIS_f+FIS_t));
end