% This program reproduces the results in Fig. 5 or 7 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 Gauss-Newton and Quasi-Newton 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, or 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
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];       % position of sensors with signal sending out
M=size(t,2);        % dimension and number of sensors
%% 9 sensor velocity in 3-D
speed=speedObj;
randseta=rand(1,M)*2*pi;       % randomly generate seta
randfai=rand(1,M)*pi;
vsx=sin(randfai).*cos(randseta).*speed;
vsy=sin(randfai).*sin(randseta).*speed;
vsz=cos(randfai).*speed;
vs=[vsx;vsy;vsz];   % velocity of sensors 
%% 9 sensor carrier frequencies
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,:); 
    vs=vs(1:Dim,:); vs=vs./repmat(sqrt(sum(vs.^2)),Dim,1)*speed; 
end

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

%% set the noise level and generate zero-mean Gaussian noise sequence
NseLvlLg=[-30:5:40];    % noise level(deviation) in log-scale
NseLvl=(10.^(NseLvlLg/10));     % signal level (amplitude)
NumNseLvl=length(NseLvl);
randn('state',1);
NoiseM=randn(2*M,Nmc);  % range(TOA) & range rate(Doppler shift) noise 
% NoiseM=randl(2*M,Nmc);   % laplacian noise
NoiseM=NoiseM-mean(NoiseM,2);   % make noise zero-mean

%% Creat time delay and Doppler shift measurements
m0=zeros(2*M,1);    % true time delays and Doppler shifts
mr0=zeros(2*M,1);   % true ranges and range rates
r01=zeros(1,M);     % r1 is the first part of r
u0mt=u0-t;          % u0-t
L=zeros(4,M);       % L(1,i)=norm(u0-ti) and L(2,i)=norm(u0-si)  L(3,i)=norm(u0i-ti) and L(4,i)=norm(u0i-si)
s=zeros(K,M);       % location of sensors when receiving signals
u0imt=zeros(K,M);
u0ims=zeros(K,M);
u0i=zeros(K,M);
pu0imt=zeros(K,M);
pu0ims=zeros(K,M);
Ai=zeros(1,M);
Bi=zeros(1,M);
Ci=zeros(1,M);
Di=zeros(1,M);
for i=1:M
    L(1,i)=norm(u0mt(:,i));    %  L(1,i)=norm(u0-ti)
    A=1-v0'*v0/c^2;
    B=-2*u0mt(:,i)'*v0/c;
    C=-L(1,i)^2;
    r01(i)=(-B+sqrt(B^2-4*A*C))/(2*A);
    u0i(:,i)=u0+r01(i)/c*v0;
    
    u0imt(:,i)=u0i(:,i)-t(:,i);
    L(3,i)=norm(u0imt(:,i));   % L(3,i)=norm(u0i-ti)
    A=1-vs(:,i)'*vs(:,i)/c^2;
    B=2*(L(3,i)-vs(:,i)'*u0imt(:,i)/c);
    mr0(i)=B/A;                 % r0
    m0(i)=mr0(i)/c;             % time delay
    
    s(:,i)=t(:,i)+m0(i)*vs(:,i);
    u0ims(:,i)=u0i(:,i)-s(:,i); %u0-s
    L(4,i)=norm(u0ims(:,i));    % L(4,i)=norm(u0i-si)
    
    pu0imt(:,i)=u0imt(:,i)/L(3,i);
    pu0ims(:,i)=u0ims(:,i)/L(4,i);
    
    Ai(i)=pu0ims(:,i)'*v0/c;
    Bi(i)=pu0imt(:,i)'*v0/c;
    Ci(i)=pu0ims(:,i)'*vs(:,i)/c;
    Di(i)=pu0imt(:,i)'*vs(:,i)/c;
    
    mr0(i+M)=1/(1+Ci(i))*((1+Ai(i))/(1-Bi(i))*pu0imt(:,i)'+pu0ims(:,i)')*(v0-vs(:,i));
    m0(i+M)=mr0(i+M)*fc(i)/c;
end

% -- processing --
%% For each noise level, performing Nmc trials of the algorithm
Ru=zeros(12,NumNseLvl);
Rv=zeros(12,NumNseLvl);
for i=1:NumNseLvl,
    fprintf('.');
    Qt=NseLvl(i)^2*eye(M)/c^2;      % Time delay covariance matrix
    Qf=1e4*NseLvl(i)^2*eye(M)/c^2;  % Doppler shift covariance matrix
    Q=zeros(2*M,2*M);               % measurement covariance matrix
    Q(1:M,1:M)=Qt;
    Q(M+1:2*M,M+1:2*M)=Qf;
    Qt0=eye(M);
    Qf0=1e4*eye(M);
    Q0=Q/NseLvl(i)^2*c^2;
    Lm=chol(Q,'lower');             % Q=Lm*Lm'
    sum_u=zeros(K,5);               
    sum_v=zeros(K,5);               
    
    %% Theoretical performance for time localization
    if (TimeDelayLoc)
        CRLB=TmDpLocObjSsrMtnEftCRLB(u0,v0,t,vs,Qt,0,fc,c);
        [mse_u] = TheoryMSE(u0,v0,t,vs,m0(1:M),Qt,fc,c);
        Ru(2,i)=sqrt(mse_u);
    else
        CRLB=TmDpLocObjSsrMtnEftCRLB(u0,v0,t,vs,Qt,Qf,fc,c);
        [mse_u, mse_v] = TheoryMSE(u0,v0,t,vs,m0,Q,fc,c);
        Ru(2,i)=sqrt(mse_u);
        Rv(2,i)=sqrt(mse_v);
    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)));
    Ru(2,i)=sqrt(mse_u);
    
    %% loop through number of ensemble runs
    for j=1:Nmc
        m=m0+Lm*NoiseM(:,j);  % create a noise vector whose covariance matrix is Q
        
        %% initialization
        if (TimeDelayLoc)
            [xkf,xk0]= TmLocObjMtnEft_CFS(m(1:M),t+ones(K,1)*m(1:M)'.*vs/2,Qt0,c);%
        else
            [xkf,xk0] = TmDpLocObjMtnEft_CFS(m,t+ones(K,1)*m(1:M)'.*vs/2,Q0,fc,c);
        end;
        uk0=xk0(1:K);
        vk0=xk0(K+1:2*K);
        
        %%  Gauss-Newton for inaccurate models of range and range rate: ignoring both object and sensor motions
        if (isnan(Ru(6,i))&isnan(Rv(6,i)))==0
            if (TimeDelayLoc)
                [x] = TmDpLocIgrObjSsrMtnEft_GN(uk0,vk0,t,vs,m(1:M),Qt0,fc,c,1e-5); 
                sum_u(:,2)=sum_u(:,2)+x(1:K);
                sum_v(:,2)=sum_v(:,2)+zeros(K,1);
                Ru(6,i)=Ru(6,i)+norm(x(1:K)-u0)^2/Nmc;
                Rv(6,i)=Rv(6,i)+norm(zeros(K,1)-v0)^2/Nmc;
            else
                [x] = TmDpLocIgrObjSsrMtnEft_GN(uk0,vk0,t,vs,m,Q0,fc,c,1e-5);       
                sum_u(:,2)=sum_u(:,2)+x(1:K);
                sum_v(:,2)=sum_v(:,2)+x(K+1:2*K);
                Ru(6,i)=Ru(6,i)+norm(x(1:K)-u0)^2/Nmc;
                Rv(6,i)=Rv(6,i)+norm(x(K+1:2*K)-v0)^2/Nmc;
            end;
        end;
        
        %% Gauss-Newton for accurate model with Range
        if (isnan(Ru(4,i))&isnan(Rv(4,i)))==0
            if (TimeDelayLoc)
                [u1,v1] = TmLocObjSsrMtnEft_GN(uk0,vk0,t,vs,m(1:M),Qt0,c,1e-5);
            else
                [u1, v1] = TmDpLocObjSsrMtnEft_GN(uk0,vk0,t,vs,m,Q0,fc,c,1e-10);
            end;
            sum_u(:,1)=sum_u(:,1)+u1;
            sum_v(:,1)=sum_v(:,1)+v1;
            
            Ru(4,i)=Ru(4,i)+norm(u1-u0)^2/Nmc;
            Rv(4,i)=Rv(4,i)+norm(v1-v0)^2/Nmc;
        end
        
        %%  Quasi-Newton for accurate model with Range
        if (isnan(Ru(8,i))&isnan(Rv(8,i)))==0
            if (TimeDelayLoc)
                [u1,v1]=TmLocObjSsrMtnEft_QN(uk0,vk0,t,vs,m(1:M),Qt0,c,1e-10);      % quasi-newton
            else
                [u1, v1] = TmDpLocObjSsrMtnEft_QN(uk0,vk0,t,vs,m,Q0,fc,c,1e-10);    % quasi-newton
            end;
            sum_u(:,3)=sum_u(:,3)+u1;
            sum_v(:,3)=sum_v(:,3)+v1;
            
            Ru(8,i)=Ru(8,i)+norm(u1-u0)^2/Nmc;
            Rv(8,i)=Rv(8,i)+norm(v1-v0)^2/Nmc;
        end
        
        %% closed-form solution for moving sensors and stationary object
        if (isnan(Ru(10,i)))==0
            if (TimeDelayLoc)
                [u1] = TmLocSsrMtnEft(m(1:M),t,vs,Qt0,c);
            else
                [u1] = TmDpLocSsrMtnEft(m,t,vs,Q0,fc,c);
            end;
            sum_u(:,4)=sum_u(:,4)+u1;
            sum_v(:,4)=sum_v(:,4);
            
            Ru(10,i)=Ru(10,i)+norm(u1-u0)^2/Nmc;
            Rv(10,i)=Rv(10,i)+norm(v0)^2/Nmc;
        end
    end
    
    %% collect statistics
    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));
    Ru(5,i)=norm(sum_u(:,2)/Nmc-u0);
    Rv(5,i)=norm(sum_v(:,2)/Nmc-v0);
    Ru(6,i)=sqrt(Ru(6,i));
    Rv(6,i)=sqrt(Rv(6,i));
    Ru(7,i)=norm(sum_u(:,3)/Nmc-u0);
    Rv(7,i)=norm(sum_v(:,3)/Nmc-v0);
    Ru(8,i)=sqrt(Ru(8,i));
    Rv(8,i)=sqrt(Rv(8,i));
    Ru(9,i)=norm(sum_u(:,4)/Nmc-u0);
    Rv(9,i)=norm(sum_v(:,4)/Nmc-v0);
    Ru(10,i)=sqrt(Ru(10,i));
    Rv(10,i)=sqrt(Rv(10,i));
    Ru(11,i)=norm(sum_u(:,5)/Nmc-u0);
    Rv(11,i)=norm(sum_v(:,5)/Nmc-v0);
    Ru(12,i)=sqrt(Ru(12,i));
    Rv(12,i)=sqrt(Rv(12,i));
end
fprintf('\n');

% -- plot results --
figure(1);
subplot(2,1,1);     % position results
p=loglog(NseLvl,(Ru(1,:)),'r-',NseLvl,(Ru(4,:)),'bo',NseLvl,(Ru(8,:)),'b*',NseLvl,(Ru(3,:)),'bx',NseLvl,(Ru(7,:)),'b^',NseLvl,(Ru(2,:)),'r--',NseLvl,(Ru(6,:)),'bs',NseLvl,(Ru(5,:)),'b+',NseLvl,(Ru(10,:)),'mv');
if (TimeDelayLoc)
    axis([min(NseLvl) 1e2 1e-3 1e3]);
else
    axis([min(NseLvl) max(NseLvl) 1e-4 1e4]);
end;
grid on;
xlabel('\sigma (m)');
ylabel(' Position RMSE (m) ');
set(p,'LineWidth',1);
legend(p(1:5),'CRLB','Gauss-Newton','Quasi-Newton','Bias of Gauss-Newton','Bias of Quasi-Newton','location','southeast');
ah=axes('position',get(gca,'position'),'visible','off');
legend(ah,p(6:9),'Theoretical RMSE when ignoring motion effects','RMSE when ignoring motion effects','Bias when ignoring motion effects','RMSE closed-form solution [40]','location','northwest');

subplot(2,1,2);     % velocity results
if (TimeDelayLoc)
    p=loglog(NseLvl,(Rv(1,:)),'r-',NseLvl,(Rv(4,:)),'bo',NseLvl,(Rv(8,:)),'b*',NseLvl,(Rv(3,:)),'bx',NseLvl,(Rv(7,:)),'b^');
    axis([min(NseLvl) 1e2 1e-2 1e4]);
    xlabel('\sigma (m) ');
    ylabel(' Velocity RMSE (m/s) ');
    grid on;
    set(p,'LineWidth',1);
    legend('CRLB','Gauss-Newton','Quasi-Newton','Bias of Gauss-Newton','Bias of Quasi-Newton','location','northwest');
else
    p=loglog(NseLvl,(Rv(1,:)),'r-',NseLvl,(Rv(4,:)),'bo',NseLvl,(Rv(8,:)),'b*',NseLvl,(Rv(3,:)),'bx',NseLvl,(Rv(7,:)),'b^',NseLvl,(Rv(2,:)),'r--',NseLvl,(Rv(6,:)),'bs',NseLvl,(Rv(5,:)),'b+');
    % grid on;
    axis([min(NseLvl) max(NseLvl) 1e-6 1e3]);
    xlabel('\sigma (m) ');
    ylabel(' Velocity RMSE (m/s) ');
    grid on;
    set(p,'LineWidth',1);
    legend(p(1:5),'CRLB','Gauss-Newton','Quasi-Newton','Bias of Gauss-Newton','Bias of Quasi-Newton','location','southeast');  % this function shold put in the final place
    ah=axes('position',get(gca,'position'),'visible','off');
    legend(ah,p(6:8),'Theoretical RMSE when ignoring motion effects','RMSE when ignoring motion effects','Bias when ignoring motion effects','location','northwest');
end;




