% This program gives an example on how to call the following two routines:
% TDOALocSenPosErrMultiCalEmtr.m and TDOALocSenPosErrMultiCalEmtrCRLB.m. 
%
% This program will reproduce the Fig. 6(a) in Le Yang and K. C. Ho, 
% "Alleviating sensor position error in source localization using
% calibration emitters at inaccurate locations," IEEE Trans. Signal 
% Processing, Vol. 58, pp. 67-83, Jan. 2010. 
%
% 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',0);                  % Initialize random number generator.

uo = [2000 2500 3000]';           % True source position.
co = [1500,1550,1500]';           % True calibration emitter position (for reproducing Fig. 6(a)).
% co = [co;[1000,1500,1550]'];      % True calibration emitter positions(for reproducing Fig. 7).

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(co)/D;                 % Number of calibration emitters.

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

r_tildeo = [];
for i = 1 : N
    c = co((i-1)*D+1:i*D);
    rt = sqrt(sum((repmat(c,1,M-1)-so(:,2:M)).^2,1))';
    rt = rt - norm(c-so(:,1));   
    r_tildeo = [r_tildeo; rt];    % True calibration TDOAs (range differences).
end;

R = (eye(M-1)+ones(M-1))/2;       
J = eye(M*D);

Q_alpha = 1e-3 * R;               % Covariance matrix of source TDOA (range difference) noise. 
Q_beta = 1e-2 * J;                % Covariance matrix of sensor position noise.
Q_c = kron(eye(N),1e-3 * R);      % Covariance matrix of calibration TDOA (range difference) noise. 

i = 1;                            % Program counter.
fprintf('Simulation in progress');

for sigma_e = -40 : 5 : 40        % Vary calibration position noise power.
    fprintf('.');                 % Program progress indicator.
    
    Q_e =eye(N*D)*10^(sigma_e/10);% Covariance matrix of the calibration position noise.
    crlb(i)=trace(TDOALocSenPosErrMultiCalEmtrIaccLocCRLB(so,uo,co,Q_alpha,Q_c,Q_beta,Q_e));
    
    SimulationMSE = 0;            
    chol_Q_alpha = chol(Q_alpha);
    chol_Q_beta = chol(Q_beta);
    chol_Q_c = chol(Q_c);
    chol_Q_e = chol(Q_e);

    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(M-1,1);
        r = ro + delta_r;                    % Noisy source TDOAs (range differences).
        
        delta_r_tilde = chol_Q_c'*randn(N*(M-1),1);
        r_tilde = r_tildeo + delta_r_tilde;  % Noisy calibration TDOAs (range differences).
        
        delta_c = chol_Q_e'*randn(N*D,1);
        c = co + delta_c;                    % Noisy calibration positions.
        
        u = TDOALocSenPosErrMultiCalEmtrIaccLoc(s,r,r_tilde,c,Q_alpha,Q_c,Q_beta,Q_e);
        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(-20:5:60,10*log10(mse),'ks','MarkerSize',8); box on; hold on;
plot(-20:5:60,10*log10(crlb),'k', 'LineWidth', 1);
xlabel('\sigma_e^2/\sigma_s^2(dB)');   ylabel('10*log(Position MSE(m^2))');
legend('Simulation MSE, proposed solution','CRLB',2);
grid on; axis([-20,60,25,50]);                                
hold off;
