% This program will reproduce Figs. 2-5 in Y. Zhang and K. C. Ho,
% "Multistatic localization in partially dynamic scenario with only sensor
% positions available," IEEE Trans. Aerosp. Electron. Syst., vol. 57, no. 5,
% pp. 3416-3432, Oct. 2021.
%
% To generate different figure, change 'figNum'.
%
% *The code may not produce the exact same figure in the paper due to
% matlab version and random seed.
%
% The program can be used for 2D(K=2) or 3D(K=3) localization.
%
% Yang Zhang and K. C. Ho   10-28-2021
%
%       Copyright (C) 2021
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA.
%       hod@missouri.edu
%

clear;
warning('off');

% ========== choice of figure ==========
figNum=6;
% ======================================

switch figNum,
    case 6,
        Gmty=1952;
        VelMag=10;                 % transmitter speed
    case 7,
        Gmty=1:100;
        VelMag=10;                 % transmitter speed
    case 8,
        Gmty=882;
        VelMag=20;                 % transmitter speed
    case 9,
        Gmty=1:100;
        VelMag=20;                 % transmitter speed
end;

rand('state',131);

% ----- simulation settings -----
K=3;                       % dimension
vto=zeros(K,1);            % transmitter velocity sets to zero for the MOST case
M=6;                       % number of receivers
L=2000;                    % number of ensemble runs
offsetdo=10;               % range offset
offsetfo=2;                % range rate offset
NsePwrAllLg=[-30:5:0];     % noise power in log scale, i.e. 20log10(nsePwr)
k=0.01;                    % coefficient of the range rate noise power
NoG=2000;                  % number of random geometries

% ------------------------- generate random geometries ---------------- %
for dd=1:NoG
    
    uu(:,dd)=100*rand(K,1);
    tt(:,dd)=100*rand(K,1);
    while(norm(uu(:,dd)-tt(:,dd))<20)
        tt(:,dd)=100*rand(K,1);
    end
    
    ss(:,1,dd)=100*rand(K,1);
    for ii=2:M
        ss(:,ii,dd)=100*rand(K,1);
        while (norm(ss(:,ii,dd)-tt(:,dd))<20 || norm(ss(:,ii,dd)-uu(:,dd))<20)
            ss(:,ii,dd)=100*rand(K,1);
        end
    end
    
    e_ang=2*pi*rand(1)-pi;
    b_ang=2*pi*rand(1)-pi;
    if (K==3) vvu(3,dd)=VelMag*sin(e_ang); end;
    VelMag_xy=VelMag*cos(e_ang);
    vvu(1:2,dd)=VelMag_xy*[cos(b_ang);sin(b_ang)];
    
end

%  replace geometry #23(poor geometry) with #200
uu(:,23)=uu(:,200);
tt(:,23)=tt(:,200);
ss(:,:,23)=ss(:,:,200);
vvu(:,23)=vvu(:,200);

% record 
MseuTSE=zeros(length(NsePwrAllLg),Gmty(end));
MseuTSES=zeros(length(NsePwrAllLg),Gmty(end));
MseuMLES=zeros(length(NsePwrAllLg),Gmty(end));
MseuNoTX=zeros(length(NsePwrAllLg),Gmty(end));
MsevuTSE=zeros(length(NsePwrAllLg),Gmty(end));
MsevuTSES=zeros(length(NsePwrAllLg),Gmty(end));
MsevuMLES=zeros(length(NsePwrAllLg),Gmty(end));
MsevuNoTX=zeros(length(NsePwrAllLg),Gmty(end));        
       
% ----- loop over geometries -----
for i=Gmty,
    
    if (figNum==7 | figNum==9) fprintf('Geometry: %d ',i); end;
    
    to=tt(:,i);         % transmitter position of the ith geometry
    s=ss(:,:,i);        % receiver positions of the ith geometry
    uo=uu(:,i);         % object position of the ith geometry
    vuo=vvu(:,i);       % transmitter velocity of the ith geometry
    
    do=sqrt(sum((repmat(to,1,M)-s(:,1:M)).^2))'+offsetdo;               % true direct path range
    ro=sqrt(sum((repmat(uo,1,M)-s(:,1:M)).^2))'+norm(uo-to)+offsetdo;	% true indirect path range
    roNoTX=sqrt(sum((repmat(uo,1,M)-s(:,1:M)).^2))'+offsetdo;           % true object-receiver range for IMLE-NoTx where object is the source
    
    do_of=do-offsetdo;
    ro_of=ro-offsetdo;
    roNoTX_of=roNoTX-offsetdo;
    m_2=1/(2*M)*sum(do_of.^2+ro_of.^2);
    mNoTX_2=1/(2*M)*sum(roNoTX_of.^2);
    
    fdo=(repmat(to,1,M)-s(:,1:M))'*vto./sqrt(sum((repmat(to,1,M)-s(:,1:M)).^2))'+offsetfo;                             % true direct path range rate
    fro=(repmat(uo,1,M)-s(:,1:M))'*vuo./sqrt(sum((repmat(uo,1,M)-s(:,1:M)).^2))'+(uo-to)'*(vuo-vto)/norm(uo-to)+offsetfo;    % true indirect path range rate
    froNoTX=(repmat(uo,1,M)-s(:,1:M))'*vuo./sqrt(sum((repmat(uo,1,M)-s(:,1:M)).^2))'+offsetfo;
    
    for nsePwrIdx=1:length(NsePwrAllLg)
        
        randn('state',319);
         
        % ----- generate noise covariance matrices -----
        nsePwr=10^(NsePwrAllLg(nsePwrIdx)/10);
        sigmar_2=nsePwr*(ro_of.^2/m_2);
        sigmad_2=nsePwr*(do_of.^2/m_2);
        sigmaNoTX_2=nsePwr*(roNoTX_of.^2/mNoTX_2);
        Qr=diag(sigmar_2)/2;
        Qd=diag(sigmad_2)/2;
        QNoTX=diag(sigmaNoTX_2)/2;
        
        % ----- correlated noise situation -----
        %     for i=1:M-1
        %         for j=i+1:M
        %             Qr(i,j)=sqrt(sigmar_2(i))*sqrt(sigmar_2(j))*(norm(u-t)^2)/((norm(u-t)+norm(u-s(:,i)))*(norm(u-t)+norm(u-s(:,j))));
        %             Qd(i,j)=sqrt(sigmad_2(i))*sqrt(sigmad_2(j))*0.1;
        %         end
        %     end
        
        Qr=(Qr+Qr');
        Qd=(Qd+Qd');
        QNoTX=(QNoTX+QNoTX');
        
        Qfr=k*Qr;
        Qfd=k*Qd;
        
        % ----- obtain CRLBs -----
        [CRLBuvMM,CRLBuvIMM]=MSLocMOMTCRLB(uo,to,vuo,vto,s,Qr,Qd,Qfr,Qfd);  % CRLB for MOMT case
        [CRLBuvID]=MSLocMOSTCRLB(uo,to,vuo,s,Qr,Qd,Qfr,Qfd);    % CRLB for MOST case
        
        CRLBuMOMT(nsePwrIdx,i)=trace(CRLBuvMM(1:K,1:K));
        CRLBvuMOMT(nsePwrIdx,i)=trace(CRLBuvMM(K+1:2*K,K+1:2*K));
        CRLBuMOST(nsePwrIdx,i)=trace(CRLBuvID(1:K,1:K));
        CRLBvuMOST(nsePwrIdx,i)=trace(CRLBuvID(K+1:2*K,K+1:2*K));
        CRLBuIMOST(nsePwrIdx,i)=trace(CRLBuvIMM(1:K,1:K));
        CRLBvuIMOST(nsePwrIdx,i)=trace(CRLBuvIMM(K+1:2*K,K+1:2*K));
        
        % ----- initialize record -----
        SimuMseuTSE=0;
        SimuMseuTSES=0;
        SimuMseuMLES=0;
        SimuMseuNoTX=0;
        SimuMsevuTSE=0;
        SimuMsevuTSES=0;
        SimuMsevuMLES=0;
        SimuMsevuNoTX=0;
        
        % ----- ensemble runs -----
        for ii=1:L
            
            if (~mod(ii,200)) fprintf('.'); end;
            
            noiser=sqrtm(Qr)*randn(M,1);
            noised=sqrtm(Qd)*randn(M,1);
            noiserNoTX=sqrtm(Qr)*randn(M,1);
            noisefr=sqrtm(Qfr)*randn(M,1);
            noisefd=sqrtm(Qfd)*randn(M,1);
            noisefrNoTX=sqrtm(Qfr)*randn(M,1);
            
            % ----- simulate measurements -----
            r=ro+noiser;                     % noisy indirect path range measurement
            rNoTX=roNoTX+noiserNoTX;         % noisy object-receiver range measurement
            d=do+noised;                     % noisy direct path range measurement
            fr=fro+noisefr;                  % noisy indirect path range rate measurement
            fd=fdo+noisefd;                  % noisy direct path range rate measurement
            frNoTX=froNoTX+noisefrNoTX;      % noisy object-receiver range rate measurement
            
            % ----- obtain location estimate -----
            [gamma_hat_MOMT]=MSLocMOMT_CFS(r,d,fr,fd,s,Qr,Qd,Qfr,Qfd);              % closed-form solution using MOMT
            [gamma_hat_MOST]=MSLocMOST_CFS(r,d,fr,fd,s,Qr,Qd,Qfr,Qfd);              % closed-form solution using MOST
            [gamma_MLE_MOST]=MSLocMOST_MLE(r,d,fr,fd,s,Qr,Qd,Qfr,Qfd,gamma_hat_MOST);  % MLE solution using MOST, initialized by CFS
            [gamma_NoTX]=MSLocNoTxMOST_MLE(rNoTX,frNoTX,s,Qr,Qfr,[uo;vuo;offsetdo;offsetfo]);   % IMLE-NoTx
            
            % ----- square errors -----
            SimuMseuTSE=SimuMseuTSE+norm((gamma_hat_MOMT(1:K)-uo)).^2;
            SimuMseuTSES=SimuMseuTSES+norm((gamma_hat_MOST(1:K)-uo)).^2;
            SimuMseuMLES=SimuMseuMLES+norm((gamma_MLE_MOST(1:K)-uo),2).^2;
            SimuMseuNoTX=SimuMseuNoTX+norm((gamma_NoTX(1:K)-uo),2).^2;
            
            SimuMsevuTSE=SimuMsevuTSE+norm((gamma_hat_MOMT(K+1:2*K)-vuo)).^2;
            SimuMsevuTSES=SimuMsevuTSES+norm((gamma_hat_MOST(1*K+1:2*K)-vuo)).^2;
            SimuMsevuMLES=SimuMsevuMLES+norm((gamma_MLE_MOST(K+1:2*K)-vuo),2).^2;
            SimuMsevuNoTX=SimuMsevuNoTX+norm((gamma_NoTX(K+1:2*K)-vuo),2).^2;
            
        end
        
        % ----- average over ensembles -----
        MseuTSE(nsePwrIdx,i)=SimuMseuTSE/L;
        MseuTSES(nsePwrIdx,i)=SimuMseuTSES/L;
        MseuMLES(nsePwrIdx,i)=SimuMseuMLES/L;
        MseuNoTX(nsePwrIdx,i)=SimuMseuNoTX/L;
        
        MsevuTSE(nsePwrIdx,i)=SimuMsevuTSE/L;
        MsevuTSES(nsePwrIdx,i)=SimuMsevuTSES/L;
        MsevuMLES(nsePwrIdx,i)=SimuMsevuMLES/L;
        MsevuNoTX(nsePwrIdx,i)=SimuMsevuNoTX/L;
        
    end
    
    fprintf('\n');
end

% ----- average over geometries -----
CRLB_uMOMT=mean(CRLBuMOMT,2);
CRLB_vuMOMT=mean(CRLBvuMOMT,2);
CRLB_uMOST=mean(CRLBuMOST,2);
CRLB_vuMOST=mean(CRLBvuMOST,2);
CRLB_uMOST_Ind=mean(CRLBuIMOST,2);
CRLB_vuMOST_Ind=mean(CRLBvuIMOST,2);
MseuTSE_uM=mean(MseuTSE,2);
MseTSES_uM=mean(MseuTSES,2);
MseMLES_uM=mean(MseuMLES,2);
MseNoTX_uM=mean(MseuNoTX,2);
MseuTSE_vuM=mean(MsevuTSE,2);
MseTSES_vuM=mean(MsevuTSES,2);
MseMLES_vuM=mean(MsevuMLES,2);
MseNoTX_vuM=mean(MsevuNoTX,2);

% ----- plot results -----
if (figNum==6 | figNum==8)
    figure;
    subplot(2,1,1);
    plot(NsePwrAllLg,10*log10(CRLBuIMOST(:,i)),'--r');
    hold on;
    plot(NsePwrAllLg,10*log10(CRLBuMOMT(:,i)),':r');
    hold  on;
    plot(NsePwrAllLg,10*log10(CRLBuMOST(:,i)),'-r');
    hold  on;
    plot(NsePwrAllLg,10*log10(MseuTSE(:,i)),'xm','MarkerSize',8);
    hold on;
    plot(NsePwrAllLg,10*log10(MseuTSES(:,i)),'db','MarkerSize',8);
    hold on;
    plot(NsePwrAllLg,10*log10(MseuMLES(:,i)),'ob','MarkerSize',8);
    hold on;
    plot(NsePwrAllLg,10*log10(MseuNoTX(:,i)),'+b','MarkerSize',8);
    hold off;
    grid on; xlabel('10 log10(\sigma^2(m^2))'); ylabel('10 log10(MSE(m^2))');
    lgd=legend('$$\rm{Tr(CRLB}_{\bf{ms,I}}(\bf{u}^o)$$','$$\rm{Tr(CRLB}_{\bf{mm}}(\bf{u}^o)$$','$$\rm{Tr(CRLB}_{\bf{ms}}(\bf{u}^o)$$','MOMT: Solution from [31]','MOST: Proposed Solution','MOST: IMLE','IMLE-NoTx','location','northwest');
    set(lgd, 'Interpreter', 'Latex');
    if (figNum==6) ylim([-15 40]); else ylim([-30 20]); end;
    
    subplot(2,1,2);
    plot(NsePwrAllLg,10*log10(CRLBvuIMOST(:,i)),'--r');
    hold on;
    plot(NsePwrAllLg,10*log10(CRLBvuMOMT(:,i)),':r');
    hold  on;
    plot(NsePwrAllLg,10*log10(CRLBvuMOST(:,i)),'-r');
    hold  on;
    plot(NsePwrAllLg,10*log10(MsevuTSE(:,i)),'xm','MarkerSize',8);
    hold on;
    plot(NsePwrAllLg,10*log10(MsevuTSES(:,i)),'db','MarkerSize',8);
    hold on;
    plot(NsePwrAllLg,10*log10(MsevuMLES(:,i)),'ob','MarkerSize',8);
    hold on;
    plot(NsePwrAllLg,10*log10(MsevuNoTX(:,i)),'+b','MarkerSize',8);
    hold off;
    grid on; xlabel('10 log10(\sigma^2(m^2))'); ylabel('10 log10(MSE((m/s)^2))');
    lgd=legend('$$\rm{Tr(CRLB}_{\bf{ms,I}}(\bf{\dot{u}}^o)$$','$$\rm{Tr(CRLB}_{\bf{mm}}(\bf{\dot{u}}^o)$$','$$\rm{Tr(CRLB}_{\bf{ms}}(\bf{\dot{u}}^o)$$','MOMT: Solution from [31]','MOST: Proposed Solution','MOST: IMLE','IMLE-NoTx','location','northwest');
    set(lgd, 'Interpreter', 'Latex');
    if (figNum==6) ylim([-30 25]); else ylim([-30 20]); end;
end;

if (figNum==7 | figNum==9)
    figure;
    subplot(2,1,1);
    plot(NsePwrAllLg,10*log10(CRLB_uMOST_Ind),'--r');
    hold on;
    plot(NsePwrAllLg,10*log10(CRLB_uMOMT),':r');
    hold  on;
    plot(NsePwrAllLg,10*log10(CRLB_uMOST),'-r');
    hold  on;
    plot(NsePwrAllLg,10*log10(MseuTSE_uM),'xm','MarkerSize',8);
    hold on;
    plot(NsePwrAllLg,10*log10(MseTSES_uM),'db','MarkerSize',8);
    hold on;
    plot(NsePwrAllLg,10*log10(MseMLES_uM),'ob','MarkerSize',8);
    hold on;
    plot(NsePwrAllLg,10*log10(MseNoTX_uM),'+b','MarkerSize',8);
    hold off;
    grid on; xlabel('10 log10(\sigma^2(m^2))'); ylabel('10 log10(MSE(m^2))');
    lgd=legend('$$\rm{Tr(CRLB}_{\bf{ms,I}}(\bf{u}^o)$$','$$\rm{Tr(CRLB}_{\bf{mm}}(\bf{u}^o)$$','$$\rm{Tr(CRLB}_{\bf{ms}}(\bf{u}^o)$$','MOMT: Solution from [31]','MOST: Proposed Solution','MOST: IMLE','IMLE-NoTx','location','northwest');
    set(lgd, 'Interpreter', 'Latex');
    if (figNum==7) ylim([-30 40]); else ylim([-30 30]); end;
    
    subplot(2,1,2);
    plot(NsePwrAllLg,10*log10(CRLB_vuMOST_Ind),'--r');
    hold on;
    plot(NsePwrAllLg,10*log10(CRLB_vuMOMT),':r');
    hold  on;
    plot(NsePwrAllLg,10*log10(CRLB_vuMOST),'-r');
    hold  on;
    plot(NsePwrAllLg,10*log10(MseuTSE_vuM),'xm','MarkerSize',8);
    hold on;
    plot(NsePwrAllLg,10*log10(MseTSES_vuM),'db','MarkerSize',8);
    hold on;
    plot(NsePwrAllLg,10*log10(MseMLES_vuM),'ob','MarkerSize',8);
    hold on;
    plot(NsePwrAllLg,10*log10(MseNoTX_vuM),'+b','MarkerSize',8);
    hold off;
    grid on; xlabel('10 log10(\sigma^2(m^2))'); ylabel('10 log10(MSE((m/s)^2))');
    lgd=legend('$$\rm{Tr(CRLB}_{\bf{ms,I}}(\bf{\dot{u}}^o)$$','$$\rm{Tr(CRLB}_{\bf{mm}}(\bf{\dot{u}}^o)$$','$$\rm{Tr(CRLB}_{\bf{ms}}(\bf{\dot{u}}^o)$$','MOMT: Solution from [31]','MOST: Proposed Solution','MOST: IMLE','IMLE-NoTx','location','northwest');
    set(lgd, 'Interpreter', 'Latex');
    ylim([-40 20]);
end;



