% This program gives an example on how to call the following routines:
% EllpLocMulTxRxRdnCCRLB.m
% EllpLocMulTxRxRdnC.m
%
% This program will reproduce the Fig. 3 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.
%
% Note: The ensemble run number is 500 (instead of 10,000 as in the paper) 
% to reduce processing time (see line 22)
%
% 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
%

clc; clear all; warning off;         % Program initialization
K = 500;       % Number of ensemble runs (K=10e3 in the paper)

uo = [0 2000]';                                 % True target position
t_bar = [1500 1500; -900 4000; -3000 -4000]';   % Nominal receiver positions
s_bar = [-1000 3000; 2500 -500; -3000 1000; 2000 -4000; -2000 -2000]';  % Nominal transmitter positions
c_bar = 1500;                                   % Nominal signal speed

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

sigma_s = 20;                                   % Std of the transmitter and receiver position noise
Qt = eye(M*L)*sigma_s^2;                        % Covariance matrix of transmitter positions
Qs = eye(N*L)*sigma_s^2;                        % Covariance matrix of receiver positions
sigma_c = 7.5;                                  % Std of sound propagation speed noise
Qc = sigma_c^2;                                 % Variance of sound propagation speed
sigma_theta = 2*pi/180;                         % Std of bearing angle measurement noise
Qangle = sigma_theta^2*eye(N);                  % Covariance matrix of bearing measurement noise

sigma_tau = 0.02:0.02:0.2;                      % Vary the std of time measurement noise

crlb_tauOnly = zeros(1,length(sigma_tau));      % CRLB of the target position using time measurements only
crlb_tau_theta = zeros(1,length(sigma_tau));    % CRLB of the target position using both time and bearing measurements

mse_tauOnly = zeros(1,length(sigma_tau));       % MSE of the target position estimated by the algorithm proposed in Section IV-A (without bearing measurements)
mse_tau_theta = zeros(1,length(sigma_tau));     % MSE of the target position estimated by the algorithm proposed in Section VI (with bearing measurements)
mse_Bayesian = zeros(1,length(sigma_tau));      % MSE of the target position estimated by the Bayesian filter method [2]
mse_Weiner = zeros(1,length(sigma_tau));        % MSE of the target position estimated by the Weiner filter method [26]

for nseIdx=1:length(sigma_tau),                 % Vary time measurement noise level
    randn('seed',3); 
    fprintf('.');                   
    
    Q = eye(M*N)*sigma_tau(nseIdx)^2;           % Covariance matrix of time measurement noise
    
    crlb_temp = diag(EllpLocMultTxRxRdnCCRLB(uo,t_bar,s_bar,c_bar,Q,Qt,Qs,Qc,[]));
    crlb_tauOnly(nseIdx) = sum(crlb_temp(1:L)); % CRLB of the target position when only time measurements are used
    clear crlb_temp;
    
    crlb_temp = diag(EllpLocMultTxRxRdnCCRLB(uo,t_bar,s_bar,c_bar,Q,Qt,Qs,Qc,Qangle));
    crlb_tau_theta(nseIdx) = sum(crlb_temp(1:L));   % CRLB of the target position when both time and bearing measurements are used
    clear crlb_temp;

    SimulationMSE_tauOnly = 0;                      % Temporal variables
    SimulationMSE_tau_theta = 0;
    SimulationMSE_Bayesian = 0;
    SimulationMSE_Weiner = 0;                       
    
    for k = 1 : K,                                  % Monte Carlo Simulation            
        c_true = c_bar + sigma_c*randn(1);          % True sound propagation speed
        t_true = t_bar + reshape(chol(Qt)'*randn(M*L,1),L,M);   % True transmitter positions
        s_true = s_bar + reshape(chol(Qs)'*randn(N*L,1),L,N);   % True receiver positions
        
        r_ut = sqrt(sum((uo*ones(1,M)-t_true).^2))';    % True target-transmitter distances
        r_us = sqrt(sum((uo*ones(1,N)-s_true).^2))';    % True target-receiver distances
        r_ts = zeros(M,N);                              % True transmitter-receiver distances
        for i = 1:M,
            for j= 1:N,
                r_ts(i,j) = sqrt(sum((t_true(:,i)-s_true(:,j)).^2));
            end
        end
        rdoao = ones(M,1)*r_us'+r_ut*ones(1,N)-r_ts;    % True range measurements
        tau = rdoao/c_true;                             % True time measurements
        tau_noisy = tau + reshape(sigma_tau(nseIdx)*randn(M*N,1),M,N);  % Noisy time measurements
        
        dif_true = uo*ones(1,N)-s_true;
        x_dif = dif_true(1,:);
        y_dif = dif_true(2,:);
        theta = atan2(y_dif,x_dif);                         % True bearing measurements
        theta_noisy = theta' + chol(Qangle)'*randn(N,1);    % Noisy bearing measurements

        % The algorithm proposed in Section IV-A (without bearing measurements)
        u_hat_tauOnly = EllpLocMultTxRxRdnC(tau_noisy,t_bar,s_bar,c_bar,[],Q,Qt,Qs,Qc,[]);
        % The algorithm proposed in Section VI (with bearing measurements)
        u_hat_tau_theta = EllpLocMultTxRxRdnC(tau_noisy,t_bar,s_bar,c_bar,theta_noisy,Q,Qt,Qs,Qc,Qangle);
%         % The Bayesian algorithm proposed in [2]
%         u_hat_bayesian = BayesianLoc(tau_noisy, t_bar, s_bar, c_bar, theta_noisy, sigma_tau(nseIdx), Qt, Qs, sigma_c, sigma_theta);
%         % The Weiner filter algorithm proposed in [26]
%         u_hat_weiner = WeinerLoc(tau_noisy, t_bar, s_bar, c_bar, theta_noisy, sigma_tau(nseIdx), Qt, Qs, Qangle);             
        
        % Update temporal variables
        SimulationMSE_tauOnly = SimulationMSE_tauOnly + norm(u_hat_tauOnly-uo,2)^2;
        SimulationMSE_tau_theta = SimulationMSE_tau_theta + norm(u_hat_tau_theta-uo,2)^2;
%         SimulationMSE_Bayesian = SimulationMSE_Bayesian + norm(u_hat_bayesian-uo,2)^2;
%         SimulationMSE_Weiner = SimulationMSE_Weiner + norm(u_hat_weiner-uo,2)^2;
    end
    % Save the computed MSE
    mse_tauOnly(nseIdx) = SimulationMSE_tauOnly/K;
    mse_tau_theta(nseIdx) = SimulationMSE_tau_theta/K;
%     mse_Weiner(nseIdx) = SimulationMSE_Weiner/K;
%     mse_Bayesian(nseIdx) = SimulationMSE_Bayesian/K;
end
fprintf('\n');

% Plot the results.
figure;
plot(sigma_tau,10*log10(crlb_tauOnly),'-k','LineWidth',1.5); grid on; hold on;
plot(sigma_tau,10*log10(crlb_tau_theta),'-.k','LineWidth',1.5); grid on; hold on;
plot(sigma_tau,10*log10(mse_tauOnly),'ok','MarkerSize',8.5,'LineWidth',1.5); hold on;
plot(sigma_tau,10*log10(mse_tau_theta),'sk','MarkerSize',8.5,'LineWidth',1.5); hold on;
xlim([0.02 0.2]);
ylim([20 80]);
xlabel('\sigma_\tau (s)'); 
ylabel('10 log(MSE) (m^2)');
legend('Hybrid CRLB (Without Bearing Measurements)','Hybrid CRLB (With Bearing Measurements)',...
        'Proposed Algorithm (Without Bearing Measurements)','Proposed Algorithm (With Bearing Measurements)');