% DTD localization using closed-form solution and MLE under multipath
% environment.
%
% This code generates Fig. 10 in the reference paper.
%
% Note: Number of ensemble runs (NumEns) is set to 50 only to reduce 
% run time.
%
% Reference:
% Y. Zhang and K. C. Ho, "Localization by signals of opportunity in the
% absence of transmitter position," IEEE Trans. Signal Process., vol. 72,
% pp. 4602-4617, 2022.
%
% Yang Zhang, K. C. Ho	10-18-2022
%
%       Copyright (C) 2022
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA
%       hod@missouri.edu
%

clear;
warning('off');

K=3;            % dimension

rand('state',0);

NumEns=50;      % ensemble runs
% NumEns=5000;  % ensemble runs

NoG = 10;       % number of geometries
RgnLgth=2000;   % geometry length
PlotGeo=0;
CorNse=0;   % correlated noise

if (K==2),
    M=14;
else
    M=21;
end;

for dd  = 1:NoG
    ss(:,:,dd)=RgnLgth*(rand(K,M)-0.5);
    uu(:,dd)=RgnLgth*(rand(K,1)-0.5);
    tt(:,dd)=RgnLgth*(rand(K,1)-0.5);
    while (norm(uu(:,dd)-tt(:,dd))<RgnLgth/5)
        uu(:,dd)=RgnLgth*(rand(K,1)-0.5);
        tt(:,dd)=RgnLgth*(rand(K,1)-0.5);
    end;
end

% -- loop over multipath factor --
for ig = 1:3
    gamma = 1 - 0.2*(ig-1);

    % -- loop over geometries
    for dd = 1:NoG
        S = ss(:,1:M,dd);
        uo = uu(:,dd);
        to = tt(:,dd);

        % -- plot geometry --
        if (PlotGeo)
            figure(11);
            if (K==2)
                plot(uo(1,1),uo(2,1),'+k'); hold on;
                plot(to(1,1),to(2,1),'^k'); hold on;
                plot(S(1,:),S(2,:),'ok'); hold off;
                xlabel('x (m)');ylabel('y (m)');
                grid on;
            else
                plot3(uo(1,1),uo(2,1),uo(3,1),'+k'); hold on;
                plot3(to(1,1),to(2,1),to(3,1),'^k'); hold on;
                plot3(S(1,:),S(2,:),S(3,:),'ok'); hold off;
                xlabel('x (m)');ylabel('y (m)');
            end;
            grid on;
            lgd=legend('Object','Transmitter','Receiver');
        end;

        % -- true DTD values --
        dID = sqrt(sum((repmat(uo,1,M)-S).^2))'+norm(uo-to);
        dD = sqrt(sum((repmat(to,1,M)-S).^2))';
        do = dID - dD;

        nsePwrAlldB=[-60:5:-20];     % noise power
        if (K==2)
            nsePwrAlldB=[-60:5:-20]-10;     % noise power
        end;

        thetao=[uo;to];               % true object and transmitter positions

        % -- loop over noise powers --
        for nsePwrIdx=1:length(nsePwrAlldB)
            randn('state',0);
            rand('state',0);
            fprintf('Geo:%d, 10log(nsePwr): %d\n',dd,nsePwrAlldB(nsePwrIdx));
            nsePwr=10^(nsePwrAlldB(nsePwrIdx)/10);
            m_2=1/(2*M)*sum(dID.^2+dD.^2);
            sigmar_2=nsePwr*(dID.^2/m_2);
            sigmad_2=nsePwr*(dD.^2/m_2);

            % -- covariance matrices --
            QdID=diag(sigmar_2)/2;
            QdD=diag(sigmad_2)/2;

            % -- for correlated noise --
            if (CorNse)
                CorCoef=0.5;
                for i=1:M-1
                    for j=i+1:M
                        QdID(i,j)=sqrt(sigmar_2(i))*sqrt(sigmar_2(j))*(norm(uo-to)^2)/((norm(uo-to)+norm(uo-S(:,i)))*(norm(uo-to)+norm(uo-S(:,j))));
                        QdD(i,j)=sqrt(sigmad_2(i))*sqrt(sigmad_2(j))*CorCoef;
                    end
                end
            end;

            QdID=(QdID+QdID');
            QdD=(QdD+QdD');
            Q=QdID+QdD;

            % -- CRLB --
            CRLB_d=DTDLoc_CRLB(uo,to,S,Q);
            CRLButGeo(dd,nsePwrIdx)=trace(CRLB_d);
            CRLBuGeo(dd,nsePwrIdx)=trace(CRLB_d(1:K,1:K));
            CRLBtGeo(dd,nsePwrIdx)=trace(CRLB_d(K+1:2*K,K+1:2*K));

            % -- initializing record --
            SimuMseuMLE=0;
            SimuMseuMult=0;
            SimuMseuRefine1=0;
            SimuMseuRefine2=0;
            SimuMseuRfn=0;
            SimuBiasuMult=0;
            SimuBiasuRfn=0;
            SimuBiasuMLE=0;

            % -- loop over ensemble runs --
            for ii=1:NumEns

                noise=sqrtm(Q)*randn(M,1);
                d=do+noise;     % measurements

                % ===== inject multipath component =====
                mu = rand(M,1)*20*sqrt(nsePwr);
                gg = ones(M,1);
                gg((rand(M,1)+gamma-1)>0) = 0;
                rMp = sqrt(nsePwr)*(randn(M,1)+mu).*gg;
                d=d+rMp;
                % ======================================

                % -- proposed algebraic closed-form solution --
                [theta, thetaTilde]=DTDLoc(d,S,Q);

                % -- proposed refinement solution --
                thetaHat=DTDLocRfn(d,S,Q,theta); % refinement method initialized using multistage estmator

                % -- MLE --
                theta_MLE=DTDLoc_MLE(d,S,Q,theta); % MLE

                % -- collecting statistics --
                SimuMseuMult=SimuMseuMult+norm((theta(1:K)-uo),2).^2;
                SimuMseuRfn=SimuMseuRfn+norm((thetaHat(1:K)-uo),2).^2;
                SimuMseuMLE = SimuMseuMLE + norm((theta_MLE(1:K)-uo),2).^2;
                SimuBiasuMult=SimuBiasuMult+(theta(1:K)-uo);
                SimuBiasuRfn=SimuBiasuRfn+(thetaHat(1:K)-uo);
                SimuBiasuMLE = SimuBiasuMLE + (theta_MLE(1:K)-uo);

            end

            MseuMultGeo(dd,nsePwrIdx)=SimuMseuMult/NumEns;
            MseuRfnGeo(dd,nsePwrIdx)=SimuMseuRfn/NumEns;
            MseuMLEGeo(dd,nsePwrIdx)=SimuMseuMLE/NumEns;
            BiasuMultGeo(dd,nsePwrIdx)=sum((SimuBiasuMult/NumEns).^2);
            BiasuRfnGeo(dd,nsePwrIdx)=sum((SimuBiasuRfn/NumEns).^2);
            BiasuMLEGeo(dd,nsePwrIdx)=sum((SimuBiasuMLE/NumEns).^2);

        end

        % -- plot individual geometry results --
        figure(1);
        subplot(2,5,dd);
        plot(nsePwrAlldB,10*log10(CRLBuGeo(dd,:)),'-r');
        hold on;
        plot(nsePwrAlldB,10*log10(MseuMultGeo(dd,:)),'ob','MarkerSize',8);
        hold on
        plot(nsePwrAlldB,10*log10(MseuRfnGeo(dd,:)),'sm','MarkerSize',8);
        hold on
        plot(nsePwrAlldB,10*log10(MseuMLEGeo(dd,:)),'+k','MarkerSize',8);
        hold off;
        grid on; xlabel('10 log_1_0(\sigma^2(m^2))'); ylabel('10 log_1_0(MSE(u)(m^2))');
        % legend('CRLB','Algebraic Solution, No Refinement','Algebraic Solution, With Refinement','MLE by Gauss-Newton','location','northwest');
        title(['Geo. ',num2str(dd)]);
        axis([-60 -20 -60 10]);

    end;

    % -- averaging over geometries --
    CRLBu=mean(CRLBuGeo,1);
    MseuMult=mean(MseuMultGeo,1);
    MseuRfn=mean(MseuRfnGeo,1);
    MseuMLE=mean(MseuMLEGeo,1);
    BiasuMult = mean(BiasuMultGeo,1);
    BiasuRfn = mean(BiasuRfnGeo,1);
    BiasuMLE = mean(BiasuMLEGeo,1);

    % -- plot results --
    figure(2);  % MSE
    plot(nsePwrAlldB,10*log10(CRLBu),'-r');
    hold on;
    plot(nsePwrAlldB,10*log10(MseuMult),'ob','MarkerSize',8);
    hold on
    plot(nsePwrAlldB,10*log10(MseuRfn),'sm','MarkerSize',8);
    hold on
    plot(nsePwrAlldB,10*log10(MseuMLE),'+k','MarkerSize',8);
    hold off;
    grid on; xlabel('10 log_1_0(\sigma^2(m^2))'); ylabel('10 log_1_0(MSE(u)(m^2))');
    legend('CRLB','Algebraic Solution, No Refinement','Algebraic Solution, With Refinement','MLE by Gauss-Newton','location','northwest');
    title(['Average Results from ',num2str(NoG),' Geometries']);

    figure(3);  % Bias
    clf;
    plot(nsePwrAlldB,10*log10(BiasuMult),'ob','MarkerSize',8);
    hold on
    plot(nsePwrAlldB,10*log10(BiasuRfn),'sm','MarkerSize',8);
    hold on
    plot(nsePwrAlldB,10*log10(BiasuMLE),'+k','MarkerSize',8);
    hold off;
    grid on; xlabel('10 log10(nsePwr)'); ylabel('20 log_1_0(Bias(u(m)))');
    legend('Algebraic Solution, No Refinement','Algebraic Solution, with Refinement','MLE by Gauss-Newton','location','northwest');
    if (K==2)
        axis([-70 -30 -90 -10]);
    else
        axis([-60 -20 -70 -10]);
    end;

    % -- save results --
    CRLBuMP(ig,:)=CRLBu;    
    MseuMultMP(ig,:)=MseuMult;
    MseuRfnMP(ig,:)=MseuRfn;
    MseuMLEMP(ig,:)=MseuMLE;
    BiasuMultMP(ig,:)=BiasuMult;
    BiasuRfnMP(ig,:)=BiasuRfn;
    BiasuMLEMP(ig,:)=BiasuMLE;

end;    % ig

% -- plot overall results --
symbs = ['+','d','v','x','s','^'];

xlabtext = '10 log_1_0(\sigma^2(m^2))';
xdata =  nsePwrAlldB;

figure(4);  % MSE
clf;
plot(xdata, 10*log10(CRLBuMP(1,:)), 'r', 'LineWidth', 1.25, ...
    'DisplayName', ['CRLB']);hold on;grid on;
for ig = 1:3
    plot(xdata, 10*log10(MseuMultMP(ig,:)), symbs(ig), 'LineWidth', 1.25, ...
        'DisplayName', ['DTD Solution (No Refinement), \gamma = ',num2str(1 - 0.2*(ig-1))]);hold on;grid on;
    plot(xdata, 10*log10(MseuRfnMP(ig,:)), symbs(ig+3), 'LineWidth', 1.25, ...
        'DisplayName', ['DTD Solution (With Refinement), \gamma = ',num2str(1 - 0.2*(ig-1))]);hold on;grid on;
end
xlabel(xlabtext, 'FontSize', 10);
ylabel('10 log_1_0(MSE(u)(m^2))', 'FontSize', 10);
lgd11 = legend('Show');
set(lgd11, 'FontSize',9, 'Location', 'Northwest');
axis([-60 -20 -60 10]);

figure(5);  % Bias
clf;
for ig = 1:3
    plot(xdata, 20*log10(BiasuMultMP(ig,:)), symbs(ig), 'LineWidth', 1.25, ...
        'DisplayName', ['DTD Solution (No Refinement), \gamma = ',num2str(1 - 0.2*(ig-1))]);hold on;grid on;
    plot(xdata, 20*log10(BiasuRfnMP(ig,:)), symbs(ig+3), 'LineWidth', 1.25, ...
        'DisplayName', ['DTD Solution (With Refinement), \gamma = ',num2str(1 - 0.2*(ig-1))]);hold on;grid on;
end
xlabel(xlabtext, 'FontSize', 10);
ylabel('20 log_1_0(Bias(u)(m))', 'FontSize', 10);
lgd11 = legend('Show');
set(lgd11, 'FontSize',9, 'Location', 'Northwest');
axis([-60 -20 -140 0]);

