%%
%% TOA estimation by TLS-ESPRIT
%%
function main
% 初期設定
clear all

% 入力パラメータ設定
twpi = 2*pi;

M = 11;         % 全アレーデータ数
K = 9;          % サブアレーデータ数
Df = 300;       % 周波数掃引幅(MHz)
fc = 2000;      % 中心周波数(MHz)
frq = linspace(fc-Df/2,fc+Df/2,M);      % 測定周波数(MHz)
ddf = frq(2) - frq(1);                  % 周波数間隔(MHz)
taumax = 1000/ddf                       % 推定可能最大遅延時間(ns)

iwave = 3;              % 到来波数
tau = [12 15 20];       % 遅延時間(ns)
pw = [1.0 1.0 0.5];     % 入力電力
cb = [ 1 0 0
       1 0 0
       1 0 0];          % source relation 

nv = ones(1,M);         % normalized noise variance
snr0 = 20;              % input SNR (dB)
snr0 = 10^(snr0/10);

n = 100;                % スナップショット数
issp = 1;               % 空間平均(0:なし, 1:Forward-only, 2:F/B)

for itr=1:3             % 3回試行
  % 相関行列 Rxx の計算
  %
  Rxxm = cor_est(n,snr0,tau,pw,frq,nv,cb);
  
  if issp == 1
    Rxx = ssp(Rxxm,K);
  elseif issp == 2
    Rxx = mssp(Rxxm,K);
  else
    Rxx = Rxxm;
    K = M;
  end
  
  % TLS-ESPRIT
  L=iwave;      % 推定到来波数
  estimates=tls_esprit(ddf,Rxx,L);
  disp('TLS-ESPRIT')
  disp('tau   :'),disp(estimates(1,:))
  disp('power :'),disp(estimates(2,:))
end
% End main

%%
%%
function cr=cor_est(n,snr0,tau,pw,frq,nv,cb)
%
K=length(frq);
cr=zeros(K,K);
for in=1:n
  cx = make_Xin(snr0,tau,pw,frq,cb,nv);
  cr = cr + cx*cx';
end
cr = cr / n;
% End cor_est

%%
%%
function cx=make_Xin(snr0,tau,pw,frq,cb,nv)
%
twpi = 2.0 * pi;
%
% generate gaussian random variables for signals and noises
%
K=length(frq);
L=length(tau);

ct = (randn(1,L)+j*randn(1,L))/sqrt(2);

% implement source correlations
%
for iL = 1:L
  cs(iL) = sqrt(pw(iL))*ct(1:iL)*cb(iL,1:iL).';
end

% generate sensor outputs
%
cx = exp(-j*twpi*frq.'*tau/1000)*cs.';

% add noise components
%
cx = cx + diag(sqrt(nv/snr0/2))*(randn(K,1)+j*randn(K,1));
% End make_Xin

%%
%%
function crs = ssp(cr, K)
%
% Spatial Smoothing Processing
%
[M,MM]=size(cr);
N=M-K+1;
crs = zeros(K,K);
for  in =1:N
  crs = crs + cr(in:in+K-1,in:in+K-1);
end
crs = crs / N;
% End ssp

%%
%%
function crs = mssp(cr, K)
%
% Modified Spatial Smoothing Processing
%
[M,MM]=size(cr);
N=M-K+1;
J = fliplr(eye(M));
crfb = (cr + J*cr.'*J)/2;
crs = zeros(K,K);
for  in =1:N
  crs = crs + crfb(in:in+K-1,in:in+K-1);
end
crs = crs / N;
% End mssp

%%
%%
function estimate =  tls_esprit(ddf,cr,Le)
%*******************************************************
% This function calculates TLS-ESPRIT estimator for
% uniform frequency linear array.
%
% Inputs
%    ddf      frequency separation in MHz
%    cr(K,K)  array output covariance matrix
%    Le       estimated number of sources
%
% Output
%    estimate  estimated delay times in ns
%              estimated powers
%*******************************************************

twpi = 2.0*pi;

% eigen decomposition of cr
[K,KK] = size(cr);
[V,D]=eig(cr);
EVA = real(diag(D)');
[EVA,I] = sort(EVA);
disp('Eigenvalues of correlation matrix in TLS-ESPRIT:')
EVA=fliplr(EVA)
EV=fliplr(V(:,I));

%  composition of E_{xy} and E_{xy}^H E_{xy} = E_xys
Exy = [EV(1:K-1,1:Le) EV(2:K,1:Le)];
E_xys = Exy'*Exy;

% eigen decomposition of E_xys
[V,D]=eig(E_xys);
EVA_xys = real(diag(D)');
[EVA_xys,I] = sort(EVA_xys);
EVA_xys=fliplr(EVA_xys);
EV_xys=fliplr(V(:,I));

% decomposition of eigenvectors
Gx = EV_xys(1:Le,Le+1:Le*2);
Gy = EV_xys(Le+1:Le*2,Le+1:Le*2);

% calculation of  Psi = - Gx [Gy]^{-1}
Psi = - Gx/Gy;

% eigen decomposition of Psi
[V,D]=eig(Psi);
EGS = diag(D).';
[EGS,I] = sort(EGS);
EGS=fliplr(EGS);
EVS=fliplr(V(:,I));

% TOA estimates
ephi = atan2(imag(EGS), real(EGS));
for iL=1:Le
  if ephi(iL) > 0
    ephi(iL) = ephi(iL) - twpi;
  end
end
taue = - ephi / twpi / ddf * 1000;
estimate(1,:)=taue;

% power estimates
T = inv(EVS);
powe = T*diag(EVA(1:Le) - EVA(K))*T';
powe = abs(diag(powe).')/K;
estimate(2,:)=powe;
% End tls-esprit
