% 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=2;
% ======================================

switch figNum,
    case 2,
        Gmty=64;
        VelMag=10;                 % transmitter speed
    case 3,
        Gmty=1:100;
        VelMag=10;                 % transmitter speed
    case 4,
        Gmty=68;
        VelMag=20;                 % transmitter speed
    case 5,
        Gmty=1:100;
        VelMag=20;                 % transmitter speed
end;

rand('state',131);

% ----- simulation settings -----
K=3;                       % dimension
vuo=zeros(K,1);            % object velocity sets to zero for the SOMT case
M=6;                       % number of receivers
L=2000;                    % number of ensemble runs
offsetdo=10;               % range offset
offsetfo=2;                % range rate offset
NsePwrAllLg=[-30:5:10];    % noise power in log scale, i.e. 20log10(nsePwr)
k=0.01;                    % coefficient of the range rate noise power
NoG=200;                   % 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
    
    for ii=1: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
    
    b_ang=2*pi*rand(1)-pi;
    e_ang=2*pi*rand(1)-pi;
    if (K==3) vvt(3,dd)=VelMag*sin(e_ang); end;
    VelMag_xy=VelMag*cos(e_ang);
    vvt(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);
vvt(:,23)=vvt(:,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));
       
% ----- loop over geometries -----
for i=Gmty,
    
    if (figNum==3 | figNum==5) 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
    vto=vvt(:,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
    
    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 -----
        [CRLBuMM,~,~]=MSLocMOMTCRLB(uo,to,vuo,vto,s,Qr,Qd,Qfr,Qfd);             % CRLB for MOMT case
        [CRLBuID,CRLBuI,CRLBuNoOfst]=MSLocSOMTCRLB(uo,to,vto,s,Qr,Qd,Qfr,Qfd);  % CRLB for SOMT case
        [CRLBuSS,~,~]=MSLocSOSTCRLB(uo,to,s,Qr,Qd);                             % CRLB for SOST case
        CRLBuMOMT(nsePwrIdx,i)=trace(CRLBuMM(1:K,1:K));
        CRLBuSOMT(nsePwrIdx,i)=trace(CRLBuID(1:K,1:K));
        CRLBuNoOfstSOMT(nsePwrIdx,i)=trace(CRLBuNoOfst(1:K,1:K));
        CRLBuISOMT(nsePwrIdx,i)=trace(CRLBuI(1:K,1:K));
        CRLBuSOST(nsePwrIdx,i)=trace(CRLBuSS(1:K,1:K));
        
        % ----- initialize record -----
        SimuMseuTSE=0;
        SimuMseuTSES=0;
        SimuMseuMLES=0;
        SimuMseuNoTX=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);
            
            % ----- 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
            
            % ----- obtain location estimate -----
            [gamma_MOMT]=MSLocMOMT_CFS(r,d,fr,fd,s,Qr,Qd,Qfr,Qfd);              % closed-form solution using MOMT
            [gamma_hat_SOMT]=MSLocSOMT_CFS(r,d,fr,fd,s,Qr,Qd,Qfr,Qfd);          % closed-form solution using SOMT
            [gamma_MLE_SOMT]=MSLocSOMT_MLE(r,d,fr,fd,s,Qr,Qd,Qfr,Qfd,gamma_hat_SOMT);  % MLE solution using SOMT initialized by CFS
            [gamma_NoTX]=MSLocNoTxSOMT_MLE(rNoTX,s,Qr,[uo;offsetdo]);           % IMLE-NoTx
            
            % ----- square errors -----
            SimuMseuTSE=SimuMseuTSE+norm((gamma_MOMT(1:K)-uo)).^2;
            SimuMseuTSES=SimuMseuTSES+norm((gamma_hat_SOMT(1:K)-uo)).^2;
            SimuMseuMLES=SimuMseuMLES+norm((gamma_MLE_SOMT(1:K)-uo),2).^2;
            SimuMseuNoTX=SimuMseuNoTX+norm((gamma_NoTX(1:K)-uo),2).^2;
        end
        
        % ----- average over ensembles -----
        MseuTSE(nsePwrIdx,i)=SimuMseuTSE/L;
        MseuTSES(nsePwrIdx,i)=SimuMseuTSES/L;
        MseuMLE(nsePwrIdx,i)=SimuMseuMLES/L;
        MseuNoTX(nsePwrIdx,i)=SimuMseuNoTX/L;
    end
    
    fprintf('\n');
end

% ----- average over geometries -----
CRLB_SOMT_Ind=mean(CRLBuISOMT,2);
CRLB_MOMT=mean(CRLBuMOMT,2);
CRLB_SOMT=mean(CRLBuSOMT,2);
CRLB_SOMTn=mean(CRLBuNoOfstSOMT,2);
CRLB_SOST=mean(CRLBuSOST,2);
MseuTSES_M=mean(MseuTSES,2);
MseuTSE_M=mean(MseuTSE,2);
MseuMLE_M=mean(MseuMLE,2);
MseuNoTX_M=mean(MseuNoTX,2);

% ----- plot results -----
if (figNum==2 | figNum==4)
    figure;
    plot(NsePwrAllLg,10*log10(CRLBuISOMT(:,i)),'--r');
    hold on;
    plot(NsePwrAllLg,10*log10(CRLBuMOMT(:,i)),':r');
    hold  on;
    plot(NsePwrAllLg,10*log10(CRLBuSOST(:,i)),'-.k');
    hold  on;
    plot(NsePwrAllLg,10*log10(CRLBuNoOfstSOMT(:,i)),'-k');
    hold  on;
    plot(NsePwrAllLg,10*log10(CRLBuSOMT(:,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(MseuMLE(:,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{sm,I}}(\bf{u}^o))$$','$$\rm{Tr(CRLB}_{\bf{mm}}(\bf{u}^o))$$','$$\rm{Tr(CRLB}_{\bf{ss}}(\bf{u}^o))$$','$$\rm{Tr(\overline{CRLB}}_{\bf{sm}}(\bf{u}^o)$$','$$\rm{Tr(CRLB}_{\bf{sm}}(\bf{u}^o))$$',...
        'MOMT: Solution from [31]','SOMT: Proposed Solution','SOMT: IMLE','IMLE-NoTx','location','northwest');
    set(lgd, 'Interpreter', 'Latex');
    ylim([-30 35]);
end;

if (figNum==3 | figNum==5)
    figure;
    plot(NsePwrAllLg,10*log10(CRLB_SOMT_Ind),'--r');
    hold on;
    plot(NsePwrAllLg,10*log10(CRLB_MOMT),':r');
    hold  on;
    plot(NsePwrAllLg,10*log10(CRLB_SOST),'-.k');
    hold  on;
    plot(NsePwrAllLg,10*log10(CRLB_SOMTn),'-k');
    hold  on;
    plot(NsePwrAllLg,10*log10(CRLB_SOMT),'-r');
    hold  on;
    plot(NsePwrAllLg,10*log10(MseuTSE_M),'xm','MarkerSize',8);
    hold on;
    plot(NsePwrAllLg,10*log10(MseuTSES_M),'db','MarkerSize',8);
    hold on;
    plot(NsePwrAllLg,10*log10(MseuMLE_M),'ob','MarkerSize',8);
    hold on;
    plot(NsePwrAllLg,10*log10(MseuNoTX_M),'+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{sm,I}}(\bf{u}^o))$$','$$\rm{Tr(CRLB}_{\bf{mm}}(\bf{u}^o))$$','$$\rm{Tr(CRLB}_{\bf{ss}}(\bf{u}^o))$$','$$\rm{Tr(\overline{CRLB}}_{\bf{sm}}(\bf{u}^o)$$','$$\rm{Tr(CRLB}_{\bf{sm}}(\bf{u}^o))$$',...
        'MOMT: Solution from [31]','SOMT: Proposed Solution','SOMT: IMLE','IMLE-NoTx','location','northwest');
    set(lgd, 'Interpreter', 'Latex');
    xlim([-30 0]);
    if (figNum==3) ylim([-25 20]); else ylim([-30 25]); end;
end;



