%%%
%%% DOA estimation by TLS-ESPRIT (main.m)
%%%
% 初期設定
clear all

% 入力パラメータ設定
derad = pi/180;         % deg -> rad
twpi = 2*pi;

kelm = 6;               % 素子数(リニアアレー)
dd = 0.5;               % 素子間隔(*λ)
d=0:dd:(kelm-1)*dd;     % 素子位置

iwave = 3;              % 到来波数
theta = [0 10 60];      % 到来角
pw= [1.0 1.0 0.5];      % 入力電力
cb = [ 1 0 0
       0 1 0
       0 0 1];          % source relation 

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

n = 100;                % スナップショット数

% 信号相関係数行列corの生成 & cb行列の正規化
%
[cor,cb] = cor_trans(cb);

% 相関行列 Rxx の計算
Rxx = cor_est(n,snr_in,theta,pw,d,nv,cb);

% Rxx の固有値展開
[EV,D]=eig(Rxx);

% 固有値降順ソーティング
EVA=diag(D)'; [EVA,I]=sort(EVA);
EVA=fliplr(EVA), EV=fliplr(EV(:,I));

% TLS-ESPRIT
L=iwave;                % 推定到来波数
estimates=tls_esprit(dd,Rxx,L);
disp('TLS-ESPRIT')
disp('angles'),disp(estimates(1,:))
disp('power '),disp(estimates(2,:))
% End main.m

%%%--------------------------------------------
%%%
%%% cor_trans.m
%%%
function [cor,cb]=cor_trans(cb)
%************************************************************
% This function calculates correlation coefficients
%************************************************************
%
% normalization of cb
%
[L LL] = size(cb);
for iL = 1:L
	wa=sum(abs(cb(iL,1:iL)).^2);
	cb(iL,1:iL)=cb(iL,1:iL)/sqrt(wa);
	cb(iL,iL) = abs(cb(iL,iL));
end
%
% calculate correlation coefficients
%
cor=eye(L);
for jL = 1:L
	for iL = 1:jL-1
		cor(iL,jL)=cb(iL,1:iL)*cb(jL,1:iL)';
		cor(jL,iL)=conj(cor(iL,jL));
	end
end
%End cor_trans.m

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

%%%--------------------------------------------
%%%
%%% make_Xin.m
%%%
function cx=make_Xin(snr0,theta,pw,d,cb,nv)
%
twpi = 2.0 * pi;
derad = pi / 180.0;
%
% generate gaussian random variables for signals and noises
%
K=length(d);
L=length(theta);

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*d.'*sin(theta*derad))*cs.';
%
% add noise components
%
cx = cx + diag(sqrt(nv/snr0/2))*(randn(K,1)+j*randn(K,1));
%End make_Xin.m

%%%--------------------------------------------
%%%
%%% tls_esprit.m
%%%
function estimate =  tls_esprit(dd,cr,Le)
%*******************************************************
% This function calculates TLS-ESPRIT estimator for
% uniform linear array.
%
% Inputs
%    dd       sensor separation in wavelength
%    cr(K,K)  array output covariance matrix
%    Le       estimated number of sources
%
% Output
%    estimate  estimated angles in degrees
%              estimated powers
%*******************************************************

twpi = 2.0*pi;
derad = pi / 180.0;
radeg = 180.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));
 
% DOA estimates
ephi = atan2(imag(EGS), real(EGS));
ange = - asin( ephi / twpi / dd ) * radeg;
estimate(1,:)=ange;

% 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.m
