% Localization of transmitters and scatterers using single receiver.
%
% The code can reproduce the results in Figs. 8-11 of the reference paper by setting FigNum.
%
% Number of ensemble runs (L) is set to 20 to reduce run time of the code.
%
% Reference: Y. Zhang and K. C. Ho, "Localization of transmitters and scatterers by single 
% receiver", IEEE Trans. Signal Process., vol. 71, pp. 2267-2282, 2023.
%
% Yang Zhang, K. C. Ho     06-28-2023
%
%       Copyright (C) 2023
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA.
%       hod@missouri.edu
%

clear all;
warning('off');
rand('state',131);

FigNum=8;       % 8, 9, 10, 11

% ========================================
% ===== Top plot: MSEvsNseTDOA=1; ========
% ===== Bottom plot: MSEvsNseTDOA=0; =====
% ========================================
MSEvsNseTDOA=1;
MSEvsNseAOA=1-MSEvsNseTDOA;

K=3;    % dimension
%L=2000; % number of enssemble runs
L=20;   % number of enssemble runs
PlotGeometry=1;

switch FigNum,
    case 8,
        Mbar=2; Nbar=2; M=2; N=2;
        txKn = [-1778 1569; 77 -1471; -1609 1435];;
        sxKn = [401 1615; 699 -101; -1896 -1312];
        txUnkn = [-1516 729; 1481 -871; -1875 -594];
        sxUnkn =[-1119 -1246; 370 -1400; -585 147];
    case 9,
        Mbar=3; Nbar=0; M=2; N=2;
        txKn = [401 1615 -1119; 699 -101 370; -1896 -1312 -585];
        sxKn = [];
        txUnkn = [-1246 -1919; -1400 -1650; 147 1900];
        sxUnkn =[1118 -1469; -783 -141; -1414 1414];
    case 10,
        Mbar=0; Nbar=3; M=2; N=2;
        txKn = [];
        sxKn = [-40 877 448; -1663 -1999 170; 67 -1145 467];
        txUnkn = [-628 -1372; -1355 -1629; 1404 1500];
        sxUnkn =[-660 910; -1826 -273;  818 -326];
    case 11,
        Mbar=0; Nbar=0; M=2; N=3;
        txKn = [];
        sxKn = [];
        txUnkn = [-1551 1872; 1858 -1529; -140 1694];
        sxUnkn =[-37 1361 -1778; -1759 964 77; 1095 -37 -1609];
end;
% if (~isempty(txKn)) txKn=txKn(1:K,:); end;
% if (~isempty(sxKn)) sxKn=sxKn(1:K,:); end;
% txUnkn=txUnkn(1:K,:);
% sxUnkn=sxUnkn(1:K,:);
tx = [txKn, txUnkn];
sx = [sxKn, sxUnkn];

if (MSEvsNseTDOA)
    nsePwrAllLg=[10:5:50];      % range difference noise powers in log-scale
else
    nsePwrAllLg=[-20:5:20];     % AOA noise powers (deg^2) in log-scale
end;

nsePwrRD_Fixed=100;      % fixed range difference noise power
nsePwrAOA_Fixed=0.1^2;   % fixed AOA noise power in deg^2

% -- plot localization geometry --
if (PlotGeometry)
    figure(1); clf;
    if (Mbar>0)
        plot3(tx(1,1:Mbar),tx(2,1:Mbar),tx(3,1:Mbar),'xk','MarkerSize',8); hold on;
    end;
    plot3(tx(1,Mbar+1:end),tx(2,Mbar+1:end),tx(3,Mbar+1:end),'xr','MarkerSize',8); hold on;
    if (Nbar>0)
        plot3(sx(1,1:Nbar),sx(2,1:Nbar),sx(3,1:Nbar),'ok','MarkerSize',8); hold on;
    end;
    plot3(sx(1,Nbar+1:end),sx(2,Nbar+1:end),sx(3,Nbar+1:end),'or','MarkerSize',8); hold on;
    plot3(0,0,0,'sk','MarkerSize',8); hold off;
    xlabel('x (m)');ylabel('y (m)');zlabel('z (m)');
    grid on;
    if (Mbar>0 & Nbar>0)
        lgd=legend('Intentional Transmitters','Unknown Transmitters','Intentional Scatterers','Unknown Scatterers','Sensor','location','best');
    elseif (Mbar>0)
        lgd=legend('Intentional Transmitters','Unknown Transmitters','Unknown Scatterers','Sensor','location','best');
    elseif (Nbar>0)
        lgd=legend('Unknown Transmitters','Intentional Scatterers','Unknown Scatterers','Sensor','location','best');
    else
        lgd=legend('Unknown Transmitters','Unknown Scatterers','Sensor','location','best');
    end;
end;

% -- true solution --
omegaTrue=[reshape(tx(:,Mbar+1:end),[],1);reshape(sx(:,Nbar+1:end),[],1)];

% -- TDOA ds from known transmitter and unknown scatterer --
for j=1:N
    for i=1:Mbar
        dsTrue(i,j)=norm(tx(:,i)-sx(:,Nbar+j))+norm(sx(:,Nbar+j))-norm(tx(:,i));
    end;
    angleSxTrue(1,j)=atan2(sx(2,Nbar+j),sx(1,Nbar+j));     % scatterer azimuth angles
    if K==3
        angleSxTrue(2,j)=asin(sx(3,Nbar+j)/norm(sx(:,Nbar+j)));     % scatterer elevation angles
    end
end

% -- TDOA dt from unknown transmitter and known scatterer --
for i=1:M
    for j=1:Nbar
        dtTrue(i,j)=norm(tx(:,Mbar+i)-sx(:,j))+norm(sx(:,j))-norm(tx(:,Mbar+i));
    end
    angleTxTrue(1,i)=atan2(tx(2,Mbar+i),tx(1,Mbar+i));     % transmitter azimuth angles
    if K==3
        angleTxTrue(2,i)=asin(tx(3,Mbar+i)/norm(tx(:,Mbar+i)));  % transmitter elevation angles
    end
end

% -- TDOA dts from unknown transmitter and unknown scatterer --
for i=1:M
    for j=1:N
        dtsTrue(i,j)=norm(tx(:,Mbar+i)-sx(:,Nbar+j))+norm(sx(:,Nbar+j))-norm(tx(:,Mbar+i));
    end
end

% -- loop through TDOA noise power --
for nsePwrIdx=1:length(nsePwrAllLg)

    % %         randn('state',319);
    rng('default');
    nsePwr=10^(nsePwrAllLg(nsePwrIdx)/10);
    fprintf('20log(NoisePower): %d\n',nsePwrAllLg(nsePwrIdx));

    if (MSEvsNseTDOA)
        nsePwrRD=nsePwr;
        nsePwrAOA=nsePwrAOA_Fixed;
    else
        nsePwrRD=nsePwrRD_Fixed;
        nsePwrAOA=nsePwr;
    end;

    % TDOA noise covariance matrix
    sigma_IP=sqrt(nsePwrRD);   % indirect path noise standard deviation
    sigma_DP=sqrt(nsePwrRD);   % direct path noise standard deviation
    sigma_TDOA=sqrt(sigma_IP^2+sigma_DP^2);   % TDOA noise standard deviation

    Q1=eye(Mbar*N)*(sigma_IP^2);   % noise covariance for measurtxent d_1

    Q2sub=eye(Nbar)*(sigma_TDOA^2-sigma_DP^2)+ones(Nbar)*sigma_DP^2;
    Q2=[];                        % noise covariance for measurtxent d_2
    for i=1:M
        Q2=blkdiag(Q2,Q2sub);
    end

    Q3sub=eye(N)*(sigma_TDOA^2-sigma_DP^2)+ones(N)*sigma_DP^2;
    Q3=[];                        % noise covariance for measurtxent d_3
    for i=1:M
        Q3=blkdiag(Q3,Q3sub);
    end

    Q23sub=ones(Nbar,N)*sigma_DP^2;
    Q23=[];                       % noise covariance between measurtxent d_2 and d_3
    for i=1:M
        Q23=blkdiag(Q23,Q23sub);
    end

    if (Mbar~=0 & Nbar~=0)
        Q_TDOA=[Q1,zeros(Mbar*N,M*Nbar),zeros(Mbar*N,M*N);  % noise covariance between measurtxent d_1, d_2 and d_3
            zeros(M*Nbar,Mbar*N),Q2,Q23;
            zeros(M*N,Mbar*N),Q23',Q3];
    elseif (Mbar~=0 & Nbar==0)
        Q_TDOA=blkdiag(Q1,Q3);
    elseif (Mbar==0 & Nbar~=0)
        Q_TDOA=[Q2,Q23;
            Q23',Q3];
    else
        Q_TDOA=Q3;
    end;

    % AOA covariance matrices
    sigma_txAOA=(sqrt(nsePwrAOA)/180)*pi;    % transmitter angle noise standard deviation
    sigma_sxAOA=(sqrt(nsePwrAOA)/180)*pi;    % scatterer angle noise standard deviation
    Q_txAOA=sigma_txAOA^2*eye(M*(K-1));  % transmitter angle noise covariance
    Q_sxAOA=sigma_sxAOA^2*eye(N*(K-1));  % scatterer angle noise covariance

    % measurement covariance matrix
    Q=blkdiag(Q_TDOA,Q_txAOA,Q_sxAOA);     % noise covariance for TDOAs and AOAs

    % TDOA and AOA covariance matrices for individual solutions
    Q_TDOAsubknwn=eye(2)*[sigma_IP^2,0;0,sigma_TDOA^2];
    Q_TDOAsub1=eye(2)*(sigma_TDOA^2-sigma_DP^2)+ones(2)*sigma_DP^2;
    Q_TDOAsub=blkdiag(Q_TDOAsub1,Q_TDOAsub1);
    Q_txAOAsubknwn=sigma_txAOA^2*eye(1*(K-1));  % transmitter angle noise covariance for individual solution 1 & 2
    Q_sxAOAsubknwn=sigma_sxAOA^2*eye(1*(K-1));  % scatterer angle noise covariance for individual solution 1 & 2
    Q_txAOAsub=sigma_txAOA^2*eye(2*(K-1));      % transmitter angle noise covariance for individual solution 3
    Q_sxAOAsub=sigma_sxAOA^2*eye(2*(K-1));      % scatterer angle noise covariance for individual solution 3
    QsubknwnTx=blkdiag(Q_TDOAsubknwn,Q_txAOAsubknwn,Q_sxAOAsubknwn); % noise covariance for individual solution 1
    QsubknwnSx=blkdiag(Q_TDOAsub1,Q_txAOAsubknwn,Q_sxAOAsubknwn);    % noise covariance for individual solution 2
    Qsub=blkdiag(Q_TDOAsub,Q_txAOAsub,Q_sxAOAsub);     % noise covariance for individual solution 3

    % CRLB
    crlb=CRLB(txKn,tx,sxKn,sx,Q_TDOA,Q_txAOA,Q_sxAOA);
    crlbTx(nsePwrIdx)=trace(crlb(1:M*K,1:M*K));

    SimuMSE_MLE=0;
    SimuMSE_CF=0;
    SimuMSE_MMBLUEDef=0;
    SimuMSE_MMBLUESel=0;

    %  ensemble runs at a given TDOA noise power
    for ii=1:L,

        % generating measurements
        noise_d=sqrtm(Q_TDOA)*randn(Mbar*N+M*(Nbar+N),1);
        if (Mbar~=0)
            ds=reshape(dsTrue',[],1)+noise_d(1:Mbar*N);
            ds=reshape(ds,N,Mbar)';   % noisy TDOA ds
        else
            txKn=[]; ds=[];
        end;
        if (Nbar~=0)
            dt=reshape(dtTrue',[],1)+noise_d(Mbar*N+1:Mbar*N+M*Nbar);
            dt=reshape(dt,Nbar,M)';   % noisy TDOA dt
        else
            sxKn=[]; dt=[];
        end;
        dts=reshape(dtsTrue',[],1)+noise_d(Mbar*N+M*Nbar+1:end);
        dts=reshape(dts,N,M)';   % noisy TDOA dts
        noise_txAOA=sigma_txAOA*randn(M,K-1);
        noise_sxAOA=sigma_sxAOA*randn(N,K-1);

        angleTx=angleTxTrue'+noise_txAOA;   % noisy transmitter angles
        angleTx=angleTx';
        angleSx=angleSxTrue'+noise_sxAOA;   % noisy scatterer angles
        angleSx=angleSx';

        % MLE by Gauss-Newton iteration itialized at true value
        omega_MLE=GN_MLE(txKn,sxKn,ds,dt,dts,angleTx,angleSx,Q,omegaTrue);      % MLE

        % proposed solution (omegaDef: without selection, omegaSel: with selection)
        [omegaDef,omegaSel]=MMBLUE(txKn,sxKn,Mbar,M,Nbar,N,ds,dt,dts,angleTx,angleSx,Q,QsubknwnTx,QsubknwnSx,Qsub);  % BLUE

        t_estMLE=omega_MLE(1:M*K);
        t_estDef=omegaDef(1:M*K);
        t_estSel=omegaSel(1:M*K);

        SimuMSE_MLE=SimuMSE_MLE+norm(t_estMLE-omegaTrue(1:M*K))^2;
        %             SimuMSE_CF=SimuMSE_CF+norm(t_estCF-omegaTrue(1:M*K))^2;
        SimuMSE_MMBLUEDef=SimuMSE_MMBLUEDef+norm(t_estDef-omegaTrue(1:M*K))^2;
        SimuMSE_MMBLUESel=SimuMSE_MMBLUESel+norm(t_estSel-omegaTrue(1:M*K))^2;
    end

    MSE_MLE(nsePwrIdx)=SimuMSE_MLE/L;
    MSE_MMBLUEDef(nsePwrIdx)=SimuMSE_MMBLUEDef/L;
    MSE_MMBLUESel(nsePwrIdx)=SimuMSE_MMBLUESel/L;

end

% -- results --
figure(FigNum);
if (MSEvsNseTDOA)
    subplot(211);
else
    subplot(212);
end;
plot(nsePwrAllLg,10*log10(crlbTx),'-r');
hold on;
plot(nsePwrAllLg,10*log10(MSE_MLE),'xk','MarkerSize',8);
hold on;
plot(nsePwrAllLg,10*log10(MSE_MMBLUEDef),'sb','MarkerSize',8);
hold on;
plot(nsePwrAllLg,10*log10(MSE_MMBLUESel),'dm','MarkerSize',8);
hold off;
if (MSEvsNseTDOA)
    ylim([20 70]);
    if (Mbar~=0 & Nbar~=0)
        xlim([10 50]);
    elseif (Mbar~=0 & Nbar==0)
        xlim([10 35]);
    elseif (Mbar==0 & Nbar~=0)
        xlim([10 40]);
    else
        xlim([10 45]);
    end;
    grid on; xlabel('20 log_{10}(\sigma_{d}(m))'); ylabel('10 log_{10}(MSE(m^2))');
    if (FigNum==8 | FigNum==11)
        legend('CRLB','IMLE','ProposedSol(NoIdvSolSel)','ProposedSol(IdvSolSel)','location','southeast');
    else
        legend('CRLB','IMLE','ProposedSol(NoIdvSolSel)','ProposedSol(IdvSolSel)','location','northwest');
    end;
else
    if (Mbar~=0 & Nbar~=0)
        axis([-20 10 30 50]);
    elseif (Mbar~=0 & Nbar==0)
        axis([-20 10 33 70]);
    elseif (Mbar==0 & Nbar~=0)
        axis([-20 10 20 70]);
    else
        axis([-20 15 30 75]);
    end;
    grid on; xlabel('20 log_{10}(\sigma_{\theta}(m))'); ylabel('10 log_{10}(MSE(m^2))');
    legend('CRLB','IMLE','ProposedSol(NoIdvSolSel)','ProposedSol(IdvSolSel)','location','northwest');
end;

% ====================================================





