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

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

Melm = 7;               % 素子数(全アレー)
kelm = 6;               % 素子数(サブアレー)
dd = 0.5;               % 素子間隔(*λ)
d=0:dd:(Melm-1)*dd;     % 素子位置

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

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

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

for ic=1:3
% 相関行列 Rxx の計算
%
Rxxm = cor_est(n,snr0,theta,pw,d,nv,cb);

if issp == 1
  Rxx = ssp(Rxxm,kelm);
elseif issp == 2
  Rxx = mssp(Rxxm,kelm);
else
  Rxx = Rxxm;
  kelm = Melm;
end
  
% Rxx の固有値展開
[EV,D]=eig(Rxx);

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

% MUSICスペクトラムの計算

for iang = 1:361
        angle(iang)=(iang-181)/2;
        phim=derad*angle(iang);
        a=exp(-j*twpi*d(1:kelm)*sin(phim)).';
        L=iwave;     % 波の数は正確な数を代入
        En=EV(:,L+1:kelm);
        SP(iang)=(a'*a)/(a'*En*En'*a);
end
   
% 正規化 & dB表現
SP=abs(SP);
SPmax=max(SP);
SP=10*log10(SP/SPmax);

% グラフ表示
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], 'YTick',[-60:10:0])
grid on   
hold on
end
% End main.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

%%%
%%% ssp.m
%%%
function crs = ssp(cr, K)
% spatial smoothing 
[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.m

%%%
%%% mssp.m
%%%
function crs = mssp(cr, K)
% modified spatial smoothing
[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.m
