% This program gives an example on how to call the following routines:
% EllpLocMulTxRxUknCCRLB.m
% EllpLocMulTxRxUknC.m
%
% This program will reproduce the Figs. 5 and 6 in Liyang Rui and K. C. Ho, 
% "Efficient closed-form estimators for multistatic sonar localization,"
% IEEE Trans. Aerosp. Electron. Syst., vol. 50, no. 1, pp. 600-614.
%
% Note: The ensemble run number is 3,000 (instead of 10,000 as in the paper) 
% to reduce processing time (see line 25).
%
% Note: The number of true signal propagation speed is 4 (instead of 100
% as in the paper) to reduce processing time (see line 26).
%
% 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 = 3000;      % Number of ensemble runs (K=10e3 in the paper)
c_K = 4;       % Number of true signal speed (c_K=100 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);                              % Localization dimension

sigma_s = 20;                                   % Std of the transmitter and receiver position noise
Qs = eye(N*L)*sigma_s^2;                        % Covariance matrix of receiver positions
Qt = eye(M*L)*sigma_s^2;                        % Covariance matrix of transmitter positions
sigma_theta = 2*pi/180;                         % Std of the bearing angle measurement noise
Q_angle = sigma_theta^2*eye(N);                 % Covariance matrix of bearing measurement noise
sigma_tau = 0.02:0.02:0.2;                      % Std vector of time measurement noise

crlb_loc_tauOnly = zeros(c_K,length(sigma_tau));
crlb_c_tauOnly = zeros(c_K,length(sigma_tau));
crlb_loc_tau_theta = zeros(c_K,length(sigma_tau));
crlb_c_tau_theta = zeros(c_K,length(sigma_tau));

mse_loc_tauOnly = zeros(c_K,length(sigma_tau));
mse_c_tauOnly = zeros(c_K,length(sigma_tau));
mse_loc_tau_theta = zeros(c_K,length(sigma_tau));
mse_c_tau_theta = zeros(c_K,length(sigma_tau));

rand('state',0);                                % Initialize random number generator
for kk = 1:c_K,
    c_true = 1450 + 100*rand(1);                % Generate true sound propagation speed
    fprintf('%d/%d: ',kk,c_K);
    
    for nseIdx=1:length(sigma_tau),             % vary the time measurement noise level
        randn('seed',0); 
        fprintf('.');

        Q = eye(M*N)*sigma_tau(nseIdx)^2;       % Covariance matrix of time measurement noise

        crlb_temp = diag(EllpLocMultTxRxUknCCRLB(uo,t_bar,s_bar,c_true,Q,Qt,Qs,[]));
        crlb_loc_tauOnly(kk,nseIdx) = sum(crlb_temp(1:L));  % CRLB of the target position when only time measurements are used
        crlb_c_tauOnly(kk,nseIdx) = crlb_temp(L+1);         % CRLB of the sound propagation speed when only time measurements are used
        clear crlb_temp;
        
        crlb_temp = diag(EllpLocMultTxRxUknCCRLB(uo,t_bar,s_bar,c_true,Q,Qt,Qs,Q_angle));
        crlb_loc_tau_theta(kk,nseIdx) = sum(crlb_temp(1:L));
        crlb_c_tau_theta(kk,nseIdx) = crlb_temp(L+1);
        clear crlb_temp;

        SimulationMSE_loc_tauOnly = 0;                      % Temporal variables
        SimulationMSE_c_tauOnly = 0;
        SimulationMSE_loc_tau_theta = 0;
        SimulationMSE_c_tau_theta = 0;                      
    
        for k = 1 : K,                                      % Monte Carlo Simulation
            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);
            for i = 1:M,
                for j= 1:N,
                    r_ts(i,j) = sqrt(sum((t_true(:,i)-s_true(:,j)).^2)); % True transmitter-receiver distances
                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(Q_angle)'*randn(N,1);% Noisy bearing measurements
            
            % The algorithm proposed in Section IV-B (without bearing measurements)
            [u_hat_tauOnly, c_hat_tauOnly] = EllpLocMultTxRxUknC(tau_noisy,t_bar,s_bar,[],Q,Qt,Qs,[]);
            % The algorithm proposed in Section VI (with bearing measurements)
            [u_hat_tau_theta, c_hat_tau_theta] = EllpLocMultTxRxUknC(tau_noisy,t_bar,s_bar,theta_noisy,Q,Qt,Qs,Q_angle);
            % Update temporal variables
            SimulationMSE_loc_tauOnly = SimulationMSE_loc_tauOnly + norm(u_hat_tauOnly-uo,2)^2;
            SimulationMSE_c_tauOnly = SimulationMSE_c_tauOnly + (c_hat_tauOnly - c_true)^2;
            SimulationMSE_loc_tau_theta = SimulationMSE_loc_tau_theta + norm(u_hat_tau_theta-uo,2)^2;
            SimulationMSE_c_tau_theta = SimulationMSE_c_tau_theta + (c_hat_tau_theta - c_true)^2;
        end
        % Save the computed MSE
        mse_loc_tauOnly(kk,nseIdx) = SimulationMSE_loc_tauOnly/K;
        mse_c_tauOnly(kk,nseIdx) = SimulationMSE_c_tauOnly/K;
        mse_loc_tau_theta(kk,nseIdx) = SimulationMSE_loc_tau_theta/K;
        mse_c_tau_theta(kk,nseIdx) = SimulationMSE_c_tau_theta/K;
    end
    fprintf('\n');
end

figure;
plot(sigma_tau,10*log10(mean(crlb_loc_tauOnly)),'-k','LineWidth',1.5); grid on; hold on;
plot(sigma_tau,10*log10(mean(crlb_loc_tau_theta)),'-.k','LineWidth',1.5); grid on; hold on;
plot(sigma_tau,10*log10(mean(mse_loc_tauOnly)),'ok','MarkerSize',8.5,'LineWidth',1.5); hold on;
plot(sigma_tau,10*log10(mean(mse_loc_tau_theta)),'sk','MarkerSize',8.5,'LineWidth',1.5); hold on;
xlim([0.02 0.2]);
ylim([25 50]);
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)');

figure;
plot(sigma_tau,10*log10(mean(crlb_c_tauOnly)),'-k','LineWidth',1.5); grid on; hold on;
plot(sigma_tau,10*log10(mean(crlb_c_tau_theta)),'-.k','LineWidth',1.5); grid on; hold on;
plot(sigma_tau,10*log10(mean(mse_c_tauOnly)),'ok','MarkerSize',8.5,'LineWidth',1.5); hold on;
plot(sigma_tau,10*log10(mean(mse_c_tau_theta)),'sk','MarkerSize',8.5,'LineWidth',1.5); hold on;
xlim([0.02 0.2]);
ylim([15 45]);
xlabel('\sigma_\tau (s)'); 
ylabel('10 log(MSE) (m^2/s^2)');
legend('Hybrid CRLB (Without Bearing Measurements)','Hybrid CRLB (With Bearing Measurements)',...
    'Proposed Algorithm (Without Bearing Measurements)','Proposed Algorithm (With Bearing Measurements)');