%
% DOA estimation with exact Rxx (main.m)
%
% 初期設定
clear all

% 入力パラメータ設定

derad = pi/180;         % deg -> rad
twpi = 2*pi;

K = 6;                  % 素子数(リニアアレー)
dd = 0.5;               % 素子間隔(*λ)
d=0:dd:(K-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,K);           % 正規化雑音電力
snr_in = 20;            % input SNR (dB)
snr_in= 10^(snr_in/10);

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

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

% スペクトラムの計算

figure
for method = 1:3
  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';
        T = zeros(K,1);
        T(1)=1;
        W=(InvR*T)/(T'*InvR*T);
        SP(iang)=1/abs(W'*a)^2;
      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,2,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
% 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