% This program reproduces the Figs. 2-5 in the refernece paper.
%
% The number of Monte-Carlo runs (M) is set to 200 only to reduce the time
% it takes to generate the results.
%
% Reference:
% J. Pei, G. Wang, K. C. Ho, and L. Huang, "Reducing bias for multistatic
% localization of a moving object by transmitter at unknown position," IEEE
% Trans. Aerosp. Electron. Syst., vol. 59, no. 5, pp. 5324-5341, Oct. 2023.
%
% Jian Pei, Gang Wang, K. C. Ho and Lei Huang	Nov. 1, 2023
%
%       Copyright (C) 2023
%       Faculty of Electrical Engineering and Computer Science
%       Ningbo University
%       Ningbo 315211, China.
%       wanggang@nbu.edu.cn
%

clear all; clc; warning('off'); tic             % Program initialization.
%rng('default');

X = 20;                                          % Scaling factor to avoid numerical problem.
% L = 5000;                                      % Number of ensemble runs.
L = 200;                                          % Number of ensemble runs.

cj = 10;                                         % Number of configurations
M = 1;                                           % Number of transmitters
K = 3;                                           % dimension
N = 7;                                           % Number of receivers

[to,so,uo,uv] = MSLocUknTxPosMvgObj_Conf(cj,K,M,10);   % to:transmitter position; so: receiver positions; uo: object position; uv: object velocity

delta_o=200;                                     % Range offset
delta_fo=5;                                      % Range rates offset

%a = -10:5:30;                                    % Measurements noise power in log-scale
a = -10:10:30;                                    % Measurements noise power in log-scale

randn('seed',123);                               % Initialize random number generator.
noise_rd = randn(4*M*N,L);                       % Generating measurement noise

for i=1:length(a)                                % Different noise

    fprintf('10log_10(nsePwr): %d \n',a(i));
    sigma = 10^(a(i)/10);                        % Measurement noise power

    sum_u = 0;sum_v = 0;sum_t = 0;sum_rank = 0;sum_rank_br = 0;
    sum_crb_u = 0;sum_crb_t = 0;
    sum_u_br = 0;sum_v_br = 0;sum_t_br = 0;

    MSE = zeros(3*K+2,3*K+2);

    for iii=1:cj

        uo_v = uv(:,iii);

        bias_sdp_u = zeros(3,1); bias_br_sdp_u = zeros(3,1);
        bias_sdp_v = zeros(3,1); bias_br_sdp_v = zeros(3,1);
        bias_sdp_t = zeros(3,1); bias_br_sdp_t = zeros(3,1);

        zo=[uo(:,iii)',uo_v',to(:,iii)',delta_o,delta_fo]';

        for j=1:1:N
            ro(j) = norm(uo(:,iii)-to(:,iii))+norm(uo(:,iii)-so(:,j,iii))+delta_o;     % true indirect ranges
            ro_v(j) = ((uo(:,iii)-so(:,j,iii))'/norm(uo(:,iii)-so(:,j,iii))+ ...
                (uo(:,iii)-to(:,iii))'/norm(uo(:,iii)-to(:,iii)))*uo_v+delta_fo; % true indirect range rates
            do(j) = norm(to(:,iii)-so(:,j,iii))+delta_o;                               % true direct ranges
            do_v(j) = delta_fo;                                                        % true direct range rates
        end

        for j=1:N
            qr(j) = ((ro(j))/sqrt(10^(-0.3)))^2;
            qd(j) = (do(j))^2;
        end

        Qr = diag(qr);
        Qd = diag(qd);
        Qo = blkdiag(Qr,Qd);
        qo = sqrt(diag(Qo));
        Q_tilde = 0.5*(Qo+qo*qo');

        k_q = 2*M*N*sigma/trace(Q_tilde);
        Q_rd = k_q*Q_tilde;                              % Covariance matrix of indirect and direct range noise
        Q_v = 0.1*Q_rd;                                  % Covariance matrix of indirect and direct range rate noise

        Q = blkdiag(Q_rd,Q_v);                           % Covariance matrix
        Q1 = (1/X)^2*Q;                                  % Covariance matrix after narrow scene

        noise_rd1 = noise_rd-mean(noise_rd,2)*ones(1,L);
        error_rd = sqrtm(Q)*noise_rd1;
        error_r = reshape(error_rd(1:M*N,:),M*N,L);
        error_d = reshape(error_rd(M*N+1:2*M*N,:),M*N,L);
        error_rv = reshape(error_rd(2*M*N+1:3*M*N,:),M*N,L);
        error_dv = reshape(error_rd(3*M*N+1:4*M*N,:),M*N,L);
        %
        parfor ii=1:1:L                                  % Monte Carlo number

            s = 1/X*so(:,1:N,iii);                   % Receiver position after narrow scene
            r = 1/X*(ro'+error_r(:,ii));             % Indirect range measurements after narrow scene
            d = 1/X*(do'+error_d(:,ii));             % Direct range measurements after narrow scene
            r_v = 1/X*(ro_v'+error_rv(:,ii));        % Indirect range rates measurements after narrow scene
            d_v = 1/X*(do_v'+error_dv(:,ii));        % Direct range rates measurements after narrow scene

            %-------------SDP--------------%
            Y = MSLocUknTxPosMvgObj_SDP(r,d,r_v,d_v,s,Q1);    	% SDP solution
            landa = eig(Y(1:3*K+2,1:3*K+2));         % Eigenvalue decomposition for Y(1:3*K+2,1:3*K+2)
            landa = sort(landa,'descend');           % The eigenvalues are sorted from large to small
            if(landa(1)/landa(2))>10^7
                rank_joint = 1;
                rank(ii) = rank_joint;
                sum_rank = sum_rank+rank_joint;      % Count the number of rank1 about SDP
            else
                rank_joint = 0;
                rank(ii) = rank_joint;
            end
            u_sdp_u(:,ii) = X*Y(1:K,3*K+8)/Y(3*K+8,3*K+8);             % Object position estimation about SDP
            uv_sdp_uv(:,ii) = X*Y(K+1:2*K,3*K+8)/Y(3*K+8,3*K+8);       % Object velocity estimation about SDP
            t_sdp_t(:,ii) = X*Y(2*K+1:3*K,3*K+8)/Y(3*K+8,3*K+8);       % Transmitter position estimation about SDP
            %                 delta_t(:,ii) = X*Y(3*K+1,3*K+8)/Y(3*K+8,3*K+8);
            %                 delta_dot(:,ii) = X*Y(3*K+1,3*K+8)/Y(3*K+8,3*K+8);
            sum_u = sum_u + (norm(u_sdp_u(:,ii)-uo(:,iii)))^2;
            sum_v = sum_v + (norm(uv_sdp_uv(:,ii)-uo_v))^2;
            sum_t = sum_t + (norm(t_sdp_t(:,ii)-to(:,iii)))^2;
            bias_sdp_u = bias_sdp_u + u_sdp_u(:,ii);
            bias_sdp_v = bias_sdp_v + uv_sdp_uv(:,ii);
            bias_sdp_t = bias_sdp_t + t_sdp_t(:,ii);
            %               
	    %------------BR-SDP-----------------%
            Y1 = MSLocUknTxPosMvgObj_BRSDP(r,d,r_v,d_v,s,Q1,1,Y);      % BR-SDP solution
            landa = eig(Y1(1:3*K+2,1:3*K+2));                          % Eigenvalue decomposition for Y1(1:3*K+2,1:3*K+2)
            landa = sort(landa,'descend');
            if(landa(1)/landa(2))>10^7
                rank_joint_br = 1;
                rank_br(ii) = rank_joint_br;
                sum_rank_br = sum_rank_br+rank_joint_br;               % Count the number of rank1 about BR-SDP
            else
                Y1 = MSLocUknTxPosMvgObj_BRSDP(r,d,r_v,d_v,s,Q1,2,Y);                     % BR-SDP solution with inequality constraint
                landa = eig(Y1(1:3*K+2,1:3*K+2));
                landa = sort(landa,'descend');
                if(landa(1)/landa(2))>10^7
                    rank_joint_br = 1;
                    rank_br(ii) = rank_joint_br;
                    sum_rank_br = sum_rank_br+rank_joint_br;
                else
                    rank_joint_br = 0;
                    rank_br(ii) = rank_joint_br;
                end
            end
            u_sdp_u_br(:,ii) = X*Y1(1:K,3*K+8)/Y1(3*K+8,3*K+8);        % Object position estimation about BR-SDP
            uv_sdp_uv_br(:,ii) = X*Y1(K+1:2*K,3*K+8)/Y1(3*K+8,3*K+8);  % Object velocity estimation about BR-SDP
            t_sdp_t_br(:,ii) = X*Y1(2*K+1:3*K,3*K+8)/Y1(3*K+8,3*K+8);  % Transmitter position estimation about BR-SDP
            %                 delta_t_br(:,ii) = X*Y1(3*K+1,3*K+8)/Y1(3*K+8,3*K+8);
            %                 delta_dot_br(:,ii) = X*Y1(3*K+2,3*K+8)/Y1(3*K+8,3*K+8);
            sum_u_br = sum_u_br + (norm(u_sdp_u_br(:,ii)-uo(:,iii)))^2;
            sum_v_br = sum_v_br + (norm(uv_sdp_uv_br(:,ii)-uo_v))^2;
            sum_t_br = sum_t_br + (norm(t_sdp_t_br(:,ii)-to(:,iii)))^2;
            bias_br_sdp_u = bias_br_sdp_u + u_sdp_u_br(:,ii);
            bias_br_sdp_v = bias_br_sdp_v + uv_sdp_uv_br(:,ii);
            bias_br_sdp_t = bias_br_sdp_t + t_sdp_t_br(:,ii);
        end

        mse(:,:,iii) = MSLocUknTxPosMvgObj_CRLB(to(:,iii),uo(:,iii),uo_v,so(:,1:N,iii),Q);     % CRLB
        theory_bias = MSLocUknTxPosMvgObj_BsThy(uo(:,iii),to(:,iii),uo_v,delta_o,delta_fo,so(:,1:N,iii),ro,do,ro_v,do_v,Q,mse(:,:,iii)); % theoretical bias
        MSE = MSE+mse(:,:,iii);

        Z_star_BS_u(iii) = norm(theory_bias(1:K),1);
        Z_star_BS_v(iii) = norm(theory_bias(K+1:2*K),1);
        Z_star_BS_t(iii) = norm(theory_bias(2*K+1:3*K),1);

        Bias_SDP_u(iii) = norm(bias_sdp_u/L - uo(:,iii),1);
        Bias_SDP_v(iii) = norm(bias_sdp_v/L - uo_v,1);
        Bias_SDP_t(iii) = norm(bias_sdp_t/L - to(:,iii),1);

        Bias_BR_SDP_u(iii) = norm(bias_br_sdp_u/L - uo(:,iii),1);
        Bias_BR_SDP_v(iii) = norm(bias_br_sdp_v/L - uo_v,1);
        Bias_BR_SDP_t(iii) = norm(bias_br_sdp_t/L - to(:,iii),1);
    end
    sumu_rank_total(i) = sum_rank;
    sumu_rank_br_total(i) = sum_rank_br;

    MSE_SDP_u(i) = sum_u/(cj*L);
    MSE_SDP_v(i) = sum_v/(cj*L);
    MSE_SDP_t(i) = sum_t/(cj*L);

    MSE_BR_SDP_u(i) = sum_u_br/(cj*L);
    MSE_BR_SDP_v(i) = sum_v_br/(cj*L);
    MSE_BR_SDP_t(i) = sum_t_br/(cj*L);

    BS_SDP_u(i) = mean(Bias_SDP_u);
    BS_SDP_v(i) = mean(Bias_SDP_v);
    BS_SDP_t(i) = mean(Bias_SDP_t);

    BS_BR_SDP_u(i) = mean(Bias_BR_SDP_u);
    BS_BR_SDP_v(i) = mean(Bias_BR_SDP_v);
    BS_BR_SDP_t(i) = mean(Bias_BR_SDP_t);

    BS_theory_u(i) = mean(Z_star_BS_u);
    BS_theory_v(i) = mean(Z_star_BS_v);
    BS_theory_t(i) = mean(Z_star_BS_t);

    MSE = MSE/cj;
    y_crb_u(i) = 10*log10(trace(MSE(1:K,1:K)));
    y_crb_v(i) = 10*log10(trace(MSE(K+1:2*K,K+1:2*K)));
    y_crb_t(i) = 10*log10(trace(MSE(2*K+1:3*K,2*K+1:3*K)));

end

figure(2);
subplot(3,1,1);
plot(a,10*log10(MSE_SDP_u),'r>');hold  on;
plot(a,10*log10(MSE_BR_SDP_u),'bd');hold on;
plot(a,y_crb_u,'-k','MarkerSize',8);hold on;
hold off;
grid on;
axis([-10 30 -5 75]);
xlabel('10lg(\sigma^2(m^2))'); ylabel('$\rm{\bf{{10lg(MSE (u)m^2 )}}}$','interpreter','latex','fontsize',12);
legend('SDP','BR-SDP','CRLB','location','northwest');

subplot(3,1,2);
plot(a,10*log10(MSE_SDP_v),'r>');hold  on;
plot(a,10*log10(MSE_BR_SDP_v),'bd');hold on;
plot(a,y_crb_v,'-k','MarkerSize',8);hold on;
hold off;
grid on;
axis([-10 30 -20 100]);
xlabel('10lg(\sigma^2(m^2))'); ylabel('$\rm{\bf{{10lg(MSE (\dot{u}) (m/s)^2 )}}}$','interpreter','latex','fontsize',12);
legend('SDP','BR-SDP','CRLB','location','northwest');

subplot(3,1,3);
plot(a,10*log10(MSE_SDP_t),'r>');hold  on;
plot(a,10*log10(MSE_BR_SDP_t),'bd');hold on;
plot(a,y_crb_t,'-k','MarkerSize',8);hold on;
hold off;
grid on;
axis([-10 30 0 60]);
xlabel('10lg(\sigma^2(m^2))'); ylabel('$\rm{\bf{{10lg(MSE (t)m^2 )}}}$','interpreter','latex','fontsize',12);
legend('SDP','BR-SDP','CRLB','location','northwest');

figure(3);
plot(a,20*log10(BS_SDP_u),'r>');hold on;
plot(a,20*log10(BS_BR_SDP_u),'bd');hold on;
plot(a,20*log10(BS_theory_u),'-k','MarkerSize',8);hold on;
hold off;
grid on; 
axis([-10 30 -65 35]);
xlabel('10lg(\sigma^2(m^2))');ylabel('$\rm{\bf{{20lg(Bias (u)(m) )}}}$','interpreter','latex','fontsize',12);
legend('SDP','BR-SDP','Theoretical','location','northwest');

figure(4);
plot(a,20*log10(BS_SDP_v),'r>');hold on;
plot(a,20*log10(BS_BR_SDP_v),'bd');hold on;
plot(a,20*log10(BS_theory_v),'-k','MarkerSize',8);hold on;
hold off;
grid on; 
axis([-10 30 -110 40]);
xlabel('10lg(\sigma^2(m^2))'); ylabel('$\rm{\bf{{20lg(Bias (\dot{u}) (m/s) )}}}$','interpreter','latex','fontsize',12);
legend('SDP','BR-SDP','Theoretical','location','northwest');

figure(5);
plot(a,20*log10(BS_SDP_t),'r>');hold on;
plot(a,20*log10(BS_BR_SDP_t),'bd');hold on;
plot(a,20*log10(BS_theory_t),'-k','MarkerSize',8);hold on;
hold off;
grid on; 
axis([-10 30 -60 40]);
xlabel('10lg(\sigma^2(m^2))');ylabel('$\rm{\bf{{20lg(Bias (t)(m) )}}}$','interpreter','latex','fontsize',12);
legend('SDP','BR-SDP','Theoretical','location','northwest');
