% This program provides an example on calling TDOALocSenPosErrCalEmtr.m and
% TDOALocSenPosErrCalEmtrCRLB.m. 
%
% This program will reproduce the Fig. 5 in K. C. Ho and Le Yang, "On the 
% Use of a Calibration Emitter for Source Localization in the Presence of 
% Sensor Position Uncertainty," IEEE Trans. Signal Processing, Vol. 56, 
% pp. 5758-5772, Dec. 2008.
%
% Le Yang, K. C. Ho      08-01-2009
%                        10-01-2010, revised
%
%       Copyright (C) 2009
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA.
%       hod@missouri.edu
%

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

uo = [2000 2500 3000]';           % True source position.
c = [1500,1550,1500]';            % Calibration emitter position.

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.
N = size(so,1);                   % Dimension of localization.

ro = sqrt(sum((repmat(uo,1,M-1)-so(:,2:M)).^2,1))';
ro = ro - norm(uo-so(:,1));              % True source RDs.

r_tildeo = sqrt(sum((repmat(c,1,M-1)-so(:,2:M)).^2,1))';
r_tildeo = r_tildeo - norm(c-so(:,1));   % True calibration RDs.

R = (eye(M-1)+ones(M-1))/2;
J = eye(M*N);
sigma2_r = -30;                   % Source RD noise power in log scale.
Q_alpha = 10^(sigma2_r/10) * R;   % Covariance matrix of source RD noise. 
Q_c = 1e-3 * R;                   % Covariance matrix of calibration RD noise. 
i = 1;                            % Program counter.
fprintf('Simulation in progress');

for sigma2_s = -70 : 5 : 10       % Vary sensor position noise power.
    fprintf('.');                 % Program progress indicator.
    
    Q_beta = J * 10^(sigma2_s/10);% Covariance matrix of sensor position noise.
    crlb(i)=trace(TDOALocSenPosErrCalEmtrCRLB(so,uo,c,Q_alpha,Q_c,Q_beta));
    
    SimulationMSE = 0;
    chol_Q_alpha = chol(Q_alpha);
    chol_Q_beta = chol(Q_beta);
    chol_Q_c = chol(Q_c);
    
    for k = 1 : L                            % Monte Carlo Simulation.
        delta_s = chol_Q_beta'*randn(M*N,1);
        s = so + reshape(delta_s,N,M);       % Noisy sensor positions.
        
        delta_r = chol_Q_alpha'*randn(M-1,1);
        r = ro + delta_r;                    % Noisy source RDs.
        
        delta_r_tilde = chol_Q_c'*randn(M-1,1);
        r_tilde = r_tildeo + delta_r_tilde;  % Noisy calibration RDs.
        
        u = TDOALocSenPosErrCalEmtr(s,r,r_tilde,c,Q_alpha,Q_c,Q_beta);
        SimulationMSE = SimulationMSE + norm(u-uo,2)^2;
    end;
    
    mse(i) = SimulationMSE/L;
    i = i + 1;                    % Update program counter.
end;
fprintf('\n');

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