function [CRLB] = EllpLocMultTxRxRdnCCRLB(uo,t_bar,s_bar,c_bar,Q,Qt,Qs,Qc,Qtheta)
%
% This program finds the hybrid CRLB of the target position in the multistatic
% sonar localization problem. It can work in two modes: the elliptic time
% measurements only and elliptic time measurements and bearing angle
% measurements both. The true transmitter and receiver positions and the
% signal propagatin speed are random variables and unknown. Their nominal values
% are available. The derivation of the CRLB can be found in 
% Liyang Rui and K. C. Ho, "Efficient closed-form estimators for 
% multistatic sonar localization," IEEE Trans. Aerosp. Electron. Syst., 
% vol. 51, no. 1, pp. 600-614.
%
% Usage: CRLB = EllpLocMultTxRxRdnCCRLB(uo,t_bar,s_bar,c_bar,Q,Qt,Qs,Qc,Qtheta)
%
% Input Parameter List
% uo: (2 x 1), true target position. 
% t_bar: (2 x M), nominal transmitter position matrix.
%     M: number of transmitters
%     t_bar(:,i) is the nominal position of the ith transmitter.
% s_bar: (2 x N), nominal receiver position matrix.
%     N: number of receivers.
%     s_bar(:,i) is the nominal position of the ith receiver.
% c_bar: nominal sound propagtion speed
% Q: (MN x MN), covariance matrix of the time measurements.
% Qt: (2M x 2M), covariance matrix of the transmitter position uncertainties
% Qs: (2N x 2N), covariance matrix of the receiver position uncertainties
% Qc: variance of the signal propagation speed uncertainty
% Qtheta: (N x N), covariance matrix of the bearing angle measurements
%     When Qtheta is empty, the program computes the hybrid CRLB using time
%     measurements only
% 
% The program returns the 2(M+N+1)+1 x 2(M+N+1)+1 CRLB of [uo;c;t;s].
%
% The program is implemented for 2D(Dim=2) localization only.
%
% Liyang Rui and K. C. Ho      12-28-2029
%
%       Copyright (C) 2019
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA.
%       hod@missouri.edu
%

M = size(t_bar,2);                          % Number of transmitters.
N = size(s_bar,2);                          % Number of receivers.
L = size(s_bar,1);                          % Localization Dimension

r_ut = sqrt(sum((uo*ones(1,M)-t_bar).^2))';   % True target-transmitter distances
r_us = sqrt(sum((uo*ones(1,N)-s_bar).^2))';   % True target-receiver distances
r_ts = zeros(M,N);
for i = 1:M,
    for j= 1:N,
        r_ts(i,j) = sqrt(sum((t_bar(:,i)-s_bar(:,j)).^2)); % True transmitter-receiver distances
    end
end
rdoao = ones(M,1)*r_us'+r_ut*ones(1,N)-r_ts;    % True value of the elliptic range 
tau = rdoao/c_bar;                  

% Build the Jacobian matrix of elliptic time measurements
D = zeros(L+1+(M+N)*L,M*N);            
for i = 1:M,
    for j = 1:N,
        rho_uti = (uo-t_bar(:,i))/r_ut(i);      % Unit vectors
        rho_usj = (uo-s_bar(:,j))/r_us(j);
        rho_tisj = (t_bar(:,i)-s_bar(:,j))/r_ts(i,j);
        
        D(1:L,(i-1)*N+j) = (rho_uti+rho_usj)/c_bar;
        D(L+1,(i-1)*N+j) = -tau(i,j)/c_bar;
        D(L+1+(i-1)*L+1:L+1+i*L,(i-1)*N+j) = -(rho_uti+rho_tisj)/c_bar;
        D(L+1+M*L+(j-1)*L+1:L+1+M*L+j*L,(i-1)*N+j) = (-rho_usj+rho_tisj)/c_bar;
    end
end
% Covariance matrix of the prior knowledge (sound speed, transmitter and receiver positions)
temp = zeros(L+1+(M+N)*L);
temp(L+1,L+1) = Qc^-1;
temp(L+1+1:L+1+M*L,L+1+1:L+1+M*L) = Qt^-1;
temp(L+1+M*L+1:L+1+M*L+N*L,L+1+M*L+1:L+1+M*L+N*L) = Qs^-1;

if isempty(Qtheta),                             % If there is no bearing angle measurement 
    FIM = D/Q*D'+temp;
    CRLB = inv(FIM);
    return;
end

% If there are bearing measurements, we build the Jacobian matrix of the bearing measurements
P = [0 -1;1 0];
d_A1 = (uo*ones(1,N)-s_bar)'./((r_us*ones(1,L)).^2)*P';
d_A2 = zeros(N, N*L);
for i = 1:N,
    d_A2(i,:)=-[zeros(1,(i-1)*L),(uo-s_bar(:,i))'./((r_us(i)'*ones(1,L)).^2)*P',zeros(1,(N-i)*L)];
end
A = [d_A1 zeros(N,1) zeros(N,M*L) d_A2];
FIM = D/Q*D'+temp+A'/Qtheta*A;
CRLB = inv(FIM);

