% This program will reproduce the results in Fig. 4, 5, 8 or 9 of the reference
% paper below for time delay only or with Doppler shift localization when 
% the motion from the sensors is not negligible.
%
% Reference:
% T. Jia, K. C. Ho, H. Wang, and X. Shen, "Effect of sensor motion on time
% delay and Doppler shift localization: analysis and solution," IEEE Trans.
% Signal Process., vol. 67, no. 22, pp. 5881-5895, Nov. 2019.
%
% Settings for lines 38-44:
% For Fig 4 (2D, TimeDelayOnly):            Dim=2; TmDlyOnly=1;
% For Fig 5 (2D, TimeDelay&DopplerShift):   Dim=2; TmDlyOnly=0;
% For Fig 8 (3D, TimeDelayOnly):            Dim=3; TmDlyOnly=1;
% For Fig 9 (3D, TimeDelay&DopplerShift):   Dim=3; TmDlyOnly=0;
%
% Note: Only 100 ensemble runs is used to avoid long running time (caused by
% the SDR solution).  To change number of ensemble runs, set NumEnsem on
% line 47.
%
% Note: To disable running SDR solution, in line 36, set RunSDRSol=0;
%
% Tianyi Jia and K. C. Ho   12-26-2019
%
%       Copyright (C) 2019
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA
%       hod@missouri.edu
%

clear all;
clc;
warning off;

%% -- run SDR solution
RunSDRSol=1;   % disable(0) or enable(1) running SDR solution to reduce running time

%% localization dimension
Dim=2;  % for 2D
% Dim=3;  % for 3D

%% mesaurements
TmDlyOnly=1;    % Using both time delays only
% TmDlyOnly=0;    % Using both time delays and Doppler Shifts

%% number of ensemble runs
NumEnsem=100;   % number of ensemble runs

%% -- simulation setting
c=1500;         % signal propagation speed

rand('state',1);
if (Dim==2) % 2D
    
    %% object location
    uo=[800 800]';          % unknown object
    %% 2D scenario: 6 sensors
    tx=[500 -300 -1000  -300  300 600];
    ty=[500  300 -1000  1000 -100 -1000];
    t=[tx;ty];         % location of sensor when signal sending out
    %% carrier frequencies
    fc=[8000 9000 8500 8800 9200 9600];    % carrier frequencies
    %% randomly generate the direction of velocity
    speed=15;               % sensor speed
    randseta=rand(1,length(t))*2*pi;
    vx=cos(randseta)*speed;
    vy=sin(randseta)*speed;
    v=[vx;vy];         % sensor velocity vector
    
else    % 3D
    
    %% object location
    uo = [600 650 550]';
    %% 3D scenario: 9 sensors
    tx = [400,300,-100,300,200,300,350,-100,350];
    ty = [150,500,-100,300,-300,100,200,400,100];
    tz = [100,200,-100,-100,-200,150,100,200,-150];
    t = [tx; ty; tz];                       % True sensor position matrix.
    %% carrier frqeuencies
    fc=[9000,8500,9200,9500,9600,8000,8800,9800,8200]; % carrier frequencies
    %% randomly generate the direction of velocity
    speed=15;
    vx=[-0.239321784507465,0.725234863493524,0.119682524922499,-0.510238795250058,-0.542112665674080,0.365525797120493,-0.693533403786447,0.393393741989973,0.516685209201206];
    vy=[-0.805993088358634,-0.216258543931738,-0.186657133529452,-0.0602381513150884,0.203607135126809,-0.111741471868543,-0.491722966590465,0.0552565199038513,-0.851301472618280];
    vz=[0.541387315125016,0.653656358456073,0.975107792877661,0.857920588952095,0.815265596134902,-0.924069659227095,-0.526516801213274,-0.917708058573772,-0.0912260780185962];
    v = [vx; vy; vz]*speed;                 % True sensor position and velocity.
    
end;

N=length(t);           % number of sensors

%%  noise power range
nsePwrAlldB=[-40:10:60];
nsePwrAll=10.^(nsePwrAlldB/10);

rng('default');

%% additive Gaussian noise
NoiseM=randn(2*N,NumEnsem);
NoiseM=NoiseM-repmat(mean(NoiseM,2),1,NumEnsem);

scale=100;          % scale value for SDR solution

%% Create true time delay and frequency measurements
mo=zeros(2*N,1);   % true range measurements, here denoted by time
uomt=repmat(uo,1,N)-t;         % uo-t
L=zeros(2,N);      % L(1,i)=norm(uo-ti) and L(2,i)=norm(uo-si)
s=zeros(Dim,N);      % location of sensor with signal being received
uoms=zeros(Dim,N);

for i=1:N,
    L(1,i)=norm(uomt(:,i));    % L(1,i)=norm(u0-ti)
    A=c^2-v(:,i)'*v(:,i);
    B=2*(L(1,i)*c-v(:,i)'*uomt(:,i));
    mo(i)=B/A;
    s(:,i)=t(:,i)+mo(i)*v(:,i);
    uoms(:,i)=uo-s(:,i);       % u0-s
    L(2,i)=norm(uoms(:,i));    % L(2,i)=norm(u0-si)
    mo(N+i)=fc(i)/(c+v(:,i)'*uoms(:,i)/L(2,i))*(-uomt(:,i)'*v(:,i)/L(1,i)-uoms(:,i)'*v(:,i)/L(2,i));  % m0(N+1:2N)= Doppler
end

%%-- For every noise power level, running NumEnsem trials of the algorithms

R=zeros(4,length(nsePwrAll));    % saving the MSE results, R(1,:) is CRLB

for i=1:length(nsePwrAll),
    nsePwr=nsePwrAll(i);
    fprintf('%2d/%d:  10log10(nsePwr) = %2d \n',i,length(nsePwrAlldB),nsePwrAlldB(i));
    Qt=nsePwr*eye(N)/(c)^2;      % treansform the time to range
    Qf=1e2*nsePwr*eye(N)/(c)^2;
    Q=zeros(2*N,2*N);
    Q(1:N,1:N)=Qt;
    Q(N+1:2*N,N+1:2*N)=Qf;
    Q0=Q/nsePwr*(c^2);
    Qt0=eye(N);
    Lm=chol(Q,'lower');          % Q=Lm*Lm'
    
    %% Theoretical performance, Qf=0 for using time dleays only
    R(1,i) = TmDpLocSenMtnEftCRLB(uo,t,v,Qt,Qf*(1-TmDlyOnly),fc);
    
    for j=1:NumEnsem,
        
        m=mo+Lm*NoiseM(:,j);  % creat a noise vector whose covariance matrix is Q
        
        if (TmDlyOnly)  % using time delays only
            
            %% GN MLE for approximate model
            uk0=uo+2*sqrt(R(1,i))*randn(Dim,1); % initial guess for MLE
            ut_gn = TmLocIgSenMtnEft_GN(m(1:N),t,Qt,c,uk0,1e-6);
            %% 2-stage closed-form solution for accurate model
            [ut_2s,u_init] = TmLocSenMtnEft(m(1:N),t,v,Qt0,c);
            if (RunSDRSol)
                %% SDR solution for accurate model
                [ut_sdr,flag] = TmLocSenMtnEft_SDR(m(1:N),t/scale,v/scale,Qt0,c/scale,u_init/scale);
                ut_sdr=ut_sdr*scale;
            end;
            
        else    % using time delays and Doppler shifts
            
            %% GN MLE for approximate model
            uk0=uo+2*sqrt(R(1,i))*randn(Dim,1); % initial guess for MLE
            ut_gn = TmDpLocIgSenMtnEft_GN(m,t,v,Q,fc,c,uk0,1e-6);
            %% 2-stage closed-form solution for accurate model
            [ut_2s,u_init] = TmDpLocSenMtnEft(m,t,v,Q0,fc,c);
            if (RunSDRSol)
                %% SDR solution for accurate model
                [ut_sdr,flag] = TmDpLocSenMtnEft_SDR(m,t/scale,v/scale,Q0,fc,c/scale,u_init/scale);
                ut_sdr=ut_sdr*scale;
            end;
            
        end;
        
        R(2,i)=R(2,i)+norm(ut_gn-uo)^2/NumEnsem;
        R(3,i)=R(3,i)+norm(ut_2s-uo)^2/NumEnsem;
        if (RunSDRSol)
            R(4,i)=R(4,i)+norm(ut_sdr-uo)^2/NumEnsem;
        end;
        
    end
    
end

%% plot results
figure(1);
if (RunSDRSol)
    p=loglog(sqrt(nsePwrAll),(R(1,:)),'r-',sqrt(nsePwrAll),(R(2,:)),'r+',sqrt(nsePwrAll),(R(3,:)),'bs',sqrt(nsePwrAll),(R(4,:)),'b^');
    legend('CRLB','Solution when ignoring sensor motion effect','Proposed closed-form solution','Proposed SDR solution','location','southeast');
else
    p=loglog(sqrt(nsePwrAll),(R(1,:)),'r-',sqrt(nsePwrAll),(R(2,:)),'r+',sqrt(nsePwrAll),(R(3,:)),'bs');
    legend('CRLB','Solution when ignoring sensor motion effect','Proposed closed-form solution','location','southeast');
end;
xlabel('\sigma (m)');
ylabel(' MSE (m^2)');
set(p,'LineWidth',1);
grid on;


