%
% DOA estimation
%
% 初期設定
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];      % 入力電力
S=diag(pw);             % 信号相関行列(無相関)

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

% 相関行列 R の計算
A=exp(-j*twpi*d.'*sin(theta*derad));
R=A*S*A'+diag(nv/snr_in);
InvR=inv(R);

% LP weight
T = zeros(kelm,1);
T(1)=1;
W=(InvR*T)/(T'*InvR*T);

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

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

L=iwave;
En=EV(:,L+1:kelm);

% Original Min-Norm Method
cv=En(1,:);
cv=cv/(cv*cv');
mn=En*cv';

% スペクトラムの計算
for method = 1:5
	for iang = 1:361
		angle(iang)=(iang-181)/2;
		phim=derad*angle(iang);
		a=exp(-j*twpi*d*sin(phim)).';
   
 	  	switch (method)
		case 1
			name='Beamformer';
			SP(iang)=(a'*R*a)/(a'*a);
		case 2
			name='Capon Beamformer';
			SP(iang)=1/(a'*InvR*a);
		case 3
			name='Linear Prediction';
			SP(iang)=1/abs(W'*a)^2;
   		case 4
			name='Min-Norm';
   			SP(iang)=(a'*a)/(a'*mn*mn'*a);
   		case 5
			name='MUSIC';
   			SP(iang)=(a'*a)/(a'*En*En'*a);
		otherwise
         	error('This is impossible')
         	break
		end
	end
   
% 正規化＆dB表現
	SP=abs(SP);
	SPmax=max(SP);
	SP=10*log10(SP/SPmax);

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