% This program gives an example on how to call the following two routines:
% TDOALocSenPosErrMultiSrc.m and TDOALocSenPosErrMultiSrcCRLB.m. 
%
% This program will reproduce the Figs. 3 and 4 in Le Yang and K. C. Ho, 
% "An approximately efficient TDOA localization algorithm in closed-form 
% for locating multiple disjoint sources with erroneous sensor positions," 
% IEEE Trans. Signal Processing, Vol. 57, pp. 4598-4615, Dec. 2009.
%
% This program requires TDOALocStnySenPosErr(s,rt,Q_beta,Qr) to produce the
% hypothesized source positions.
%
% Le Yang and K. C. Ho   10-01-2010
%
%       Copyright (C) 2010
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA.
%       hod@missouri.edu
%

warning off; clear all;           % Program initialization.
clc;          
L = 1e4;                          % Number of ensemble runs.            
randn('seed',9);                  % Initialize random number generator.

uo = [2000 2500 3000]';           % True source positions for reproducing
uo = [uo; [1500,1550,1500]'];     % the Figs. 3 and 4.

x = [300,400,300,350,-100,200];   % True sensor position matrix.
y = [100 150 500 200 -100,-300];
z = [150 100 200 100 -100,-200];
so = [x; y; z];  

M = size(so,2);                   % Number of sensors.
D = size(so,1);                   % Dimension of the localization problem.
N = length(uo)/D;                 % Number of sources to be localized.

ro = [];
for i = 1 : N
    u = uo((i-1)*D+1:i*D);
    r = sqrt(sum((repmat(u,1,M-1)-so(:,2:M)).^2,1))';
    r = r - norm(u-so(:,1));      
    ro = [ro; r];                 % True source TDOAs (range differences).
end;

R = (eye(M-1)+ones(M-1))/2;       
J = eye(M*D);
Q_alpha = 1e-3 * kron(eye(N),R);  % Covariance matrix of source TDOA (range difference) noise. 

i = 1;                            % Program counter.
fprintf('Simulation in progress');
for sigma_s = -40 : 5 : 0         % Vary sensor position noise power.
    fprintf('.');                 % Program progress indicator.
    Q_beta = J * 10^(sigma_s/10); % Covariance matrix of the sensor position noise.

    CRLB = TDOALocSenPosErrMultiDisjSrcCRLB(so,uo,Q_alpha,Q_beta);
    crlb(i) = trace(CRLB);        % CRLB evaluation.
    crlb1(i) = trace(CRLB(1:D,1:D));         % CRLB for the distant source.
    crlb2(i) = trace(CRLB(D+1:2*D,D+1:2*D)); % CRLB for the near source.
    
    SimulationMSE = 0;      SimulationMSE1 = 0;      SimulationMSE2 = 0;
    chol_Q_alpha = chol(Q_alpha);
    chol_Q_beta = chol(Q_beta);
    
    for k = 1 : L                 % Monte Carlo Simulation.
        delta_s = chol_Q_beta'*randn(M*D,1);
        s = so+ reshape(delta_s,D,M);        % Noisy sensor positions.
        
        delta_r = chol_Q_alpha'*randn(N*(M-1),1);
        r = ro + delta_r;                    % Noisy source TDOAs (range differences).
        
        u = TDOALocSenPosErrMultiDisjSrc(s,r,Q_alpha,Q_beta);
        SimulationMSE = SimulationMSE + norm(u-uo,2)^2;
        SimulationMSE1 = SimulationMSE1 + norm(u(1:D)-uo(1:D),2)^2;
        SimulationMSE2 = SimulationMSE2 + norm(u(D+1:2*D)-uo(D+1:2*D),2)^2;
    end;
    
    mse(i) = SimulationMSE/L;
    mse1(i) = SimulationMSE1/L;
    mse2(i) = SimulationMSE2/L;
    i = i + 1;                               % Update program counter.
end;

% Plot the results.
figure(1); plot(-10:5:30,10*log10(mse),'ks','MarkerSize',8); hold on;
plot(-10:5:30,10*log10(crlb),'k', 'LineWidth', 1); 
xlabel('\sigma_s^2/\sigma_r^2(dB)');
ylabel('10*log(Position MSE(m^2))');
legend('Simulation MSE, proposed solution','CRLB',2);
grid on; axis([-10,30,25,70]); hold off;

figure(2); plot(-10:5:30,10*log10(mse1),'k^','MarkerSize',8); hold on;
plot(-10:5:30,10*log10(mse2),'ks','MarkerSize',8);
plot(-10:5:30,10*log10(crlb1),'k', 'LineWidth', 1); 
plot(-10:5:30,10*log10(crlb2),'k', 'LineWidth', 1);
xlabel('\sigma_s^2/\sigma_r^2(dB)');
ylabel('10*log(Position MSE(m^2))');
grid on; axis([-10,30,15,70]); hold off;