% This program reproduces the results in Figs. 6-7  of the reference paper
% below.
%
% Change "NumEns" to 1000 for reproducing Fig. 6 and 100 for Fig. 7.  
% Current setting uses 10 ensemble runs to reduce processing time (long 
% processing time mainly coming from the SDP solution).
%
% Reference:
% M. Ahmed, K. C. Ho, and G. Wang, "Localization of a moving source by
% frequency measurements," IEEE Trans. Signal Process., vol. 68,
% pp. 4839-4854, 2020.
%
% Musaab Ahmed, K. C. Ho	08-18-2021
%
%       Copyright (C) 2021
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA
%       hod@missouri.edu
%

clear;
%clc;
warning off;
rng(1132076);

% ------------------- choosing figure --------------------
% 6: Fig.6,  7: Fig.7
FigNum=6;
% --------------------------------------------------------

% ------------------- choosing solution methods --------------------
% 1: SCF,   2: SDP,   [1:2]: SCF and SDP
SolMthd=1:2;
% ------------------------------------------------------------------

% ----- simulation settings -----
M=7;   %Number of Sensors
GeoNum=1:10;    % number of geometries for averaging results
nsePwrMsrLogScl=-60:5:-10;  % 20 log10(noisePowerMeasurement)
nsePwrSLogScl=-30:5:20;     % 20 log10(noisePowerSensorPosition)
nsePwrfoLogScl=-50:5:-15;   % 20 log10(noisePowerSourceFrequency)
NumEns=10; 	%1e3;           % number of ensembles
fo_o=15e3;      % true value of  the source frequency
c=1500;         % signal propagation speed
T=1;            % time step of successive measurements
N=35;           % number of measurements in successive times

% ----- noise powers -----
switch FigNum,
    case 6,
        nsePwrLogScl=nsePwrMsrLogScl;
    case 7,
        nsePwrLogScl=nsePwrMsrLogScl;
    case 10,
        nsePwrLogScl=nsePwrSLogScl;
    case 11,
        nsePwrLogScl=nsePwrfoLogScl;
end;

% -- generate randomly localization geometries --
min_dis=350;
SS=zeros(2,M,GeoNum(end));
uu=zeros(2,GeoNum(end));
uu_dot=zeros(2,GeoNum(end));
for l=1:GeoNum(end),
    a=0;
    while a<min_dis
        S=-750+1500*rand(M,2);
        a=sqrt((repmat(S(:,1),1,M)-repmat(S(:,1)',M,1)).^2+(repmat(S(:,2),1,M)-repmat(S(:,2)',M,1)).^2)+1/eps*eye(M);a=min(a(:));
    end
    SS(:,:,l)=S';
    uu(:,l)=-2000+4000*rand(2,1);
    ang=rand*359;
    uu_dot(:,l)=10*[cosd(ang);sind(ang)];
end

% -- initialize records --
Ep=zeros(NumEns,GeoNum(end),length(nsePwrLogScl),4); % position square error
Ev=Ep;                              % velocity square error
CRLB_p=zeros(GeoNum(end),length(nsePwrLogScl));    % position CRLB
CRLB_v=CRLB_p;                      % velocity CRLB

% -- covariance matrices structure --
R=eye(M*N);   	% DSF measurement noise covariance matrix structure
Rs=eye(2*M);    % sensor position error covariance matrix structure

% -- pre-generate random errors for ensemble runs --
EE_s=randn(2*M,NumEns);     % sensor position error
EE_n=randn(M*N,NumEns);     % measurement error
EE_fo=randn(1,NumEns);      % source frequency error

% ---------- processing ----------
fprintf('Progress:%3d%%',0);

% -- loop over measurement noise power --
for nseIdx=1:length(nsePwrLogScl),
    % -- noise --
    switch FigNum,
        case 6,
            sigma2_n=nsePwrMsrLogScl(nseIdx);   % 20 log10(measurement noise power)
            Qn = 10^(sigma2_n/10)*R;    % covariance matrix of freq. measurements.
            Delta = chol(Qn)'*EE_n;     % measurement noise
            sigma2_fo=-80;              % 20 log10(source frequency noise power)
            Qfo=10^(sigma2_fo/10);      % variance of source freq. error
            Delta_fo=chol(Qfo)*EE_fo;   % source frequency error
            sigma2_s=-60;               % 20 log10(sensor position noise power)
            Qs =10^(sigma2_s/10)*Rs ;   % covariance matrix of sensor positions.
            Delta_s = chol(Qs)'*EE_s;   % sensor position errors
        case 7,
            sigma2_n=nsePwrMsrLogScl(nseIdx);   % 20 log10(measurement noise power)
            Qn = 10^(sigma2_n/10)*R;    % covariance matrix of freq. measurements.
            Delta = chol(Qn)'*EE_n;     % measurement noise
            sigma2_s=-60;               % 20 log10(sensor position noise power)
            Qs =10^(sigma2_s/10)*Rs ;   % covariance matrix of sensor positions.
            Delta_s = chol(Qs)'*EE_s;   % sensor position errors
        case 10,
            sigma2_n=-120;              % 20 log10(measurement noise power)
            Qn = 10^(sigma2_n/10)*R;    % covariance matrix of freq. measurements.
            Delta = chol(Qn)'*EE_n;     % measurement noise
            sigma2_fo=-120;             % 20 log10(source frequency noise power)
            Qfo=10^(sigma2_fo/10);      % variance of source freq. error
            Delta_fo=chol(Qfo)*EE_fo;   % source frequency error
            sigma2_s=nsePwrSLogScl(nseIdx); % 20 log10(sensor position noise power)
            Qs =10^(sigma2_s/10)*Rs ;   % covariance matrix of sensor positions.
            Delta_s = chol(Qs)'*EE_s;   % sensor position errors
        case 11,
            sigma2_n=-60;              % 20 log10(measurement noise power)
            Qn = 10^(sigma2_n/10)*R;    % covariance matrix of freq. measurements.
            Delta = chol(Qn)'*EE_n;     % measurement noise
            sigma2_fo=nsePwrfoLogScl(nseIdx);    % 20 log10(source frequency noise power)
            Qfo=10^(sigma2_fo/10);      % variance of source freq. error
            Delta_fo=chol(Qfo)*EE_fo;   % source frequency error
            sigma2_s=-120; % 20 log10(sensor position noise power)
            Qs =10^(sigma2_s/10)*Rs ;   % covariance matrix of sensor positions.
            Delta_s = chol(Qs)'*EE_s;   % sensor position errors
    end;
    
    % -- loop over geometries --
    for geo=GeoNum,
        
        % -- localization geometry --
        So=SS(:,:,geo);  % true sensor positions.
        uo=uu(:,geo);    % true source location.
        uo_dot=uu_dot(:,geo);    % true source velocity.
        uo_all=uo*ones(1,N)+uo_dot*(0:N-1)*T;
        
        % -- true DSF --
        ro=sqrt(sum((kron(uo_all,ones(1,M))-repmat(So,1,N)).^2,1));
        f_o=fo_o-fo_o/c*(((kron(uo_all,ones(1,M))-repmat(So,1,N))./repmat(ro,2,1))'*uo_dot);
        
        % -- CRLB --
        if (FigNum~=7)
            CRLB=DSF2DLocMtTm_CRLB(uo,uo_dot,So,fo_o,c,T,Qn,Qs,Qfo);
        else
            Qfo=10^9;
            CRLB=DSF2DLocFoUnkwnMtTm_CRLB(uo,uo_dot,So,fo_o,c,T,Qn,Qs,Qfo);
        end;
        CRLB_p(geo,nseIdx)=sum(CRLB(1:2));
        CRLB_v(geo,nseIdx)=sum(CRLB(3:4));
        
        % -- loop over method(s) --
        for sol_method=SolMthd,
            
            % -- ensemble runs --
            parfor loop=1:NumEns,   % perform parallel for loop
                warning off;
                
                % -- form measurements --
                f=f_o+Delta(:,loop);             % measurements vector after adding Noise
                S=So+reshape(Delta_s(:,loop),2,M); % Sensor Locations after adding Noise
                if (FigNum~=7)
                    fo=fo_o+Delta_fo(loop);            % source freq. after adding Noise.
                end;
                
                % -- weighting matrix setting --
                idc=4;
                if (FigNum==3) idc=2; end;
                %idc=0==>No weighting matrix
                %idc=1==>weighting matrix for meas. only
                %idc=2==>weighting matrix for meas. and sen err. only
                %idc=3==>weighting matrix for meas. and source freq. err. only
                %otherwise==>Full weighting matrix
                
                % -- solution method --
                if (FigNum~=7)  % source frequency known
                    switch sol_method
                        case 1
                            theta=DSF2DLocMtTm_CFS(f,S,fo,c,T,Qn,Qs,Qfo,idc);
                        case 2
                            cvx_expert true;
                            theta=DSF2DLocMtTm_SDP(f,S,fo,c,T,Qn,Qs,Qfo,idc);
                    end
                else    % source frequency unknown
                    cvx_expert true;
                    theta=DSF2DLocFoUnkwnMtTm(f,S,c,T,Qn,Qs,idc,sol_method);
                end;
                
                % -- results --
                Ep(loop,geo,nseIdx,sol_method)=sum((theta(1:2)-uo).^2);
                Ev(loop,geo,nseIdx,sol_method)=sum((theta(3:4)-uo_dot).^2);
                
            end;    % loop
            
        end;    % sol_method
        
        fprintf('\b\b\b\b%3d%%',round(((nseIdx-1)*length(GeoNum)+(find(GeoNum==geo)))*100/(length(GeoNum)*length(nsePwrLogScl))));
        
    end;     % geo
    
end;    % nseIdx

fprintf('\n');

% ----- plotting results -----
switch FigNum,
    case 6,
        axisPos=[-60 -10 -60 60];
        axisVel=[-60 -10 -100 20];
        str='20 log($\mathbf{\sigma}$(Hz))';
    case 7,
        axisPos=[-60 -10 -60 60];
        axisVel=[-60 -10 -80 20];
        str='20 log($\mathbf{\sigma}$(m))';
    case 10,
        axisPos=[-30 20 -20 60];
        axisVel=[-30 20 -60 40];
        str='20 log($\mathbf{\sigma}_{s}$(m))';
    case 11,
        axisPos=[-50 -15 -15 1];
        axisVel=[-50 -15 -65 -35];
        str='20 log($\mathbf{\sigma}_{f_o}$(Hz))';
end;
clor = [0, 114, 189;217, 83, 25;237, 177, 32;126, 47, 142;119, 172, 48]/256;

figure; clf;
subplot(211);
hold on;
if (~isempty(intersect(SolMthd,1)))
    ep=Ep(:,GeoNum,:,1);mse=mean(mean(ep,1),2);mse=mse(:);
    plot(nsePwrLogScl, 10*log10(mse), '-*', 'LineWidth', 1.5, 'DisplayName', 'CFS','Color',clor(1,:));
end;
if (~isempty(intersect(SolMthd,2)))
    ep=Ep(:,GeoNum,:,2);mse=mean(mean(ep,1),2);mse=mse(:);
    plot(nsePwrLogScl, 10*log10(mse), '-o', 'LineWidth', 1.5, 'DisplayName', 'SDP','Color',clor(2,:));
end;
CRLB_p_avg=mean(CRLB_p(GeoNum,:),1);
plot(nsePwrLogScl, 10*log10(CRLB_p_avg), '-^', 'LineWidth', 1.5, 'DisplayName', 'CRLB','Color',clor(5,:));
xlabel(str, 'FontSize', 12,'Interpreter','latex');
ylabel('10 log(MSE($\mathbf{u}$(m)))', 'FontSize', 12,'Interpreter','latex');
lgd = legend('Show');
set(lgd, 'FontSize',9, 'Location', 'Northwest');
axis(axisPos);
grid on;
hold off;

subplot(212)
hold on;
if (~isempty(intersect(SolMthd,1)))
    ev=Ev(:,GeoNum,:,1);mse=mean(mean(ev,1),2);mse=mse(:);
    plot(nsePwrLogScl, 10*log10(mse), '-*', 'LineWidth', 1.5, 'DisplayName', 'CFS','Color',clor(1,:));
end;
if (~isempty(intersect(SolMthd,2)))
    ev=Ev(:,GeoNum,:,2);mse=mean(mean(ev,1),2);mse=mse(:);
    plot(nsePwrLogScl, 10*log10(mse), '-o', 'LineWidth', 1.5, 'DisplayName', 'SDP','Color',clor(2,:));
end;
CRLB_v_avg=mean(CRLB_v(GeoNum,:),1);
plot(nsePwrLogScl, 10*log10(CRLB_v_avg), '-^', 'LineWidth', 1.5, 'DisplayName', 'CRLB','Color',clor(5,:));
xlabel(str, 'FontSize', 12,'Interpreter','latex');
ylabel('10 log(MSE($\mathbf{\dot{u}}$(m/s)))', 'FontSize', 12,'Interpreter','latex');
lgd = legend('Show');
set(lgd, 'FontSize',9, 'Location', 'Northwest');
axis(axisVel);
grid on;
hold off;

% --------------------------------------------------------------

