%%%
%%% DOA estimation by MUSIC and Root-MUSIC(main.m)
%%%
% 初期設定
clear all

% 入力パラメータ設定
derad = pi/180; % deg -> rad
radeg = 180/pi;
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 = 20;                 % スナップショット数

% 信号相関係数行列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));

% 波の数設定（正確な数）と雑音部分空間の抽出
L=iwave;     
En=EV(:,L+1:kelm);

% MUSICスペクトラムの計算
angle=-90:0.5:90;
A=exp(-j*twpi*d.'*sin(derad*angle));

for iang = 1:361
	a = A(:,iang);
      	SP(iang)=(a'*a)/(a'*En*En'*a);
end

% 正規化 & dB表現
SP=abs(SP);
SPmax=max(SP);
SP=10*log10(SP/SPmax);

% グラフ表示
SPmin = min(SP)*1.05;
h=plot(angle,SP);
set(h,'Linewidth',2)
xlabel('angle (degree)')
ylabel('magnitude (dB)')
axis([-90 90 -60 0])
set(gca, 'XTick',[-90:30:90])
grid on   


%%Root-MUSIC
% Root-MUSIC多項式の係数ベクトルの計算
Vc = zeros(2*kelm,1);
for jj = 1:kelm-L
        for k=1:kelm
        Vc(k:kelm+k-1,1)=Vc(k:kelm+k-1,1)+...
                         conj(EV(:,L+jj))*EV(kelm-k+1,L+jj);
        end
end

% 多項式の解を求める
z=roots(flipud(Vc));
zamp=abs(z)
zphi=atan2(imag(z), real(z));

% 推定角度の表示
ange = - asin( zphi / twpi / dd ) * radeg
% 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
