% This program reproduces Fig. 4 of the reference paper for moving source localization.
%
% Only 10 Monte-Carlo runs (Mc) are used in the setting to reduce the time  
% it takes to generate the results.
%
% Reference:
% D. Lin, G. Wang, K. C. Ho, and L. Huang, "Source localization by frequency
% measurements in unknown signal propagation speed environments," IEEE
% Trans. Aerosp. Electron. Syst., vol. 59, no. 4, pp. 3953-3970, Aug. 2023.
%
% Danyan Lin, Gang Wang, K. C. Ho	Nov. 1, 2023
%
%       Copyright (C) 2023
%       the Faculty of Electrical Engineering and Computer Science
%       Ningbo University
%       Ningbo 315211, China
%       wanggang@nbu.edu.cn
%

clear;
clc;
warning('off');
KK = 10;                   %the number of configurations
N = 10;                    %the number of sensors
Mc = 10;                   %the number of Mc runs
% Mc = 1000;                    %the number of Mc runs

s_min_dis = 300/1000;    %the distance between sensors (km)
d_s_dis = 100/1000;      %the distance between source and sensors (km)
s_dot_dis = 2/1000;      %the distance between the velocities of sensors (km/s)

[S,S_dot,P,V,C] = DSFLocUknPropSpdMvgSrc_CnstgConf(12,s_min_dis,d_s_dis,s_dot_dis,KK);
% sigma2_dBm = -45:5:15;  %noise power
sigma2_dBm = -45:10:15;  %noise power
sigma2_n = 10.^(sigma2_dBm/10);
CRLB_p = zeros(1,length(sigma2_dBm));
CRLB_v = zeros(1,length(sigma2_dBm));
CRLB_c = zeros(1,length(sigma2_dBm));
MSE_p1 = zeros(1,length(sigma2_dBm));
MSE_v1 = zeros(1,length(sigma2_dBm));
MSE_c1 = zeros(1,length(sigma2_dBm));
MSE_p2 = zeros(1,length(sigma2_dBm));
MSE_v2 = zeros(1,length(sigma2_dBm));
MSE_c2 = zeros(1,length(sigma2_dBm));

for kk=1:KK
    fprintf('\nConfiguration: %d/%d\n',kk,KK);
    s = S(:,1:N,kk); %sensor positions for the kk-th configuration
    s_dot = S_dot(:,1:N,kk); %sensor velocities for the kk-th configuration
    p = P(:,:,kk);  %source position for the kk-th configuration
    c = C(kk);   %signal propagation speed for the kk-th configuration
    v = V(:,:,kk);  %source velocity for the kk-th configuration
    f0 = 10000;  % carrier frequency

    for IteIdx=1:length(sigma2_dBm)
        it_AE = 0;    %it_AE is to count the iteration times of AE

        Qe = sigma2_n(IteIdx)*eye(N);      %the covariance matrix of measurement noise
        randn('seed',0);
        noiseOri = sqrt(sigma2_n(IteIdx))*randn(N,Mc); %generate Gaussian noise
        noise_mean = mean(noiseOri,2);
        noise = noiseOri-repmat(noise_mean,1,Mc);  %generate zero-mean noise

        num1 = 0; %num1 is to count the number of rank-1 solutions of SDR method
        error_Form1_p1 = 0;
        error_Form1_v1 = 0;
        error_Form1_c1 = 0;

        num2 = 0; %num2 is to count the number of rank-1 solutions of AE method
        error_Form1_p2 = 0;
        error_Form1_v2 = 0;
        error_Form1_c2 = 0;

        CRLB = DSFLocUknPropSpdMvgSrc_CRLB(p,v,s,s_dot,f0,c,Qe);
        CRLB_p(IteIdx) = CRLB_p(IteIdx)+sum(CRLB(1:2)); %the CRLB for estimating the source position
        CRLB_v(IteIdx) = CRLB_v(IteIdx)+sum(CRLB(3:4));  %the CRLB for estimating the source velocity
        CRLB_c(IteIdx) = CRLB_c(IteIdx)+sum(CRLB(5)); %the CRLB for estimating the signal propagation speed

        parfor  mc=1:Mc
            f = [];
            for i=1:N
                f(i,:) = f0-f0/c*(p-s(:,i))'*(v-s_dot(:,i))/norm(p-s(:,i))+noise(i,mc); %the frequency measurements
            end
            % ================================SDR====================================
            [p1,c1,v1,rank1] = DSFLocUknPropSpdMvgSrc_SDR(s,s_dot,f,Qe,f0); %the solution of SDR method
            if rank1==1
                num1=num1+1;
            end
            error_Form1_p1 = error_Form1_p1+norm(p1-p)^2;
            error_Form1_v1 = error_Form1_v1+norm(v1-v)^2;
            error_Form1_c1 = error_Form1_c1+norm(c1-c)^2;
            % =================================AE======================================
            [c2,Iter_AE,p2,v2,rank2] = DSFLocUknPropSpdMvgSrc_AE(s,s_dot,f,Qe,f0); %the solution of AE method
            it_AE = it_AE+Iter_AE;
            if rank2==1
                num2=num2+1;
            end
            error_Form1_p2 = error_Form1_p2+norm(p2-p)^2;
            error_Form1_v2 = error_Form1_v2+norm(v2-v)^2;
            error_Form1_c2 = error_Form1_c2+norm(c2-c)^2;

        end

        it_sum_AE(kk,IteIdx) = it_AE/Mc;

        num1_sum(kk,IteIdx) = num1;
        MSE_p1(IteIdx) = MSE_p1(IteIdx)+error_Form1_p1/Mc;
        MSE_v1(IteIdx) = MSE_v1(IteIdx)+error_Form1_v1/Mc;
        MSE_c1(IteIdx) = MSE_c1(IteIdx)+error_Form1_c1/Mc;

        num2_sum(kk,IteIdx) = num2;
        MSE_p2(IteIdx) = MSE_p2(IteIdx)+error_Form1_p2/Mc;
        MSE_v2(IteIdx) = MSE_v2(IteIdx)+error_Form1_v2/Mc;
        MSE_c2(IteIdx) = MSE_c2(IteIdx)+error_Form1_c2/Mc;

    end
    CRLB_p_dBm = 10*log10(CRLB_p/kk);
    CRLB_v_dBm = 10*log10(CRLB_v/kk);
    CRLB_c_dBm = 10*log10(CRLB_c/kk);

    MSE_p1_dBm = 10*log10(MSE_p1/kk);
    MSE_v1_dBm = 10*log10(MSE_v1/kk);
    MSE_c1_dBm = 10*log10(MSE_c1/kk);

    MSE_p2_dBm = 10*log10(MSE_p2/kk);
    MSE_v2_dBm = 10*log10(MSE_v2/kk);
    MSE_c2_dBm = 10*log10(MSE_c2/kk);

end

figure(1)
plot(sigma2_dBm,MSE_p1_dBm+60,'m-*','LineWidth',2);hold on;
plot(sigma2_dBm,MSE_p2_dBm+60,'g-d','LineWidth',2);hold on;
plot(sigma2_dBm,CRLB_p_dBm+60,'b-','LineWidth',2);hold on;
legend('SDR','AE','CRLB','location','northwest');
grid on;
axis([-45 15 -20 80])
xlabel('10lg(\sigma^2)');
ylabel('10lg(MSE(p))');
title('MSE(p)');
figure(2)
plot(sigma2_dBm,MSE_v1_dBm+60,'m-*','LineWidth',2);hold on;
plot(sigma2_dBm,MSE_v2_dBm+60,'g-d','LineWidth',2);hold on;
plot(sigma2_dBm,CRLB_v_dBm+60,'b-','LineWidth',2);hold on;
legend('SDR','AE','CRLB','location','northwest');
grid on;
axis([-45 15 -60 20])
xlabel('10lg(\sigma^2)');
ylabel('10lg(MSE(v))');
title('MSE(v)');
figure(3)
plot(sigma2_dBm,MSE_c1_dBm+60,'m-*','LineWidth',2);hold on;
plot(sigma2_dBm,MSE_c2_dBm+60,'g-d','LineWidth',2);hold on;
plot(sigma2_dBm,CRLB_c_dBm+60,'b-','LineWidth',2);hold on;
legend('SDR','AE','CRLB','location','northwest');
grid on;
axis([-45 15 -40 60])
xlabel('10lg(\sigma^2)');
ylabel('10lg(MSE(c))');
title('MSE(c)');
