% This program generates the results for the source location estimate under LER scenario.
% The figure generated from this code corresponds to Fig. 2 in the reference paper.  
%
% 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.
L = 2000;                    % Number of ensemble runs.
M = 7;                       % Number of sensors

% true source DOA
azi_0 = 9;
ele_0= 53;

azi_1 = [37, 153, 109, 158, -68, 62, -27]; % azimuth angle
ele_1 = [-76, -8, -46, 21, 17, -17, 10];
x = 100*cos(ele_1/180*pi).*cos(azi_1/180*pi);
y = 100*cos(ele_1/180*pi).*sin(azi_1/180*pi);
z = 100*sin(ele_1/180*pi);
S = [x; y; z];   % sensors' locations

NsePwrVecdB = -70;

R_tdoa = (eye(M-1)+ones(M-1))/2;             % covariance structure of TDOA
nsePwr = 10^(NsePwrVecdB/10);
Q = nsePwr * R_tdoa;  
W = inv(Q);      % weighting matrix to be used in the algorithm
     
rto = [5,10,20,30,40,50,100];
fprintf('Simulation in progress ');
for j = 1:length(rto)
    fprintf('.');
    
    uo = [100/rto(j)*cos(ele_0/180*pi)*cos(azi_0/180*pi);
        100/rto(j)*cos(ele_0/180*pi)*sin(azi_0/180*pi);
        100/rto(j)*sin(ele_0/180*pi)];
    
    ro = sqrt(sum((uo*ones(1, M)-S).^2))';   % True source-sensor ranges
    rdo = ro(2:end) - ro(1);        % True range differences (TDOA)
    
    R = sqrt(sum(S.^2))'; % sensor ranges
    Rd = R(2:end) - R(1);   % sensor range differences
    h1_0 = rdo - Rd;
    
    G1(:,1) = (2*(x(1)-x(2:M))+(x(1)/R(1)+x(2:M)./R(2:M)').*rdo')./(R(1)+R(2:M))';
    G1(:,2) = (2*(y(1)-y(2:M))+(y(1)/R(1)+y(2:M)./R(2:M)').*rdo')./(R(1)+R(2:M))';
    G1(:,3) = (2*(z(1)-z(2:M))+(z(1)/R(1)+z(2:M)./R(2:M)').*rdo')./(R(1)+R(2:M))';
    
    bias = inv(G1'*W*G1)*G1'*W*h1_0 - uo;
    cov(j) = trace(inv(G1'*W*G1));
    bias2(j) = bias'*bias;
    theory_mse(j) = cov(j)+bias2(j);
    crlb(j) = trace(TDOALocCRLB(S,uo,Q));
    
    randn('seed',0);
    SimulationMSE = 0;
    SimulationMSE1 = 0;
    
    for k = 1: L      
        rdNse = sqrt(nsePwr/2) * randn(M,1);
        rd = rdo + rdNse(2:end) - rdNse(1);     % measurements
        [phi1,phi] = TDOALoc_LER(S,zeros(3,1),rd,Q,1);   % obtain proposed solution
        SimulationMSE = SimulationMSE + norm(phi-uo,2)^2;
        SimulationMSE1 = SimulationMSE1 + norm(phi1-uo,2)^2;
    end
    mse(j) = SimulationMSE/L;     % MSE of preliminary solution
    mse1(j) = SimulationMSE1/L;   % MSE of biased reduced solution
end
fprintf('\n');

figure(2); 
p1 = semilogy(rto,(theory_mse),'--'); hold on;
p2 = semilogy(rto,(mse),'+'); hold on;
p3 = semilogy(rto,(mse1),'*');hold on;
p4 = semilogy(rto,(crlb));hold off;
set(p1,{'Color'},{'black'})
set(p2,{'Color'},{'black'})
set(p3,{'Color'},{'black'})
set(p4,{'Color'},{'black'})

legend('Theoretical MSE','Proposed Solution without Bias Reduction','Proposed Solution with Bias Reduction','CRLB');
xlabel('Range ratio'); ylabel('MSE'); grid on;
