function CRLB=DSF3DTMA_CRLB(u,u_dot,fc,S,N,c,T,Qn,Qfc,Qs)
% CRLB=DSF3DTMA_CRLB(u,u_dot,fc,S,N,c,T,Qn,Qfc,Qs)
%
% CRLB for the initial object position and velocity for TMA.
%
% Input:
% u:      (3 x 1), object position.
% u_dot:  (3 x 1), object velocity.
% fc:     scalar, center frequency.
% S:      (3 x M), sensor poition matrix, M=num. of sensors.
% N:      scalar, number of time measurements.
% c:      scalar, signal propagation speed.
% T:      scalar, time measurement interval.
% Qn:     (MN x MN), covariance matrix of DSF measurements.
% Qfc:    scalar, variance of center frequency.
% Qs:     (3M x 3M), covariance matrix of sensor positions.
%
% Output:
% CRLB:  (6 x 6), [pos (*); (*)', vel].
%
% Reference: M. H. Ahmed, K. C. Ho, and G. Wang, "3-D target localization
% and motion analysis based on Doppler shifted frequencies," IEEE Trans.
% Aerosp. Electron. Syst., vol. 58, no. 2, pp. 815-833, Apr. 2022.
%
% M. H. Ahmed, K. C. Ho     10-28-2022
%
%       Copyright (C) 2022
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA.
%       hod@missouri.edu
%

M=length(S);

u_all=u*ones(1,N)+u_dot*(0:N-1)*T;
d=sqrt(sum((repelem(u_all',M,1)-repmat(S',N,1)).^2,2));
a=zeros(M*N,3);b=a;C=zeros(M*N,3*M);de_fc=zeros(M*N,1);

for k=1:M*N
    i=mod(k+M-1,M)+1;
    m=fix((k-1)/M);

    ums=u+m*T*u_dot-S(:,i);

    a(k,:)=fc/c/d(k)*((ums'*u_dot*ums)/d(k)^2-u_dot)';
    b(k,:)=fc/c/d(k)*(m*T*ums'*u_dot*ums/d(k)^2-(ums+m*T*u_dot))';
    de_fc(k)=1+1/c*ums'*u_dot/d(k);
    C(k,:)=[zeros(1,3*(i-1)) -a(k,:) zeros(1,3*(M-i))];
end

de_s_alpha=[zeros(M*3,1) eye(3*M)];
de_fc_alpha=[1 zeros(1,3*M)];
f_beta=[a b];
f_alpha=[de_fc C];

X=f_beta'/Qn*f_beta;
Y=f_beta'/Qn*f_alpha;

Z=f_alpha'/Qn*f_alpha+de_s_alpha'/Qs*de_s_alpha+de_fc_alpha'/Qfc*de_fc_alpha;

CRLB=(inv(X)+X\Y/(Z-Y'/X*Y)*Y'/X);

FIM=f_beta'*inv(Qn+C*Qs*C'+de_fc*Qfc*de_fc')*f_beta;
CRLB=inv(FIM);

end
