% This program provides an example on how to call Refine_TDOA_distance_matrix.m 
%
% This program will reproduce the Figs. 4 and 5 in Trung-Kien Le, 
% K. C. Ho and Trung-Hieu Le, "Rank Properties for Matrices 
% Constructed from Time Differences of Arrival, " IEEE Trans. Signal
% Process., vol. 66, No. 13, pp. 3491-3503, Jul. 2018.
%
% This program is an example of calling Refine_TDOA_distance_matrix.m for 
% refining TDOAs
%
% NOTE: The setting below is for the 2-D case.  For the 3-D case, set
% dimension = 3 by uncommenting line 32
%
% NOTE: To generate Fig. 4 or Fig. 5, use NumConfig = 20 and
% NumEnsembleRuns = 50 by uncommenting lines 44 and 45.  The current setting 
% below use NumConfig = 5 and  NumEnsembleRuns = 20 for having some 
% results faster.
%
% Trung-Kien Le, Dominic K. C. Ho   07-15-2018
%
%       Copyright (C) 2018
%       "Rank Properties for Matrices Constructed from Time Differences of Arrival"
%       University of Missouri
%       Columbia, MO 65211, USA.
%       lekt@missouri.edu  and  hod@missouri.edu
%

clc; clear all; warning off;          % Program initialization.
M          = 8;                       % The number of sources 
N          = 15;                      % The number of sensors
dimension  = 2;                       % 2-D, for Fig. 4 in the paper
% dimension  = 3;                       % 3-D, for Fig. 5 in the paper
factor_rho = 10;                      % This is the parameter K in the last 
                                      % paragraph, the second column in the
                                      % 9-th page of the paper, i.e.,
                                      % rho^2 = factor_rho^2*sigma^2, where
                                      % rho^2 is noise power in outliers,
                                      % and sigma^2 is noise power in
                                      % measured TDOAs.
    
sigma      = [0.01 , 0.02 , 0.04 , 0.06 , 0.08 , 0.1];            % in m 
NumConfig = 5;                                                    % Number of configurations
NumEnsembleRuns = 20;                                              % Number of ensemble runs for each configuration                
% NumConfig = 20;                                                    % Number of configurations
% NumEnsembleRuns = 50;                                              % Number of ensemble runs for each configuration                
RMSE0      = zeros(2,length(sigma),NumConfig);                     % Containing Root-mean-square errors in the case of p = 0 (no outliers)
Bias0      = zeros(2,length(sigma),NumConfig,NumEnsembleRuns,M,N-1);    % Containing Bias in the case of p = 0 (no outliers)
RMSE5      = zeros(2,length(sigma),NumConfig);                     % Containing Root-mean-square errors in the case of p = 5 (five outliers)
RMSE5_bad  = zeros(2,length(sigma),NumConfig);                     % Containing Root-mean-square erros of these five outliers
Bias5      = zeros(2,length(sigma),NumConfig,NumEnsembleRuns,M,N-1);    % Containing Bias in the case of p = 5 (five outliers)
                
%===== Setup Positions and Ground-truth of TDOA ===========================
XX     = zeros(length(sigma),NumConfig,dimension,M);   % Using to contain the positions of X
YY     = zeros(length(sigma),NumConfig,dimension,N);   % Using to contain the positions of Y
TTDOA0 = zeros(length(sigma),NumConfig,M,N-1);         % Using to contain the ground-truth of TDOA
for i = 1:length(sigma)
    for num1 = 1:NumConfig
        ok = 0;
        while ok == 0
            X     = 20*rand(dimension,M); Y = 20*rand(dimension,N); Z = [X Y];
            Thres = zeros(M+N,M+N);
            for k = 1:dimension
                Thres = Thres + (Z(k,:)'*ones(1,M+N)-(Z(k,:)'*ones(1,M+N))').^2;
            end
            for j = 1:M+N, Thres(j,j) = 20; end
            if min(Thres(:)) > 0.2, ok = 1; end
        end
        TOA = zeros(M,N);
        for k = 1:dimension
            TOA = TOA + (X(k,:)'*ones(1,N)-(Y(k,:)'*ones(1,M))').^2;
        end
        TOA                = TOA.^(0.5);                          % Ground-truth of TOA
        TDOA0              = TOA(:,2:N) - repmat(TOA(:,1),1,N-1); % Ground-truth of TDOA
        XX(i,num1,:,:)     = X;
        YY(i,num1,:,:)     = Y;
        TTDOA0(i,num1,:,:) = TDOA0;
    end
end  
%==========================================================================
    
%===== Setup Gaussian Noises (for no outlier) =============================
Noise = zeros(length(sigma),NumConfig,NumEnsembleRuns,M,N-1);
Q     = 1/2*(ones(N-1,N-1) + eye(N-1));
Q_cd  = chol(Q);             % Cholesky decomposition
for i = 1:length(sigma)
    Q_cd_i = sigma(i)*Q_cd;
    randn('state',0);
    for num1 = 1:NumConfig
        for num2 = 1:NumEnsembleRuns
            for m = 1:M
                noise                  = Q_cd_i'*randn(N-1,1);    % TDOA noise matrix (N-1 x 1)
                noise                  = reshape(noise,1,N-1);
                Noise(i,num1,num2,m,:) = noise;
            end
        end
    end
    %===== Confirm the noises has zeros-mean ==============================
    for num1 = 1:NumConfig
        for m = 1:M
            for n = 1:N-1
                Noise(i,num1,:,m,n) = Noise(i,num1,:,m,n) - mean(Noise(i,num1,:,m,n));
            end
        end
    end
    %===== Confirm the noise has sigma(i) standard deviation ==============
    for num1 = 1:NumConfig
        A = zeros(1,NumEnsembleRuns);
        for num2 = 1:NumEnsembleRuns
            B       = reshape(Noise(i,num1,num2,:,:),M,N-1);
            A(num2) = (mean(B(:).^2))^(0.5);
        end
        Noise(i,num1,:,:,:) = Noise(i,num1,:,:,:)*(sigma(i)/(mean(A)));
    end    
end
%==========================================================================
    
%===== Setup Large Gaussian Noises (for having 5 outliers) ================
Normal_Noise_5 = zeros(length(sigma),NumConfig,NumEnsembleRuns,M,N-1);
Large_Noise_5  = zeros(length(sigma),NumConfig,NumEnsembleRuns,M,N-1);
Index_5        = cell(1,length(sigma),NumConfig);
randn('state',0);
for i = 1:length(sigma)
    for num1 = 1:NumConfig
        Q      = 1/2*(ones(N-1,N-1) + eye(N-1));
        Q_cd   = chol(Q);
        Q_cd_i = sigma(i)*Q_cd;
        for num2 = 1:NumEnsembleRuns
            for m = 1:M
                noise                           = Q_cd_i'*randn(N-1,1);   % TDOA noise matrix (N-1 x 1)
                noise                           = reshape(noise,1,N-1);
                Normal_Noise_5(i,num1,num2,m,:) = noise;
            end
        end
        %===== Confirm the noises has zeros-mean ==========================
        for m = 1:M
            for n = 1:N-1
                Normal_Noise_5(i,num1,:,m,n) = Normal_Noise_5(i,num1,:,m,n) - mean(Normal_Noise_5(i,num1,:,m,n));
            end
        end
        %===== Confirm the noise has sigma(i) standard deviation ==========
        A = zeros(1,NumEnsembleRuns);
        for num2 = 1:NumEnsembleRuns
            B       = reshape(Normal_Noise_5(i,num1,num2,:,:),M,N-1);
            A(num2) = (mean(B(:).^2))^(0.5);
        end
        Normal_Noise_5(i,num1,:,:,:) = Normal_Noise_5(i,num1,:,:,:)*(sigma(i)/(mean(A)));
    
        rho = factor_rho*sigma(i);
        Idx = zeros(2,5);
        for k = 1:5
            a        = floor(M*rand(1))+1; 
            b        = floor((N-1)*rand(1))+1;
            Idx(:,k) = [a ; b]; 
        end
        noise = rho*randn(5,NumEnsembleRuns);
        %===== Confirm zero-mean ==========================================
        for k = 1:5
            noise(k,:) = noise(k,:) - mean(noise(k,:));
        end
        %===== Confirm standard deviation is rho ==========================
        A = zeros(1,NumEnsembleRuns);
        for num2 = 1:NumEnsembleRuns
            A(num2) = (mean(noise(:,num2).^2))^(0.5);
        end
        noise = noise*(rho/mean(A));
        for k = 1:5
            Large_Noise_5(i,num1,:,Idx(1,k),Idx(2,k)) = noise(k,:);
        end
        Index_5{i,num1} = Idx;
    end
end
        
%==========================================================================
%==========================================================================    
for i = 1:length(sigma)
    fprintf('.');
    %===== p = 0 =====
    for num1 = 1:NumConfig
        TDOA0 = reshape(TTDOA0(i,num1,:,:),M,N-1);   % Ground-truth for TDOA
        T1    = zeros(1,NumEnsembleRuns);
        T2    = zeros(1,NumEnsembleRuns);
        for num2 = 1:NumEnsembleRuns
            TDOA                     = TDOA0 + reshape(Noise(i,num1,num2,:,:),M,N-1);     % Add typical noises
            TDOA_re                  = Refine_TDOA_distance_matrix(TDOA, dimension, 20); 
            Bias0(1,i,num1,num2,:,:) = TDOA - TDOA0;
            Bias0(2,i,num1,num2,:,:) = TDOA_re - TDOA0;
            T1(num2)                 = (mean((TDOA(:) - TDOA0(:)).^2))^0.5;
            T2(num2)                 = (mean((TDOA_re(:) - TDOA0(:)).^2))^0.5;
        end
        RMSE0(1,i,num1) = mean(T1);
        RMSE0(2,i,num1) = mean(T2);
    end

    %===== p = 5 =====
    for num1 = 1:NumConfig
        TDOA0 = reshape(TTDOA0(i,num1,:,:),M,N-1);
        T1    = zeros(1,NumEnsembleRuns);
        T2    = zeros(1,NumEnsembleRuns);
        B1    = zeros(1,NumEnsembleRuns);
        B2    = zeros(1,NumEnsembleRuns);
        Idx   = Index_5{i,num1};
        for num2 = 1:NumEnsembleRuns
            TDOA_5                   = TDOA0 + reshape(Normal_Noise_5(i,num1,num2,:,:),M,N-1) + reshape(Large_Noise_5(i,num1,num2,:,:),M,N-1);
            TDOA_re                  = Refine_TDOA_distance_matrix(TDOA_5, dimension, 20);
            Bias5(1,i,num1,num2,:,:) = TDOA_5 - TDOA0;
            Bias5(2,i,num1,num2,:,:) = TDOA_re - TDOA0;
            T1(num2)                 = (mean((TDOA_5(:) - TDOA0(:)).^2))^0.5;
            T2(num2)                 = (mean((TDOA_re(:) - TDOA0(:)).^2))^0.5;
            Bad1                     = 0;
            Bad2                     = 0;
            for k = 1:5
                Bad1 = Bad1 + (TDOA_5(Idx(1,k),Idx(2,k)) - TDOA0(Idx(1,k),Idx(2,k)))^2;
                Bad2 = Bad2 + (TDOA_re(Idx(1,k),Idx(2,k)) - TDOA0(Idx(1,k),Idx(2,k)))^2;    
            end
            Bad1     = Bad1/5; Bad1 = Bad1^(0.5);
            Bad2     = Bad2/5; Bad2 = Bad2^(0.5);
            B1(num2) = Bad1;
            B2(num2) = Bad2;
        end
        RMSE5(1,i,num1)     = mean(T1);
        RMSE5(2,i,num1)     = mean(T2);
        RMSE5_bad(1,i,num1) = mean(B1);
        RMSE5_bad(2,i,num1) = mean(B2);
    end
%    toc
end
fprintf('\n');
    
%===== Figures ============================================================
A = mean(Bias0(2,:,:,:,:,:),4); A = reshape(A,length(sigma),NumConfig,M,N-1); B = zeros(length(sigma),NumConfig);
for i = 1:length(sigma)
    for num1 = 1:NumConfig
        C = reshape(A(i,num1,:,:),M,N-1); B(i,num1) = (mean(C(:).^2))^(0.5);
    end
end
B = mean(B,2); reshape(B,1,length(sigma));
figure; subplot(1,3,1); hold on; plot([1,2,4,6,8,10],100*mean(reshape(RMSE0(1,:,:),length(sigma),NumConfig),2),'bo--'); 
                                 plot([1,2,4,6,8,10],100*mean(reshape(RMSE0(2,:,:),length(sigma),NumConfig),2),'r^-'); 
                                 plot([1,2,4,6,8,10],100*B,'ks-.'); 
                                 axis tight; grid on;
                                 xlabel('\sigma (cm)'); ylabel('RMSE and Bias (cm)'); title('p = 0, for all TDOAs');
A = mean(Bias5(2,:,:,:,:,:),4); A = reshape(A,length(sigma),NumConfig,M,N-1); B = zeros(length(sigma),NumConfig);
for i = 1:length(sigma)
    for num1 = 1:NumConfig
        C = reshape(A(i,num1,:,:),M,N-1); B(i,num1) = (mean(C(:).^2))^(0.5);
    end
end
B = mean(B,2); reshape(B,1,length(sigma));
subplot(1,3,2); hold on; plot([1,2,4,6,8,10],100*mean(reshape(RMSE5(1,:,:),length(sigma),NumConfig),2),'bo--'); 
                         plot([1,2,4,6,8,10],100*mean(reshape(RMSE5(2,:,:),length(sigma),NumConfig),2),'r^-'); 
                         plot([1,2,4,6,8,10],100*B,'ks-.'); 
                         axis tight; grid on;
                         xlabel('\sigma (cm)'); ylabel('RMSE and Bias (cm)'); title('p = 5, for all TDOAs');
A = zeros(length(sigma),NumConfig,5);
for i = 1:length(sigma)
    for num1 = 1:NumConfig
        Idx = Index_5{i,num1};
        for k = 1:5
            A(i,num1,k) = mean(Bias5(2,i,num1,:,Idx(1,k),Idx(2,k)),4); 
        end
    end
end
B = zeros(length(sigma),NumConfig);
for i = 1:length(sigma)
    for num1 = 1:NumConfig
        C = reshape(A(i,num1,:),1,5); B(i,num1) = (mean(C.^2))^(0.5);
    end
end
B = mean(B,2); reshape(B,1,length(sigma));
subplot(1,3,3); hold on; plot([1,2,4,6,8,10],100*mean(reshape(RMSE5_bad(1,:,:),length(sigma),NumConfig),2),'bo--'); 
                         plot([1,2,4,6,8,10],100*mean(reshape(RMSE5_bad(2,:,:),length(sigma),NumConfig),2),'r^-'); 
                         plot([1,2,4,6,8,10],100*B,'ks-.'); 
                         axis tight; grid on;
                         xlabel('\sigma (cm)'); ylabel('RMSE and Bias (cm)'); title('p = 5, for worse TDOAs');
                         