function CRLB = TDOAFDOALocMvgSenPosErrCRLB(so,s_doto,uo,u_doto,Q_beta,Q_alpha)
%
% This program finds the CRLB of the source position and velocity when the 
% sensor positions and velocities are subject to random noises. The 
% positioning parameters are TDOA and FDOA. The derivation of the CRLB can 
% be found in K. C. Ho, Xiaoning Lu, L. Kovavisaruch, "Source Localization 
% Using TDOA and FDOA Measurements in the Presence of Receiver Location 
% Errors: Analysis and Solution," IEEE Trans. on Signal Processing, 
% Vol. 55, pp. 684-696, 2007.
%
% Usage: CRLB = TDOAFDOALocMvgSenPosErrCRLB(so,s_doto,uo,u_doto,Q_beta,Q_alpha)
%
% Input Parameter List
% so:  2xM or 3xM true sensor position matrix.
%      M: number of sensors.
%      so(:,i) is the true position of the ith sensor.
% s_doto: 2xM or 3xM true sensor velocity matrix.
%         s_doto(:,i) is the true velocity of the ith sensor.
% uo: 2x1 or 3x1 true source position. 
% u_doto: 2x1 or 3x1 true source velocity.
% Q_beta: 4Mx4M, or 6Mx6M covariance matrix of the sensor position and velocity
%                       reshape([s,s_dot],4M,1) or reshape([s,s_dot],6M,1).
% Q_alpha: 2(M-1)x2(M-1) covariance matrix of the TDOAs (range differences) 
%                           and FDOAs (range rate differences).
%
% The program returns the 4x4 or 6x6 CRLB of [uo;u_doto].
%
% The program can be used for 2D(Dim=2) or 3D(Dim=3) localization.
%
%
% Le Yang, K. C. Ho      08-01-2009
%                        09-05-2010, revised
%
%       Copyright (C) 2009
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA.
%       hod@missouri.edu
%

M = size(so,2);           % Number of sensors.
N = size(so,1);           % Dimension of the localization problem.

E = repmat(eye(N),M,1);
a1 = (uo-so(:,1))'/norm((uo-so(:,1)));

b1 = (u_doto-s_doto(:,1))'/norm(uo-so(:,1)); 
temp = (u_doto-s_doto(:,1))'*(uo-so(:,1))*(uo-so(:,1))';
temp = temp/norm(uo-so(:,1))^3;
b1 = b1 - temp;

for i = 2 : M             % Generate the partial derivatives.
    C1(i-1,:) = [zeros(1,(i-1)*N),(uo-so(:,i))'/norm((uo-so(:,i))),...
                                                         zeros(1,N*(M-i))];
    C1(i-1,1:N) = -a1;
    
    bi = (u_doto-s_doto(:,i))'/norm(uo-so(:,i)); 
    temp = (u_doto-s_doto(:,i))'*(uo-so(:,i))*(uo-so(:,i))';
    temp = temp/norm(uo-so(:,i))^3;
    bi = bi - temp;
    C2(i-1,:) = [zeros(1,(i-1)*N),bi,zeros(1,N*(M-i))];
    C2(i-1,1:N) = -b1;
end;
dr_du = [[C1;C2]*E,[zeros(M-1,N);C1*E]];
dr_ds = [-[C1;C2],[zeros(M-1,M*N);-C1]];

X = dr_du'*inv(Q_alpha)*dr_du;
Y = dr_du'*inv(Q_alpha)*dr_ds;
Z = inv(Q_beta) + dr_ds'*inv(Q_alpha)*dr_ds;

CRLB = inv([X,Y;Y',Z]); 
CRLB = CRLB(1:2*N,1:2*N); % Return the CRLB.