% This program gives an example on how to call the following routine:
% TDOALocBiasRed.m
%
% This program will reproduce the Figs. 1 and 2 in K. C. Ho, 
% "Bias reduction for an explicit solution of source localization using TDOA,"
% IEEE Trans. on Signal Process., vol. 60, pp. 2101-2114, May 2012.
%
%
% Yue Wang and K. C. Ho    07-01-2012
%
%       Copyright (C) 2011
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA.
%       hod@missouri.edu
%

clear; warning off;     % program initialization
L = 10000;              % number of ensemble runs

x = [0, 10*cos(2*pi/8*(0:7))];
y = [0, 10*sin(2*pi/8*(0:7))];
S = [x; y];               % sensor positions
uo = [100*cos(2*pi/8*0.25), 100*sin(2*pi/8*0.25)]';   % source position

M = size(S,2);                          % number of sensors
N = size(S,1);                          % dimension of localization

randn('state',17);                      % initialize random number generator

aveNse=0;
for k=1:L 
    aveNse=aveNse+randn(M,1); 
end
aveNse=aveNse/L/sqrt(2);
PP=aveNse(2:end)-aveNse(1);             % mean of measurement noise to be subtracted out
                                        % to ensure zero mean noise

ro = sqrt(sum((uo*ones(1,M)-S).^2))';   % true source-sensor ranges
rdo = ro(2:end)-ro(1);                  % true range difference

R = (eye(M-1)+ones(M-1))/2;             % covariance structure of range difference rd

SNRdB=-30:5:10; SNR=10.^(SNRdB/10);
NsePwrVecdB=10*log10(1./(SNR*(8*pi*pi*16e18))*(3e8)^2);

fprintf('Simulation in progress');

for nseIdx=1:length(NsePwrVecdB),       % vary measurement noise level
    fprintf('.');
    nsePwr = 10^(NsePwrVecdB(nseIdx)/10);
    Q = nsePwr * R;                     % covariance matrix of rd noise
    crlb(nseIdx) = trace(TDOALocCRLB(S,uo,Q));
    
    [biasThy] = BiasTDOALoc(S,ro,Q,uo); % theoretical bias of the solution from [11]
    biasTH(nseIdx) = norm(biasThy,2);
    
    MSE = 0;
    MSES = 0;
    MSER = 0;
    Bias = zeros(N,1);
    BiasS = zeros(N,1);
    BiasR = zeros(N,1);
    randn('state',17);
    for k = 1 : L,                      % Monte Carlo Simulation
        tmp=randn(M,1);
        rdNse = sqrt(nsePwr) * ((tmp(2:M)-tmp(1))/sqrt(2)-PP(1:M-1));
        rd = rdo + rdNse;
        
        u = TDOALoc(S,rd,Q);            % solution from Chan&Ho1994, ref. [11]
        uS=TDOALoc_BiasSub(S, rd, Q);   % solution from BiasSub method
        uR =TDOALoc_BiasRed(S, rd, Q);  % solution from BiasRed method
        MSE = MSE + norm(u-uo,2)^2;
        MSES = MSES + norm(uS-uo,2)^2;
        MSER = MSER + norm(uR-uo,2)^2;
        Bias = Bias + (u-uo);
        BiasS = BiasS + (uS-uo);
        BiasR = BiasR + (uR-uo);
    end;
    mse(nseIdx) = MSE/L;
    mseS(nseIdx) = MSES/L;
    mseR(nseIdx) = MSER/L;
    bias(nseIdx) = norm(Bias/L);
    biasS(nseIdx) = norm(BiasS/L);
    biasR(nseIdx) = norm(BiasR/L);
    
end;
fprintf('\n');

% ----- Plot the results -----
figure(1); set(gca,'FontSize',12); 
plot(SNRdB,10*log10(mseS),'^k','LineWidth',2,'MarkerSize',8);hold on;
plot(SNRdB,10*log10(mseR),'xk','LineWidth',2,'MarkerSize',8);hold on
plot(SNRdB,10*log10(crlb),'k','LineWidth',2,'MarkerSize',8); hold on;
grid on; hold off;

xlabel('SNR(dB)'); 
ylabel('10 log(MSE)');
legend('BiasSub','BiasRed','CRLB');
ylim([-15 35]);

figure(2); set(gca,'FontSize',12); 
plot(SNRdB,20*log10(bias),'ok','LineWidth',2,'MarkerSize',8); hold on;
plot(SNRdB,20*log10(biasTH),'k','LineWidth',2,'MarkerSize',8); hold on;
plot(SNRdB,20*log10(biasS),'^k','LineWidth',2,'MarkerSize',8);hold on;
plot(SNRdB,20*log10(biasR),'xk','LineWidth',2,'MarkerSize',8);hold on
grid on; hold off;

xlabel('SNR(dB)'); 
ylabel('20 log(bias)');
legend('WMLS','Theory','BiasSub','BiasRed');
ylim([-100 40]);

