% TMA in 3-D using DSF measurements.
% Performance comparison by using
% - closed form solution: DSF3DTMA_CFS.m
% - SDP solution: DSF3DTMA_SDP.m
% - MLE using CFS for initialization: DSF3DTMA_CFS_MLE.m
%
% The code can reproduce the results in Fig. 4 of the reference paper.
%
% Number of ensemble runs (NumEns) is set to 5 and number of geometries (no_of_geo)
% is set to 1 to reduce run time of the code.
%
% Reference: M. H. Ahmed, K. C. Ho, and G. Wang, "3-D target localization
% and motion analysis based on Doppler shifted frequencies," IEEE Trans.
% Aerosp. Electron. Syst., vol. 58, no. 2, pp. 815-833, Apr. 2022.
%
% M. H. Ahmed, K. C. Ho     10-28-2022
%
%       Copyright (C) 2022
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA.
%       hod@missouri.edu
%

clear all;
warning off;
clc;

rng(4165);

% ----- setttings -----
no_of_geo=1; %10;       % number of geometries
NumEns=5;  %1e3         % number of ensemble runs
sigma2_dsf=-60:5:-10;    % 10*log10(dsf measurement noise power)
sigma2_fc=-100;         % 10*log10(center freq. noise power)
sigma2_s=-100;          % 10*log10(sensor position noise power)
M=10;               % number of sensors
T=1;                % measurement sample period
N=35;               % number of measurement sets
fc_o=15e3;          % true carrier frequency
c=1500;             % signal propagation speed
min_dis=400;        % minimum distance between two sensors

% -- weighting matrix choices --
wghIdc=1;
%  wghIdc=0   No weighting matrix
%  wghIdc=1   weighting matrix for measurement errors only
%  wghIdc=2   weighting matrix for measurement and sensor pos. errors
%  wghIdc=3   weighting matrix for measurement and source freq. errors
%  otherwise  Full weighting matrix

% -- initialization --
[SS,uu,uu_dot]=sen_pos(M,N,T,no_of_geo,min_dis);    % localization geometries

R=eye(M*N);         % dsf covariance matrix structure
Rs=eye(3*M);        % sensor position covariance matrix structure

EE_n=randn(M*N,NumEns);   % dsf noise sequence
EE_s=randn(3*M,NumEns);   % sensor position noise sequence
EE_fc=randn(1,NumEns);    % center frequency noise sequence

%==========================================================================
%   Simulation Start
%==========================================================================
for nseIdx=1:length(sigma2_dsf) % loop through dsf measurement noise power
    sigma2_n=sigma2_dsf(nseIdx);

    Qn = (10^(sigma2_n/10)*R);     % covariance matrix of measurements.
    chol_Qn = chol(Qn);
    Delta = chol_Qn'*EE_n;      % measurement error vector
    Qfc=10^(sigma2_fc/10);      % variance of center frequency
    chol_Qf = chol(Qfc);
    Delta_fc=chol_Qf*EE_fc;     % center frequency error
    Qs =10^(sigma2_s/10)*Rs;    % covariance matrix of receiver positions.
    chol_Qs = chol(Qs);
    Delta_s = chol_Qs'*EE_s;    % receiver error vector

    for geo=1:no_of_geo

        S_o=SS(:,:,geo);        % true sensor positions
        u=uu(:,geo);            % true object position
        u_dot=uu_dot(:,geo);    % true object velocity
        u_all=u*ones(1,N)+u_dot*(0:N-1)*T;  % true object positons over entire track

        % -- true DSFs --
        d=sqrt(sum((repelem(u_all',M,1)-repmat(S_o,N,1)).^2,2));
        f_o=fc_o-fc_o/c*(((repelem(u_all',M,1)-repmat(S_o,N,1))./repmat(d,1,3))*u_dot);

        % -- CRLB --
        CRLB=DSF3DTMA_CRLB(u,u_dot,fc_o,S_o',N,c,T,Qn,Qfc,Qs);
        CRLB=diag(CRLB);
        CRLB_p(geo,nseIdx)=sum(CRLB(1:3));  % object position CRLB
        CRLB_v(geo,nseIdx)=sum(CRLB(4:6));  % object velocity CRLB

        %==========================================================================
        for sol_method=1:3,     % CFS, SDP and CFS-MLE

            for loop=1:NumEns,    % ensemble runs

                % -- adding noise to generate measurements --
                f=f_o+Delta(:,loop);    % dsf measurements
                fc=fc_o+Delta_fc(loop); % noisy center frequency
                S=S_o+reshape(Delta_s(:,loop),M,3); % noisy sensor positions
                S=S';

                % -- method --
                switch sol_method
                    case 1  % CFS solution
                        theta=DSF3DTMA_CFS(f,fc,S,N,c,T,Qn,Qfc,Qs,wghIdc);
                    case 2  % SDP solution
                        theta=DSF3DTMA_SDP(f,fc,S,N,c,T,Qn,Qfc,Qs,wghIdc);
                    case 3  % CFS-MLE solution
                        theta=DSF3DTMA_CFS_MLE(f,fc,S,N,c,T,Qn,Qfc,Qs,wghIdc);
                    otherwise
                        theta=zeros(6,1);
                end

                % -- errors in object position and velocity estimates --
                Ep(loop,geo,nseIdx,sol_method)=sum((u-theta(1:3)).^2);
                Ev(loop,geo,nseIdx,sol_method)=sum((u_dot-theta(4:6)).^2);
            end

        end
        clc;
        fprintf('Processing Completed %3.2d%% ',...
            round(((nseIdx-1)*no_of_geo+geo)*100/(no_of_geo*length(sigma2_dsf))));
    end

end

fprintf('\n');

% -- plot results --
clor =[0, 114, 189;247, 13, 115;237, 177, 32;126, 47, 142;119, 172, 48;163,20,47]/256;

figure;
set(gcf,'PaperPosition',[0,0,5.7,6])
set(gcf,'PaperSize',[5.7,6])

subplot(2,1,1)
hold on
ep=Ep(:,:,:,1);mse=nanmean(ep,[1 2]);mse=mse(:);    % positon mse for CFS
plot(sigma2_dsf, 10*log10(mse), '-*', 'LineWidth', 1.5, 'DisplayName', 'CFS','Color',clor(1,:));
ep=Ep(:,:,:,2);mse=nanmean(ep,[1 2]);mse=mse(:);    % position mse for SDP
plot(sigma2_dsf, 10*log10(mse), '-o', 'LineWidth', 1.5, 'DisplayName', 'SDP','Color',clor(2,:));
ep=Ep(:,:,:,3);mse=nanmean(ep,[1 2]);mse=mse(:);    % position mse for CFS_MLE
plot(sigma2_dsf, 10*log10(mse), '--', 'LineWidth', 1.5, 'DisplayName', 'CFS-ML','Color',clor(6,:));
CRLB_p_avg=mean(CRLB_p,1);      % position CRLB
plot(sigma2_dsf, 10*log10(CRLB_p_avg), '-^', 'LineWidth', 1.5, 'DisplayName', 'CRLB','Color',clor(5,:));
xlabel('20 log($\sigma$(Hz))', 'FontSize', 10,'Interpreter','latex');
ylabel('10 log(MSE($\mathbf{p}$(m)))', 'FontSize', 10,'Interpreter','latex');
axis([sigma2_dsf(1) sigma2_dsf(end) -50 100]);
lgd = legend('Show');
set(lgd, 'FontSize',8, 'Location', 'northwest');
grid on;box on
hold off

subplot(2,1,2)
hold on
ev=Ev(:,:,:,1);mse=nanmean(ev,[1 2]);mse=mse(:);    % velocity mse for CFS
plot(sigma2_dsf, 10*log10(mse), '-*', 'LineWidth', 1.5, 'DisplayName', 'CFS','Color',clor(1,:));
ev=Ev(:,:,:,2);mse=nanmean(ev,[1 2]);mse=mse(:);    % velocity mse for SDP
plot(sigma2_dsf, 10*log10(mse), '-o', 'LineWidth', 1.5, 'DisplayName', 'SDP','Color',clor(2,:));
ev=Ev(:,:,:,3);mse=nanmean(ev,[1 2]);mse=mse(:);    % velocity mse for CFS_MLE
plot(sigma2_dsf, 10*log10(mse), '--', 'LineWidth', 1.5, 'DisplayName', 'CFS-ML','Color',clor(6,:));
CRLB_v_avg=mean(CRLB_v,1);      % velocity CRLB
plot(sigma2_dsf, 10*log10(CRLB_v_avg), '-^', 'LineWidth', 1.5, 'DisplayName', 'CRLB','Color',clor(5,:));
xlabel('20 log($\sigma$(Hz))', 'FontSize', 10,'Interpreter','latex');
ylabel('10 log(MSE($\mathbf{v}$(m/s)))', 'FontSize', 10,'Interpreter','latex');
axis([sigma2_dsf(1) sigma2_dsf(end) -100 20]);
lgd = legend('Show');
set(lgd, 'FontSize',8, 'Location', 'northwest');
grid on;box on
hold off


% =========================================================================
function [SS,uu,uu_dot]=sen_pos(M,N,T,no_of_geo,min_dis)
% [SS,uu,uu_dot]=sen_pos(M,N,T,no_of_geo,min_dis)
%
% Generation of localization geometries
%
% Input
% M:            number of sensors
% N:            number of time measurements
% T:            time interval between measurements
% no_of_geo:    number of geometries
% min_dis:      minimum distance between two sensors
%
% Output
% SS:           sensor positions
% uu:           object position
% uu_dot:       object velocity
%

M1=floor(M/2);
SS=zeros(M,3,no_of_geo);
uu=zeros(3,no_of_geo);
uu_dot=zeros(3,no_of_geo);
for l=1:no_of_geo
    a=0;
    while a<min_dis
        S(:,1:2)=-1000+2000*rand(M,2);
        S(1:M1,3)=100*rand(M1,1);
        S(M1+1:M,3)=900+100*rand(M-M1,1);

        a=sqrt((S(:,1)-S(:,1)').^2+(S(:,2)-S(:,2)').^2+(S(:,3)-S(:,3)').^2)+1/eps*eye(M);a=min(a(:));
    end
    SS(:,:,l)=S;
    uu(1:2,l)=-1500+3000*rand(2,1);
    uu(3,l)=1000*rand;
    u_end=1e5*[1;1;1];
    while abs(u_end(1))>1500||abs(u_end(2))>1500||abs(u_end(3))>1000
        theta_ang=rand*359;
        phi_ang=rand*179;
        uu_dot(:,l)=10*[-cosd(theta_ang)*cosd(phi_ang);-cosd(theta_ang)*sind(phi_ang);cosd(phi_ang)];
        u_end=uu(:,l)+uu_dot(:,l)*(N-1)*T;
    end

end

end

