warning off;

clear all; % Program initialization.

clc;
close all;
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;
% [x, error] = tdoa(time_estimate, AP, Node, Tag)
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)=tractdoaLocSenPosErrMultiCalEmtrIaccLocCRLB(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(crl),'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;