function u_hat = EllpLocMultTxRxRdnC(tau,t_bar,s_bar,c_bar,theta,Q,Qt,Qs,Qc,Qtheta)
%
% This program realizes the target localization algorithm proposed for the multistatic
% sonar system. It works in two modes: the elliptic time measurements only
% and ellitpic time measurements and bearing angle measurements both. The true
% transmitter and receiver positions and the signal propagation speed are random
% variables and unknown. Their nominal values are available. The algorithm
% derivation 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: u_hat = EllpLocMultTxRxRdnC(tau,t_bar,s_bar,c_bar,theta,Q,Qt,Qs,Qc,Qtheta)
%
% Input Parameter List
% tau: (M x N), noisy time measurement matrix
% 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: the nominal sound propagation speed
% theta: (N x 1), bearing angle measurement vector
% 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: the standard deviation of the speed of sound 
% Qtheta: (N x N), covariance matrix of bearing angles
%
% The program returns the 2 x 1 vector of position estimate u_hat.
%
% When using only time delay measurements, set theta=[] and Qtheta=[].
%
% The program is implemented for 2D(Dim=2) localization only.
%
% Liyang Rui and K. C. Ho      12-28-2019
%
%       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

tau_vct = reshape(tau',M*N,1);          % Restructure time measurements vector

% Stage 1 processing has two iterations. Iteration 1: Assume matrices Dt and Ds (Ds_angle) are zero and B1 (B_angle) is ones; 
% Iteration 2: update matrices Dt and Ds using the result from iteration 1.
h1 = zeros(M*N+N,1);
G1 = zeros(M*N+N,L+M);
Dt = zeros(M*N,M*L);
Ds = zeros(M*N,N*L);
B1 = eye(M*N);
Ds_angle = zeros(N,N*L);
B_angle = eye(N);

for iteration = 1:2,
    if iteration > 1,          % Update some matrices if target position estimate exist
        u_temp = phi1(1:L);
        r_us = sqrt(sum((u_temp*ones(1,N)-s_bar).^2))';
        r_ut = sqrt(sum((u_temp*ones(1,M)-t_bar).^2))';   
        r_ts = zeros(M,N);
        for j = 1:N,
            rho_usj = (u_temp-s_bar(:,j))/r_us(j);
            for i= 1:M,
                rho_uti = (u_temp-t_bar(:,i))/r_ut(i);
                r_ts(i,j) = sqrt(sum((t_bar(:,i)-s_bar(:,j)).^2));
                rho_tisj = (t_bar(:,i)-s_bar(:,j))/r_ts(i,j);
                Dt((i-1)*N+j,(i-1)*L+1:i*L) = -(rho_uti+rho_tisj);
                Ds((i-1)*N+j,(j-1)*L+1:j*L) = -(rho_usj-rho_tisj);                
            end
            if ~isempty(theta),
                Ds_angle(j,(j-1)*L+1:j*L)=-[-sin(theta(j)) cos(theta(j))];
                B_angle(j,j) = [cos(theta(j)) sin(theta(j))]*(u_temp-s_bar(:,j));
            end
        end
        B1 = 2*kron(eye(M),diag(r_us));
    end
    
    for j = 1:N,
        s_temp = s_bar(:,j);
        for i = 1:M,
            t_temp = t_bar(:,i);
            r_temp = sqrt(sum((t_temp-s_temp).^2));
            tau_temp = tau(i,j);
        
            h1((i-1)*N+j) = 2*(t_temp'*t_temp-t_temp'*s_temp+r_temp*tau_temp*c_bar)+(tau_temp*c_bar)^2;
            G1((i-1)*N+j,1:L) = 2*(t_temp-s_temp)';
            G1((i-1)*N+j,L+i) = 2*(r_temp+tau_temp*c_bar);
        end
        if ~isempty(theta),
            h1(M*N+j) = s_temp'*[-sin(theta(j));cos(theta(j))];
            G1(M*N+j,1:L) = [-sin(theta(j)) cos(theta(j))];
        end
    end

    temp = (c_bar^2)*Q + Dt*Qt*Dt' + Ds*Qs*Ds' + tau_vct*tau_vct'*Qc;
    if isempty(theta),
        W1_inv = [B1*temp*B1' zeros(M*N,N); zeros(N,M*N) eye(N)];
    else
        W1_inv = [B1*temp*B1' B1*Ds*Qs*Ds_angle'; ...
            Ds_angle*Qs*Ds'*B1' B_angle*Qtheta*B_angle+Ds_angle*Qs*Ds_angle'];
    end
    phi1 = ((G1'/W1_inv*G1)\G1')/W1_inv*h1;
end

% Stage 2 processing
h2 = phi1;
G2 = zeros(L+M,L+1); G2(1:L,1:L) = eye(L);
B2 = zeros(L+M); B2(1:L,1:L) = eye(L);
for i = 1:M,
    t_temp = t_bar(:,i);
    h2(L+i) = h2(L+i)^2-t_temp'*t_temp;
    G2(L+i,:) = [-2*t_temp' 1];
    B2(L+i,L+i) = 2*phi1(L+i);
end
W2_inv = B2/(G1'/W1_inv*G1)*(B2');
phi2 = ((G2'/W2_inv*G2)\G2')/W2_inv*h2;

% Stage 3 processing
h3 = phi2; h3(1:L) = h3(1:L).*h3(1:L);
G3 = zeros(L+1,L); G3(1:L,1:L) = eye(L); G3(L+1,1:L) = ones(1,L);
B3 = zeros(L+1); B3(1:L,1:L) = 2*diag(phi2(1:L)); B3(L+1,L+1) = 1;
W3_inv = B3/(G2'/W2_inv*G2)*(B3');
phi3 = ((G3'/W3_inv*G3)\G3')/W3_inv*h3;

% Stage 4: Map back to u_hat
u_hat = sign(phi2(1:L)).*sqrt(abs(phi3));