% This program gives an example on how to call the following routine:
% TDOALocWghSenPosErrMultiDisjSrc.m 
%
% This program will reproduce the Fig. 4 in Ming Sun and K. C. Ho, 
% "An Asymptotically Efficient Estimator for TDOA and FDOA Positioning of 
% Multiple Disjoint Sources in the Presence of Sensor Location Uncertainties,"
% IEEE Trans. Signal Process., vol. 59, pp. 3434-3440, Jul. 2011.
%
%
% Ming Sun and K. C. Ho      03-11-2011
%
%       Copyright (C) 2011
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA.
%       hod@missouri.edu
%

clear all; warning off;               % Program initialization.
clc;

%% parameter setting
L = 5e3;                              % Number of ensemble runs.            

uo = [314 483 209 600 650 550]';      % True source position matrix.
so(:,1) = [300 100 150]';             % True sensor position matrix.
so(:,2) = [400 150 100]';  
so(:,3) = [300 500 200]';  
so(:,4) = [350 200 100]';  
so(:,5) = [-100 -100 -100]';  
so(:,6) = [200 -300 -200]';  

M = size(so,2);                       % Number of sensors.
N = size(so,1);                       % Dimension of localization.
K = size(reshape(uo,N,[]),2);         % Number of sources.

J = diag([10,10,10,2,2,2,10,10,10,40,40,40,20,20,20,3,3,3]);

R = (eye(M-1)+ones(M-1))/2;
Qm = [];
for k = 1:K
    Qm = blkdiag(Qm,R);
end

for k = 1:K
    ro(:,k) = sqrt(sum((repmat(uo(N*(k-1)+1:N*k),1,M-1)-so(:,2:M)).^2,1))';
    ro(:,k) = ro(:,k) - norm(uo(N*(k-1)+1:N*k)-so(:,1));   % True source RDOAs.
    ro=reshape(ro,[],1);
end

randn('seed',9);                               % Initialize random number generator.
fprintf('TDOA: Simulation in progress');

nseIdx = -30:3:0;       
for i = 1 : length(nseIdx)                     % Vary sensor position noise power.
    fprintf('.');
    Q_alpha = 10^(-4) * Qm;                    % Covariance matrix of source RD noise. 
    Q_beta = J * 10^(nseIdx(i)/10);            % Covariance matrix of sensor position noise.
    
    CRLBu= TDOALocSenPosErrMultiDisjSrcCRLB(so,uo,Q_alpha,Q_beta);
    for k = 1 : K
        crlbu(i,k) = trace(CRLBu((k-1)*N+1:k*N,(k-1)*N+1:k*N));
    end
   
    MSE_Source = zeros(2,K);
    chol_Q_alpha=chol(Q_alpha);
    chol_Q_beta=chol(Q_beta);

    for j = 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(K*(M-1),1);
        r = ro + delta_r;                      % Noisy source RDs
        
        u = TDOALocSenPosErrMultiDisjSrcSH(s,r,Q_beta,Q_alpha);    % proposed new solution
        u_J = TDOALocSenPosErrMultiDisjSrc(s,r,Q_alpha,Q_beta);   % previous solution (ref. [11]) 
        for k = 1 : K
            MSE_Source(1,k) = MSE_Source(1,k) + norm(u(N*(k-1)+1:N*k)-uo(N*(k-1)+1:N*k),2)^2;
            MSE_Source(2,k) = MSE_Source(2,k) + norm(u_J(N*(k-1)+1:N*k)-uo(N*(k-1)+1:N*k),2)^2;
        end
    end;
    
    mse_source(i,:) = MSE_Source(1,:)/L;
    mse_joint_source(i,:) = MSE_Source(2,:)/L;
end;
fprintf('\n');

% Plot the configuration.
figure(1);set(gca,'FontSize',14); 
plot3(so(1,:),so(2,:),so(3,:),'kx','MarkerSize',10,'LineWidth',2);
hold on;
plot3(uo(1),uo(2),uo(3),'ks','MarkerSize',10,'LineWidth',2);
hold on;
plot3(uo(4),uo(5),uo(6),'ko','MarkerSize',10,'LineWidth',2);
grid on;
hold off;
xlabel('x');ylabel('y');zlabel('z');
xlim([-500,1000]);ylim([-500,1000]);zlim([-500,1000]);
legend('Sensors','Source 1','Source 2','Location','NorthWest');

figure(2);
set(gca,'FontSize',14); 
plot(nseIdx,10*log10(mse_source(:,1)),'kx','LineWidth',2,'MarkerSize',10); hold on;
plot(nseIdx,10*log10(mse_joint_source(:,1)),'ks','LineWidth',2,'MarkerSize',10); hold on;
plot(nseIdx,10*log10(crlbu(:,1)),'k','LineWidth',2); hold on;
plot(nseIdx,10*log10(mse_source(:,2)),'kx','LineWidth',2,'MarkerSize',10); hold on;
plot(nseIdx,10*log10(mse_joint_source(:,2)),'ks','LineWidth',2,'MarkerSize',10); hold on;
plot(nseIdx,10*log10(crlbu(:,2)),'k','LineWidth',2); hold on;xlabel('10log(\sigma_s^2)'); grid on; box on;
ylabel('10log(position MSE)');ylim([-20,40]);
legend('Proposed algorithm', 'Solution from [11]','CRLB',2);       
hold off;

