% This program provides an example on how to call TDOAFDOALocMvgSenPosErr.m 
% and TDOAFDOALocMvgSenPosErrCRLB.m. 
%
% This program will reproduce the Fig. 9 in K. C. Ho, Xiaoning Lu, 
% L. Kovavisaruch, "Source Localization Using TDOA and FDOA Measurements 
% in the Presence of Receiver Location Errors: Analysis and Solution," IEEE
% Trans. on Signal Processing, Vol. 55, pp. 684-696, 2007.
% Please note that the paper has a typo on page 686 near the end of column
% 2 in specifying the sensor position error power.  Rs should be
% Rs=sigma_s^2[1,1,1,2,2,2,10,10,10,40,40,40,20,20,20,3,3,3] and it is
% corrected in this code.
%
%
% Le Yang, K. C. Ho      08-01-2009
%                        09-05-2010, revised
%
%       Copyright (C) 2009
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA.
%       hod@missouri.edu
%

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

uo = [2000 2500 3000]';               % True source position.
u_doto = [-20 15 40]';                % True source velocity.

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.

M = size(so,2);                       % Number of sensors.
N = size(so,1);                       % Dimension of localization problem.

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 position and velocity.

ro = sqrt(sum((repmat(uo,1,M-1)-so(:,2:M)).^2,1))';
r_doto=(repmat(uo,1,M-1)-so(:,2:M))'*(repmat(u_doto,1,M-1)-s_doto(:,2:M));
r_doto = diag(r_doto)./ro;
% True TDOAs (range differences).
ro = ro - norm(uo-so(:,1));                           
% True FDOAs (range rate differences).
r_doto = r_doto - (uo-so(:,1))'*(u_doto-s_doto(:,1))/norm(uo-so(:,1)); 

R = (eye(M-1)+ones(M-1))/2;
J = [1,1,1,2,2,2,10,10,10,40,40,40,20,20,20,3,3,3];
J = diag([J, 0.5*J]);
Q_alpha = 1e-4 * R;                                    % Covariance matrix of measurements.
Q_alpha = [Q_alpha,zeros(M-1); zeros(M-1),1e-5*R];     
i = 1;                                                 % Program counter.
fprintf('Simulation in progress');

for sigma2_s = -60 : 5 : 0                             % Vary the sensor location noise power.
    fprintf('.');
    Q_beta = J*10^(sigma2_s/10);                       % Covariance matrix of receiver locations.
    
    CRLB = TDOAFDOALocMvgSenPosErrCRLB(so,s_doto,uo,u_doto,Q_beta,Q_alpha);
    crlbu(i) = trace(CRLB(1:N,1:N));
    crlbudot(i) = trace(CRLB(N+1:end,N+1:end));
    SimulationMSEu = 0; SimulationMSEudot = 0;
    
    chol_Q_beta = chol(Q_beta);
    chol_Q_alpha = chol(Q_alpha);
    for k = 1 : L                                      % Monte Carlo simulation.
        % Noisy receiver locations and velocities.
        Delta_s = chol_Q_beta'*randn(2*M*N,1);
        s = so + reshape(Delta_s(1:N*M),N,M);   
        s_dot = s_doto + reshape(Delta_s(N*M+1:end),N,M);
        
        % Noisy TDOA and FDOA measurements.
        Delta_r = chol_Q_alpha'*randn(2*(M-1),1);
        r = ro + Delta_r(1:M-1);
        r_dot = r_doto + Delta_r(M:end);
       
        [u,u_dot] = TDOAFDOALocMvgSenPosErr(s,s_dot,r,r_dot,Q_beta,Q_alpha);
        SimulationMSEu = SimulationMSEu + norm(u-uo)^2;
        SimulationMSEudot = SimulationMSEudot + norm(u_dot-u_doto)^2;
    end;
    mseu(i) = SimulationMSEu/L;
    mseudot(i) = SimulationMSEudot/L;
    i = i + 1;                        % Update program counter.    
end;
fprintf('\n');

% Plot the result.
figure(1); subplot(211);
plot(-60:5:0,10*log10(mseu),'kx','MarkerSize',8); grid on; hold on;
plot(-60:5:0,10*log10(crlbu),'k','LineWidth',1); 
xlabel('10log(\sigma_s^2(m^2))'); ylabel('10log(position MSE(m^2))');
legend('Simulation MSE, proposed solution','CRLB',2);
ylim([10 80]);
hold off;

subplot(212);
plot(-60:5:0,10*log10(mseudot),'kx','MarkerSize',8); grid on; hold on;
plot(-60:5:0,10*log10(crlbudot),'k','LineWidth',1); 
xlabel('10log(\sigma_s^2(m^2))'); ylabel('10log(velocity MSE((m/s)^2))');
legend('Simulation MSE, proposed solution','CRLB',2);
ylim([0 120]);
hold off;