function CRLB=DSF3DLoc_CRLB(u,u_dot,fc,S,c,Qn,Qfc,Qs)
% CRLB=DSF3DLoc_CRLB(u,u_dot,fc,S,c,Qn,Qfc,Qs)
%
% CRLB for the object position and velocity.
%
% 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.
% c:      scalar, signal propagation speed.
% Qn:     (M x M), 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);
d=sqrt(sum((S-repmat(u,1,M)).^2));
a=zeros(M,3);b=a;C=zeros(M,3*M);de_fc=zeros(M,1);
for i=1:M
    su=S(:,i)-u;
    a(i,:)=fc/c*((u_dot/d(i)-(su'*u_dot*su)./d(i)^3)');
    b(i,:)=fc/c*((su./d(i))');
    de_fc(i)=1+1/c*(su'./d(i)*u_dot);
    C(i,:)=[zeros(1,3*(i-1)) -a(i,:) zeros(1,3*(M-i))];
end
f_w=[a b];
f_alpha=[de_fc C];
Q_alpha=blkdiag(Qfc,Qs);
X=f_w'/Qn*f_w;
Y=f_w'/Qn*f_alpha;

Z=f_alpha'/Qn*f_alpha+inv(Q_alpha);

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

end

