% This program generates the results for the source location estimate in
% MEO geolocation scenario
% The figure generated from this code corresponds to Fig. 6 in the reference paper.  
%
% Note: there may be a slight difference with Fig. 6 in the paper due to
% the difference in random seed initialization.
%
% Reference: S. Li and K. C. Ho, “Accurate and effective localization of an 
% object in large equal radius scenario,” IEEE Trans. Wireless Commun., 
% vol. 15, no. 12, pp. 8273-8285, Dec. 2016.
%
% Sasa Li, K. C. Ho    12-10-2016
%
%       Copyright (C) 2016
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA.
%       hod@missouri.edu
%

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

L = 1000;        % Number of ensemble runs.
M = 5;           % Number of sensors

R_toa = diag(ones(M,1)); % covariance matrix structure for TOA
NsePwrVecdB = -100:10:-20;
NsePwr = 10.^(NsePwrVecdB./10);
rand('seed',3);

fprintf('Simulation in progress ');        

for GeoN = 1:100,   % 100 geometries
    if (~mod(GeoN,10)) fprintf('.'); end;
        rr = 6378;
        RR = 20200+rr;
        source_a_angle = (-180 + rand*360)*pi/180;
        source_e_angle = (-45 + rand*90)*pi/180;
        sensor_a_angle = (source_a_angle-15 + rand([1, M])*30)*pi/180; % azimuth angle
        sensor_e_angle = (source_e_angle-15 + rand([1, M])*30)*pi/180; % elevation angle
    
    uo = [rr*cos(source_e_angle)*cos(source_a_angle);
        rr*cos(source_e_angle)*sin(source_a_angle);
        rr*sin(source_e_angle)]; % true source location
    
    x = RR.*cos(sensor_e_angle).*cos(sensor_a_angle);
    y = RR.*cos(sensor_e_angle).*sin(sensor_a_angle);
    z = RR.*sin(sensor_e_angle);
    S0=[x; y; z];   % sensor positions
    N = size(S0,1);
    
    ro = sqrt(sum((uo*ones(1, M) - S0).^2))';   % True source-sensor ranges
    rdo = ro(2:end) - ro(1); % TDOA
    
    R_tdoa = Cov_TDOA(ro, M);   % TDOA covariance matrix

    for nseIdx = 1: length(NsePwrVecdB),    % vary measurement noise level
        nsePwr = NsePwr(nseIdx);
        Q_r = nsePwr * R_tdoa; % Covariance matrix of TDOA (range difference) noise
        crlb(GeoN, nseIdx) = trace(TDOALocCRLB(S0,uo,Q_r));
        
        randn('seed',1);
        
        SimulationMSE1 = 0; % mse from the new method
        SimulationMSE2 = 0; % mse from Chan-Ho method

        for k = 1: L        % Monte Carlo Simulation.
            chol_Q_r = chol(Q_r);
            delta_r = chol_Q_r'*randn(M-1,1);
            
            rd = rdo + delta_r;
            rdNse = sqrt(nsePwr/2) * randn(M,1);
            r_toa = ro + rdNse;
           
            center = findCenter(S0);
            [u1,uTmp] = TDOALoc_LER(S0,center,rd,Q_r,2);   % proposed solution
            u2 = TDOALoc(S0,rd,Q_r);    
            
            SimulationMSE1 = SimulationMSE1 + norm(u1-uo,2)^2;
            SimulationMSE2 = SimulationMSE2 + norm(u2-uo,2)^2;
        end
        
        mse1(GeoN,nseIdx) = SimulationMSE1/L;   % Chan
        mse2(GeoN,nseIdx) = SimulationMSE2/L;   % new
    end
end

fprintf('\n');

new_mse = mean(mse1);
chan_mse = mean(mse2);
avg_crlb = mean(crlb);

figure(1);
p1 = loglog(NsePwr,(new_mse),'*'); hold on;
p2 =loglog(NsePwr,(chan_mse),'^'); hold on;
p3 =loglog(NsePwr,(avg_crlb)); hold off;grid on;
set(p1,{'Color'},{'black'})
set(p2,{'Color'},{'black'})
set(p3,{'Color'},{'black'})
legend('Proposed Solution','Chan-Ho','CRLB')
xlabel('\sigma_{r}^2 (km^2)'); ylabel('MSE (km^2)'); grid on;
ylim([10^(-7), 1000]);