% Localization using DSF and DD measurements.
%
% Performance comparison in known source frequency case by using
% - closed form solution: DSFDDLoc_CFS_Knownfo.m
% - SDP solution: DSFDDLoc_SDP_Knownfo.m
% - MLE solution initialized by closed-form solution: DSFDDLoc_MLE_Knownfo.m
%
% Performance comparison in unknown source frequency case by using
% - closed form solution: DSFDDLoc_CFS_Unknownfo.m
% - SDP solution: DSFDDLoc_SDP_Unknownfo.m
% - MLE solution initialized by closed-form solution: DSFDDLoc_MLE_Unknownfo.m
%
% The code can reproduce the results in Figs. 2-5 of the reference paper.
%
% Number of ensemble runs (NumEns) is set to 3 and number of geometries (NumGeo)
% is set to 1 to reduce run time of the code. To reproduce the results in
% Figs. 2-5, set NumEns=50, and NumGeo=10.
%
% We can disable the generation of SDP solution by setting SolSDP=0.
%
% Reference: X. Ke and K. C. Ho, "Localization by Doppler derivatives and 
% Doppler-shifted frequencies," IEEE Trans. Signal Process., vol. 72, pp. 2890-2904, 2024.
%
% X. Ke, K. C. Ho     7-21-2024
%
%       Copyright (C) 2024
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA.
%       hod@missouri.edu
%
%% 
clear all;
%clc;
%close all;
warning off;
rng(1327);

SolSDP = 1;      % generate SDP solution by CVX: Yes: 1, No: 0

% -- setttings --
M_max  = 18;     % maximum number of sensors can be used
M = 12;          % number of sensors 

K = 3;           % localization dimension, 2-D: K=2, 3-D: K=3
norm_u_dot = 10; % the norm of object velocity
fo_o = 15000;    % true carrier frequency
c = 1500;        % signal propagation speed
min_dis = 200;   % minimum distance between any two sensors
NumGeo = 1;      % number of sensors
NumEns = 3;      % number of ensemble runs

% -- index for similation figures --
% To reproduce figure 2, set (FigIdx1,FigIdx2) = (1,1);
% To reproduce figure 3, set (FigIdx1,FigIdx2) = (1,2);
% To reproduce figure 4, set (FigIdx1,FigIdx2) = (2,1);
% To reproduce figure 5, set (FigIdx1,FigIdx2) = (2,2);
FigIdx1 = 1; % FigIdx1 = 1: MSE vs DSF noise power, FigIdx1 = 2: MSE vs DD noise power
FigIdx2 = 1; % FigIdx2 = 1: known center frequency case, FigIdx2 = 2: unknown center frequency case

% -- dsf and dd measurements noise power --
switch FigIdx1
    case 1
        sigma2_dsf = -40:5:20;  % 10*log10(dsf measurement noise power)
        sigma2_dd = -40*ones(1,length(sigma2_dsf));    % 10*log10(dd measurement noise power) 
    case 2
        sigma2_dd = -60:5:-10; % 10*log10(dd measurement noise power) 
        sigma2_dsf = -20*ones(1,length(sigma2_dd));   % 10*log10(dsf measurement noise power)
end
 
[uu,uu_dot,SS] = DSFDDLoc_GeoGenerator(K,M_max,NumGeo,min_dis,norm_u_dot); % localization geometries

% -- DSF and DD noise sequence --
EE_eta = zeros(2*M_max,NumEns,NumGeo);
for g = 1:1:NumGeo
    EE_eta(:,:,g) = randn(2*M_max,NumEns); 
end
EE_n = EE_eta(1:M,:,:);          % DSF noise sequence
EE_n_dot = EE_eta(M+1:2*M,:,:);  % DD noise sequence

% -- initializ record for CRLB and MSE --
dsfdd_crlb_u = zeros(length(sigma2_dsf),1);     % CRLB of object position by DSF and DD measurements
dsfdd_crlb_u_dot = zeros(length(sigma2_dsf),1); % CRLB of object velocity by DSF and DD measurements
dsfdd_crlb_fo = zeros(length(sigma2_dsf),1);    % CRLB of center frequency by DSF and DD measurements
dsf_crlb_u = zeros(length(sigma2_dsf),1);       % CRLB of object position by DSF measurement
dsf_crlb_u_dot = zeros(length(sigma2_dsf),1);   % CRLB of object velocity by DSF measurement
dsf_crlb_fo = zeros(length(sigma2_dsf),1);      % CRLB of center frequency by DSF measurement

NumAlg = 3; % number of algorithms to be evaluated
mse = zeros(3,NumAlg,length(sigma2_dsf),NumGeo); % MSEs of object position, object velocity, and center frequency (if it is not known).

for g = 1:1:NumGeo                           % loop through geometries
    for Idx=1:length(sigma2_dsf)             % loop through measurement noise power
        S = SS(:,1:M,g);                     % sensor positions
        u_o = uu(:,g);                       % true object position
        u_dot_o = uu_dot(:,g);               % true object velocity
        QF = 10^(sigma2_dsf(Idx)/10)*eye(M); % covariance matrix of DSF measurement
        QD = 10^(sigma2_dd(Idx)/10)*eye(M);  % covariance matrix of DD measurement
        Q = blkdiag(QF,QD);                  % covariance matrix of DSF and DD measurements
        
        % -- true DSFs and DDs --
        r_o = zeros(M,1);  
        f_o = zeros(M,1);        
        f_dot_o = zeros(M,1);   
        for i = 1:1:M
            r_o(i) = norm(u_o - S(:,i)); % true distance between object and sensor i
            f_o(i,1) = fo_o*( 1 - (u_o-S(:,i))'*u_dot_o/(c*r_o(i,1)) ) ;                            % true DSF
            f_dot_o(i,1) =  ( ( ((f_o(i,1)-fo_o)*c )^2 )/fo_o -fo_o*norm(u_dot_o)^2 )/(c*r_o(i,1)); % true DD
        end
        
        % -- CRLB --
        CRLB_DSF =   DSFDDLoc_CRLB(fo_o,c,u_o,u_dot_o,S,Q,1,FigIdx2); % CRLB using DSF only
        CRLB_DSFDD = DSFDDLoc_CRLB(fo_o,c,u_o,u_dot_o,S,Q,2,FigIdx2); % CRLB using DSF and DD
        dsfdd_crlb_u(Idx) = dsfdd_crlb_u(Idx) + trace(CRLB_DSFDD(1:K,1:K))/NumGeo; 
        dsfdd_crlb_u_dot(Idx) = dsfdd_crlb_u_dot(Idx) + trace(CRLB_DSFDD(K+1:2*K,K+1:2*K))/NumGeo;
        dsf_crlb_u(Idx) = dsf_crlb_u(Idx) + trace(CRLB_DSF(1:K,1:K))/NumGeo;
        dsf_crlb_u_dot(Idx) = dsf_crlb_u_dot(Idx) + trace(CRLB_DSF(K+1:2*K,K+1:2*K))/NumGeo;
        if FigIdx2~=1
            dsfdd_crlb_fo(Idx) = dsfdd_crlb_fo(Idx) + trace(CRLB_DSFDD(end,end))/NumGeo;
            dsf_crlb_fo(Idx) = dsf_crlb_fo(Idx) + trace(CRLB_DSF(end,end))/NumGeo;
        end

        % -- initialize record for norm(estimate error)^2 --
        error2_u = zeros(NumAlg,NumEns);   
        error2_udot = zeros(NumAlg,NumEns);
        error2_fo = zeros(NumAlg,NumEns);

        for nn = 1:1:NumEns   % loop through ensemble runs
            warning('off','all');         
            f = f_o+10^(sigma2_dsf(Idx)/20)*EE_n(:,nn,g);            % DSF measurement
            f_dot = f_dot_o+10^(sigma2_dd(Idx)/20)*EE_n_dot(:,nn,g); % DD measurement
            
            u_hat = zeros(K,NumAlg);     % initialize record for estimate of object position
            u_dot_hat = zeros(K,NumAlg); % initialize record for estimate of object velocity
            fo_hat = zeros(1,NumAlg);    % initialize record for estimate of center frequency

            switch FigIdx2
                case 1
                    % -- proposed closed-form estimator when center frequency is known --
                    [u_hat(:,1),u_dot_hat(:,1)] = DSFDDLoc_CFS_Knownfo(f,f_dot,c,fo_o,S,Q,2);
                    
                    % -- proposed SDP estimator when center frequency is known --
                    if (SolSDP)
                        [u_hat(:,2),u_dot_hat(:,2)] = DSFDDLoc_SDP_Knownfo(f,f_dot,c,fo_o,S,Q);
                    end;
                    
                    % -- proposed MLE-CFS estimator when center frequency is known --
                    phi0 = [u_hat(:,1);u_dot_hat(:,1)]; % initialize the MLE by closed-form solution
                    [u_hat(:,3),u_dot_hat(:,3)] = DSFDDLoc_MLE_Knownfo(f,f_dot,c,fo_o,S,Q,phi0);
                    
                otherwise
                    % -- proposed closed-form estimator when center frequency is unknown --
                    [u_hat(:,1),u_dot_hat(:,1),fo_hat(1)] = DSFDDLoc_CFS_Unknownfo(f,f_dot,c,S,Q,2);
                    
                    % -- proposed SDP estimator when center frequency is unknown --
                    if (SolSDP)
                        [u_hat(:,2),u_dot_hat(:,2),fo_hat(2)] = DSFDDLoc_SDP_Unknownfo(f,f_dot,c,S,Q);
                    end;
                    
                    % -- proposed MLE-CFS estimator when center frequency is unknown --
                    theta0 = [u_hat(:,1);u_dot_hat(:,1);fo_hat(1)]; % initialize the MLE by closed-form solution
                    [u_hat(:,3),u_dot_hat(:,3),fo_hat(3)] = DSFDDLoc_MLE_Unknownfo(f,f_dot,c,S,Q,theta0);
                    
            end
            error2_u(:,nn) = ((u_hat - u_o).^2)'*ones(K,1);            % norm(object position estimate error)^2
            error2_udot(:,nn) = ((u_dot_hat - u_dot_o).^2)'*ones(K,1); % norm(object velocity estimate error)^2
            error2_fo(:,nn) = ((fo_hat - fo_o).^2)';                   % norm(center frequency estimate error)^2
        end
        mse(1,:,Idx,g) = nanmean(error2_u',1);    % MSE for object position estimate
        mse(2,:,Idx,g) = nanmean(error2_udot',1); % MSE for object velocity estimate
        mse(3,:,Idx,g) = nanmean(error2_fo',1);   % MSE for center position estimate
        fprintf('progress: %2.1f%% \n',100*( (g-1)/NumGeo + (Idx)/(NumGeo*length(sigma2_dsf))))
    end
end
%% 
% -- create plot to show the results --
mse = nanmean(mse,4); % average the MSE values across all geometries
mse_u = (reshape(mse(1,:,:),[3,length(sigma2_dsf)]))';
mse_u_dot = (reshape(mse(2,:,:),[3,length(sigma2_dsf)]))';
mse_fo = (reshape(mse(3,:,:),[3,length(sigma2_dsf)]))';
fig_width = 520;  % width of the figure window in pixels
fig_height = 520; % height of the figure window in pixels
FS = 8; % fontsize
switch FigIdx1
    case 1
        str = '20 log($\sigma$ (Hz))';
        Yaxis_ub_u = 50;
        Yaxis_ub_u_dot = 10;
        Yaxis_ub_fo = 30;
        Xaxis_pixel = sigma2_dsf;
    case 2
        str = '20 log($\dot \sigma$ (Hz/s))';
        Yaxis_ub_u = 40;
        Yaxis_ub_u_dot = 0;
        Yaxis_ub_fo = 10;
        Xaxis_pixel = sigma2_dd;
end
close all;
figure(1)
if FigIdx2==2
    fig_height = 1.42*fig_height;
end
set(gcf, 'Position', [100, 100, fig_width, fig_height]);

% -- show MSE of object position estimate --
if FigIdx2==1
    subplot(2,1,1)
else
    subplot(3,1,1)
end
plot(Xaxis_pixel, 10*log10(dsfdd_crlb_u),'-r','Linewidth',1); hold on; grid on;
plot(Xaxis_pixel, 10*log10(dsf_crlb_u),'--k','Linewidth',1); hold on;
plot(Xaxis_pixel, 10*log10(mse_u(:,1)),'om','Linewidth',0.8);
if (SolSDP)
    plot(Xaxis_pixel, 10*log10(mse_u(:,2)),'+b','Linewidth',0.9);
end;
plot(Xaxis_pixel, 10*log10(mse_u(:,3)),'^','Color',[0.4940 0.1840 0.5560],'Linewidth',1.0);
if (SolSDP)
    legend({'CRLB','CRLB-DSF','CFS','SDP','MLE-CFS'},'Location','northwest','FontSize',FS)
else
    legend({'CRLB','CRLB-DSF','CFS','MLE-CFS'},'Location','northwest','FontSize',FS)
end;
xlabel(str, 'FontSize', 14,'Interpreter','latex');
ylabel('10 log(MSE($\mathbf{u}$(m)))', 'FontSize', 12,'Interpreter','latex');
ylim([-inf Yaxis_ub_u]);

% -- show MSE of object velocity estimate --
if FigIdx2==1
    subplot(2,1,2)
else
    subplot(3,1,2)
end
plot(Xaxis_pixel, 10*log10(dsfdd_crlb_u_dot),'-r','Linewidth',1); hold on; grid on;
plot(Xaxis_pixel, 10*log10(dsf_crlb_u_dot),'--k','Linewidth',1); hold on;
plot(Xaxis_pixel, 10*log10(mse_u_dot(:,1)),'om','Linewidth',0.8);
if (SolSDP)
    plot(Xaxis_pixel, 10*log10(mse_u_dot(:,2)),'+b','Linewidth',0.9);
end;
plot(Xaxis_pixel, 10*log10(mse_u_dot(:,3)),'^','Color',[0.4940 0.1840 0.5560],'Linewidth',1.0);
if (SolSDP)
    legend({'CRLB','CRLB-DSF','CFS','SDP','MLE-CFS'},'Location','northwest','FontSize',FS);
else
    legend({'CRLB','CRLB-DSF','CFS','MLE-CFS'},'Location','northwest','FontSize',FS)
end;
xlabel(str, 'FontSize', 14,'Interpreter','latex');
ylabel('10 log(MSE($\dot \mathbf{u}$(m/s)))', 'FontSize', 12,'Interpreter','latex');
ylim([-inf Yaxis_ub_u_dot]);

% -- show MSE of center frequency estimate --
if FigIdx2==2
    subplot(3,1,3)
    plot(Xaxis_pixel, 10*log10(dsfdd_crlb_fo),'-r','Linewidth',1); hold on; grid on;
    plot(Xaxis_pixel, 10*log10(dsf_crlb_fo),'--k','Linewidth',1); hold on;
    plot(Xaxis_pixel, 10*log10(mse_fo(:,1)),'om','Linewidth',0.8);
    if (SolSDP)
        plot(Xaxis_pixel, 10*log10(mse_fo(:,2)),'+b','Linewidth',0.9);
    end;
    plot(Xaxis_pixel, 10*log10(mse_fo(:,3)),'^','Color',[0.4940 0.1840 0.5560],'Linewidth',1.0);
    if (SolSDP)
        legend({'CRLB','CRLB-DSF','CFS','SDP','MLE-CFS'},'Location','northwest','FontSize',FS);
    else
        legend({'CRLB','CRLB-DSF','CFS','MLE-CFS'},'Location','northwest','FontSize',FS);
    end;
    xlabel(str, 'FontSize', 14,'Interpreter','latex');
    ylabel('10 log(MSE($f_o$(Hz)))', 'FontSize', 12,'Interpreter','latex');
    ylim([-inf Yaxis_ub_fo]);
end

%% ======================================================================================== 
function [uu,uu_dot,SS] = DSFDDLoc_GeoGenerator(K,M,no_of_geo,min_dis,norm_u_dot)
% [uu,uu_dot,SS] = DSFDDLoc_GeoGenerator(K,M,no_of_geo,min_dis,norm_u_dot)
% Generation of localization geometries
%
% Input
% K:           scalar, localziation dimension 
% M:           scalar, number of sensors
% no_of_geo:   scalar, number of geometries
% min_dis:     scalar, minimum distance between any two sensors
% norm_u_dot:  scalar, norm of object velocity
%
% Output
% uu:          (3 x no_of_geo), object position
% uu_dot:      (3 x no_of_geo), object velocity
% SS:          (3 x M x no_of_geo), sensor positions


SS=zeros(K,M,no_of_geo);
if K==2
    cu = [0;0];
    Au = 100*[1;1];
    cS1 = [-25;-25];
    cS2 = [25;25];
    AS = 1000*[1;1];
    theta = 2*pi*rand(no_of_geo,1);
    uu_dot = norm_u_dot*[cos(theta),sin(theta)]';
elseif K==3
    cu = [0;0;-250];
    Au = 100*[1;1;0.5];
    cS1 = [0;0;-50];
    cS2 = [0;0;-450];
    AS = 1000*[1;1;0.1];
    theta = 2*pi*rand(no_of_geo,1);
    phi = pi*(rand(no_of_geo,1)-0.5);
    uu_dot = norm_u_dot*[cos(phi).*cos(theta),cos(phi).*sin(theta),sin(phi)]';
end

uu = cu-Au/2+Au.*rand(K,no_of_geo);
for g = 1:1:no_of_geo
    cS = cS1;
    SS(:,1,g) = cS-AS/2+AS.*rand(K,1);
    for m = 2:1:M
        while 1
            if m>ceil(M/2)
                cS = cS2;
            end
            tS = cS-AS/2+AS.*rand(K,1);
            a = norm(tS - SS(:,1,g));
            for j = 2:1:m-1
                a = min(a,norm(tS-SS(:,j,g)));
            end
            if (a>min_dis)
                SS(:,m,g) = tS;
                break;
            end
        end
    end
end

end