% This program gives an example on how to call the following routine:
% TDOAFDOALocMultiDisjMvgSrcSenLocErrSH.m
%
% This program will reproduce the Fig. 2 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-05-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;
L = 5e3;                              % Number of ensemble runs.            
randn('seed',0);                      % Initialize random number generator.

uo = [314  483  209]';                % True source positions for reproducing the Fig. 2.
u_doto = [-20 15 40]';                % True source velocities for reproducing the Fig. 2.

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

xd = [30 -30 10 10 -20 20];
yd = [-20 10 -20 20 10 -10];
zd = [20 20 10 30 10 10];
s_doto = [xd; yd; zd];                % True sensor velocity matrix.

M = size(so,2);                       % Number of sensors.
N = size(so,1);                       % Dimension of localization problem.
K = length(uo)/N;                     % Number of sources.

ro = zeros(K*(M-1),1);
r_doto = zeros(K*(M-1),1);
for k = 1 : K
    ro((M-1)*(k-1)+1:(M-1)*k) = sqrt(sum((repmat(uo(N*(k-1)+1:N*k),1,M-1)-so(:,2:M)).^2,1))';
    temp =(repmat(uo(N*(k-1)+1:N*k),1,M-1)-so(:,2:M))'*(repmat(u_doto(N*(k-1)+1:N*k),1,M-1)-s_doto(:,2:M));
    r_doto((M-1)*(k-1)+1:(M-1)*k) = diag(temp)./ro((M-1)*(k-1)+1:(M-1)*k);    
    ro((M-1)*(k-1)+1:(M-1)*k) = ro((M-1)*(k-1)+1:(M-1)*k) - norm(uo(N*(k-1)+1:N*k)-so(:,1));           % True RDs.      
    r_doto((M-1)*(k-1)+1:(M-1)*k) = r_doto((M-1)*(k-1)+1:(M-1)*k) - (uo(N*(k-1)+1:N*k)-so(:,1))'*...
        (u_doto(N*(k-1)+1:N*k)-s_doto(:,1))/norm(uo(N*(k-1)+1:N*k)-so(:,1)); % True FDOAs.
end

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

R = (eye(M-1)+ones(M-1))/2;
Q_alpha = [];
for k = 1 : K
    Q_temp = blkdiag(1e-4*R,1e-6*R);  
    Q_alpha = blkdiag(Q_alpha,Q_temp);   % Covariance matrix of measurement noise.     
end

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

for sigma2_s = -30 : 3 : 0               % Vary the sensor location noise power. 
    fprintf('.');
    Q_beta = J*10^(sigma2_s/10);         % Covariance matrix of sensor location noise.
    
    CRLB = TDOAFDOALocMultiDisjMvgSrcSenLocErrCRLB(so,s_doto,uo,u_doto,Q_beta,Q_alpha);
    temp = reshape(diag(CRLB),2*N,K);
    for k = 1 : K
        crlbu(i,k) = sum(temp(1:N,k));        % CRLB of the source position
        crlbudot(i,k) = sum(temp(N+1:2*N,k)); % CRLB of the source velocity
    end
    SimulationMSEu = zeros(1,K); 
    SimulationMSEudot = zeros(1,K);
    SimulationMSEuL = zeros(1,K); 
    SimulationMSEuLdot = zeros(1,K); 
    
    chol_Q_beta = chol(Q_beta);
    chol_Q_alpha = chol(Q_alpha);
    for l = 1 : L                                           % Monte Carlo simulation.        
        Delta_s = chol_Q_beta'*randn(2*M*N,1);
        s = so + reshape(Delta_s(1:N*M),N,M);               % Noisy sensor positions.
        s_dot = s_doto + reshape(Delta_s(N*M+1:end),N,M);   % Noisy sensor velocities.
        
        Delta_r = chol_Q_alpha'*randn(2*K*(M-1),1);
        r = ro + Delta_r(1:K*(M-1));                        % Noisy TDOAs.
        r_dot = r_doto + Delta_r(K*(M-1)+1:end);            % Noisy FDOAs.
       
        [u,u_dot] = TDOAFDOALocMultiDisjMvgSrcSenLocErrSH(s,s_dot,r,r_dot,Q_beta,Q_alpha);
        [uL,uL_dot] = TDOAFDOALocMvgSenPosErr(s,s_dot,r,r_dot,Q_beta,Q_alpha);
        for k = 1 : K
            SimulationMSEu(k) = SimulationMSEu(k) + norm(u(:,k)-uo(:,k))^2;
            SimulationMSEudot(k) = SimulationMSEudot(k) + norm(u_dot(:,k)-u_doto(:,k))^2;
            SimulationMSEuL(k) = SimulationMSEuL(k) + norm(uL(:,k)-uo(:,k))^2;
            SimulationMSEuLdot(k) = SimulationMSEuLdot(k) + norm(uL_dot(:,k)-u_doto(:,k))^2;
        end;
    end;
    mseu(i,:) = SimulationMSEu/L;
    mseudot(i,:) = SimulationMSEudot/L;
    mseuL(i,:) = SimulationMSEuL/L;
    mseuLdot(i,:) = SimulationMSEuLdot/L;
    i = i + 1;                                              % Update program counter.    
end;
fprintf('\n');

% Plot the configuration.
figure(100);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);
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');

% Plot the result.
x_tt = [-30:3:0];
figure(10); subplot(211);set(gca,'FontSize',14); 
plot(x_tt,10*log10(mseu(:,1)),'kx','MarkerSize',10,'LineWidth',2); grid on; hold on;
plot(x_tt,10*log10(mseuL(:,1)),'ksquare','MarkerSize',10,'LineWidth',2);  hold on;
plot(x_tt,10*log10(crlbu(:,1)),'k','LineWidth',2); hold on;
xlabel('10log(\sigma_s^2)'); ylabel('10log(position MSE)');
legend('Proposed method','Solution from [8]','CRLB','Location','NorthWest');
xlim([-30 -1]);ylim([-10 20]);
hold off;

subplot(212);set(gca,'FontSize',14); 
plot(x_tt,10*log10(mseudot(:,1)),'kx','MarkerSize',10,'LineWidth',2); grid on; hold on;
plot(x_tt,10*log10(mseuLdot(:,1)),'ksquare','MarkerSize',10,'LineWidth',2); hold on;
plot(x_tt,10*log10(crlbudot(:,1)),'k','LineWidth',2); hold on;
xlabel('10log(\sigma_s^2)'); ylabel('10log(velocity MSE)');
legend('Proposed method','Solution from [8]','CRLB','Location','NorthWest');
xlim([-30 -1]);ylim([-10 30]);
hold off;      

