function CRLB = TDOALocSenPosErrMultiCalEmtrIaccLocCRLB(so,uo,co,Q_alpha,Q_c,Q_beta,Q_e)
% This program calculates the CRLB of a stationary source when the sensor
% positions are subject to random noise and calibration emitters at
% inaccurate locations are present. The positioning parameter is TDOA.
%
% The derivation of the CRLB can be found in Le Yang and K. C. Ho, 
% "Alleviating sensor position error in source localization using
% calibration emitters at inaccurate locations," IEEE Trans. Signal 
% Processing, Vol. 58, pp. 67-83, Jan. 2010.
%
% Usage: CRLB = TDOALocSenPosErrMultiCalEmtrIaccLocCRLB(so,uo,co,Q_alpha,Q_c,Q_beta,Q_e)
%
% Input parameter list:
% so: 2xM or 3xM true sensor position matrix. 
%     M: the number of sensors.
%     so(:,i) is the true position of the ith sensor.
% uo: 2x1 or 3x1 true source position vector.
% co: 2Nx1 or 3Nx1 true calibration position vector.
%     N: the number of calibration emitters.
% Q_alpha: (M-1)x(M-1) covariance matrix of the TDOAs (range differences) from uo.
% Q_c: N(M-1)xN(M-1) covariance matrix of the TDOAs (range differences) from co.
% Q_beta: 2Mx2M or 3Mx3M covariance matrix of the sensor positions  
%                                       reshape(s,2M,1) or reshape(s,3M,1).
% Q_e: 2Nx2N or 3Nx3N covariance matrix of the calibration positions.
%
% The program returns the 2x2 or 3x3 CRLB of the source location.
%
% The program can be used for 2D(Dim=2) or 3D(Dim=3) localization.
%
% Le Yang and K. C. Ho   10-01-2010
%
%       Copyright (C) 2010
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA.
%       hod@missouri.edu
%

M = size(so,2);      % Number of sensors.
D = size(so,1);      % Dimension of the localization problem.
N = length(co)/D;    % Number of calibration emitters.

for j = 2: M         % Generate the partial derivatives.
    dr_du(j-1,:) = (uo-so(:,j))'/norm(uo-so(:,j)) - (uo-so(:,1))'/norm(uo-so(:,1));
    dr_ds(j-1,:) = [(uo-so(:,1))'/norm(uo-so(:,1)),zeros(1,(j-2)*D),-(uo-so(:,j))'/norm(uo-so(:,j)),zeros(1,(M-j)*D)];
end;
X = dr_du' * inv(Q_alpha) * dr_du;           
Y = dr_du' * inv(Q_alpha) * dr_ds;

for i = 1 : N
    c = co((i-1)*D+1:i*D);
    for j = 2 : M
        drt_dc((i-1)*(M-1)+(j-1),:) = [zeros(1,(i-1)*D), (c-so(:,j))'/norm(c-so(:,j))-(c-so(:,1))'/norm(c-so(:,1)),zeros(1,(N-i)*D)];
        drt_ds((i-1)*(M-1)+(j-1),:) = [(c-so(:,1))'/norm(c-so(:,1)),zeros(1,(j-2)*D),-(c-so(:,j))'/norm(c-so(:,j)),zeros(1,(M-j)*D)];
    end;
end;
Z = inv(Q_beta) + dr_ds'*inv(Q_alpha)*dr_ds + drt_ds'*inv(Q_c)*drt_ds;

R = drt_dc'*inv(Q_c)*drt_ds;  
P = inv(Q_e) + drt_dc'*inv(Q_c)*drt_dc;

% Return the CRLB of uo.
CRLB = inv(X) + inv(X)*Y*inv(Z-Y'*inv(X)*Y-R'*inv(P)*R)*Y'*inv(X);