% This program reproduces the results in Fig. 3 or 4 of the reference paper
% below for time delay only or with Doppler shift localization when taking
% the motions of the object and the sensors into consideration.
%
% It is an example to call the closed-form solution routines for the two
% localization cases.
%
% Reference:
% T. Jia, K. C. Ho, H. Wang and X. Shen, "Localization of a moving object
% with sensors in motion by time delays and Doppler shifts," IEEE Trans.
% Signal Process., vol. 68, pp. 5824-5841, 2020.
%
% Set TimeDelayLoc=1 for time delay location
% Set TimeDelayLoc=0 for time delay and Doppler shift localization
%
% Set Dim=1 for 3-D localization
% Set Dim=0 for 2-D localization
%
% Tianyi Jia and K. C. Ho   11-08-2020
%
%       Copyright (C) 2020
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA
%       hod@missouri.edu
%

clear all; %clc;

% -- choose between time delay localization and time delay and Doppler --
% -- shift localization --
TimeDelayLoc=1;

%% -- Localization Dimension --
Dim=3;  % Dim=3: 3-D localization, Dim=2: 2-D localization

Nmc=2000;           % number of ensemble runs

% -- Simulation Settings --
%% object position
u0=[500,800,80]';   % object position
%% object velocity
speedObj=5;
rand('state',1);
randseta=rand(1)*2*pi;
rand('state',2);
randfai=rand(1)*pi;
vx=sin(randfai)*cos(randseta)*speedObj;
vy=sin(randfai)*sin(randseta)*speedObj;
vz=cos(randfai)*speedObj;
v0 = [vx; vy; vz];  % object velocity

%% 9 sensor positions in 3-D and carrier frequencies
tx=[350 900 700 250 1400 1000 700  1200 500];
ty=[200 200 700 1000 1200 800 1200 500  900];
tz=[140 60  100 110   80  90  100  0  0];
t=[tx;ty;tz];           % location of sensors
fc=[8 8.5 9 9.5 10 10.5 11 11.5 12]*1e3;   % carrier frequencies
%% for 2-D case 
if (Dim==2)
    u0=u0(1:Dim); 
    v0=v0(1:Dim); v0=v0/norm(v0)*speedObj; 
    t=t(1:Dim,:); 
end

c=1500;             % signal propagation speed
[K,N]=size(t);      % dimension and number of sensors

NseLvlLg=[-40:5:20];  % noise level(deviation) in log-scale
NseLvl=(10.^(NseLvlLg/10));  % signal level (amplitude)
NumNseLvl=length(NseLvl);

NoiseM=randn(2*N,Nmc);  % range(TOA) & rangeRate(Doppler shift) noise
NoiseM=NoiseM-mean(NoiseM,2);   % make noise zero-mean

% -- measurements --
%% Create true measurements
u0mt=u0-t;         % u0-t
L=zeros(1,N);      % L(1,i)=norm(u0-ti) and L(2,i)=norm(u0-si)
m0=zeros(2*N,1);
u0imt=zeros(K,N);
Lu0i=zeros(1,N);
for i=1:N,
    L(1,i)=norm(u0mt(:,i));    % L(1,i)=norm(u0-ti)
    A=v0'*v0-c^2;
    B=4*u0mt(:,i)'*v0;
    C=4*L(1,i)^2;
    m0(i)=(-B-sqrt(B^2-4*A*C))/(2*A);
    u0imt(:,i)=u0mt(:,i)+m0(i)/2*v0;  % u0+dui-ti
    Lu0i(1,i)=norm(u0imt(:,i));
    m0(i+N)=fc(i)*2*u0imt(:,i)'/Lu0i(1,i)*v0/(c-u0imt(:,i)'/Lu0i(1,i)*v0);
end

% -- processing --
%% For each noise level, performing Nmc ensemble runs 
Ru=zeros(4,NumNseLvl);       % Ru(1,:) is CRLB; R(4,:) is RMSE of closed-form solution;
Rv=zeros(4,NumNseLvl);       % Rv(1,:) is CRLB; R(4,:) is RMSE of closed-form solution;

for i=1:NumNseLvl,           % loop through noise levels
    fprintf('.');
    Qt=NseLvl(i)^2*eye(N)/c^2;          % Time delay covariance matrix
    Qf=1e4*NseLvl(i)^2*eye(N)/c^2;      % Doppler shift covariance matrix
    Q=zeros(2*N,2*N);                   % measurement covariance matrix
    Q(1:N,1:N)=Qt;
    Q(N+1:2*N,N+1:2*N)=Qf;
    Q0=Q/NseLvl(i)^2*c^2;
    Lm=chol(Q,'lower');     % Q=Lm*Lm'
    sum_u=zeros(K,2);       % accumulator of position estimate
    sum_v=zeros(K,2);       % accumulator of velocity estimate
    
    %% Theoretical performance for time localization
    if (TimeDelayLoc)
        CRLB=TmDpLocObjMtnEftCRLB(u0,v0,t,Qt,0,fc,c);       % CRLB
    else
        CRLB = TmDpLocObjMtnEftCRLB(u0,v0,t,Qt,Qf,fc,c);    % CRLB
    end;
    Ru(1,i)=sqrt(trace(CRLB(1:K,1:K)));
    Rv(1,i)=sqrt(trace(CRLB(K+1:2*K,K+1:2*K)));
    
    %% loop through number of ensemble runs
    for j=1:Nmc
        m=m0+Lm*NoiseM(:,j);  % creat a noise vector whose covariance matrix is Q
        
        %% 2-step solution for accurate model with Range
        
        if (TimeDelayLoc)
            x = TmLocObjMtnEft_CFS(m(1:N),t,Qt,c);
        else
            x = TmDpLocObjMtnEft_CFS(m,t,Q,fc,c);
        end;
        
        sum_u(:,1)=sum_u(:,1)+x(1:K);
        sum_v(:,1)=sum_v(:,1)+x(K+1:2*K);
        
        Ru(4,i)=Ru(4,i)+norm(x(1:K)-u0)^2/Nmc;
        Rv(4,i)=Rv(4,i)+norm(x(K+1:2*K)-v0)^2/Nmc;
        
    end
    Ru(3,i)=norm(sum_u(:,1)/Nmc-u0);
    Rv(3,i)=norm(sum_v(:,1)/Nmc-v0);
    Ru(4,i)=sqrt(Ru(4,i));
    Rv(4,i)=sqrt(Rv(4,i));
end
fprintf('\n');

% -- plot results --
figure(1);
subplot(2,1,1);     % position
p=loglog(NseLvl,Ru(1,:),'r-',NseLvl,Ru(4,:),'bo');
legend('CRLB','Closed-form solution','location','northwest');
if (TimeDelayLoc)
    axis([NseLvlLg(1) 1e1 1e-4 1e2]);
else
    axis([1e-4 1e2 1e-6 1e2]);
end;
xlabel(' \sigma (m) ');
ylabel(' Position RMSE (m) ');
set(p,'LineWidth',1);
grid on;

subplot(2,1,2);     % velocity
p=loglog(NseLvl,Rv(1,:),'r-',NseLvl,Rv(4,:),'bo');
legend('CRLB','Closed-form solution','location','northwest');
if (TimeDelayLoc)
    axis([1e-4 1e1 1e-4 1e4]);
else
    axis([1e-4 1e2 1e-6 1e2]);
end;
xlabel('\sigma (m) ');
ylabel(' Velocity RMSE (m/s) ');
set(p,'LineWidth',1);
grid on;

