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

% 入力パラメータ設定
twpi = 2*pi;

M = 11;         % 全アレーデータ数
K = 9;          % サブアレーデータ数
Df = 300;       % 周波数掃引幅(MHz)
fc = 2000;      % 中心周波数(MHz)
frq = linspace(fc-Df/2,fc+Df/2,M);      % 測定周波数(MHz)
ddf = frq(2) - frq(1);                  % 周波数間隔(MHz)
taumax = 1000/ddf                       % 推定可能最大遅延時間(ns)

iwave = 3;              % 到来波数
tau = [12 15 20];       % 遅延時間(ns)
pw = [1.0 1.0 0.5];     % 入力電力
cb = [ 1 0 0
       1 0 0
       1 0 0];          % source relation 

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

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

for itr=1:3             % 3回試行
  % 相関行列 Rxx の計算
  %
  Rxxm = cor_est(n,snr0,tau,pw,frq,nv,cb);

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

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

  % MUSICスペクトラムの計算
  
  for itau = 1:61
    delay(itau)=(itau-1)/2;
    a=exp(-j*twpi*frq(1:K)*delay(itau)/1000).';
    L=iwave;     % 波の数は正確な数を代入
    En=EV(:,L+1:K);
    SP(itau)=(a'*a)/(a'*En*En'*a);
  end
  
  % 正規化 & dB表現
  SP=abs(SP);
  SPmax=max(SP);
  SP=10*log10(SP/SPmax);
  
  % グラフ表示
  h=plot(delay,SP);
  set(h,'Linewidth',2)
  xlabel('delay time (ns)')
  ylabel('P_{MU} (dB)')
  axis([0 30 -40 0]);
  set(gca, 'XTick',[0:5:30], 'YTick',[-40:10:0])
  grid on   
  hold on
end
% End main

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

%%
%%
function cx=make_Xin(snr0,tau,pw,frq,cb,nv)
%
twpi = 2.0 * pi;
%
% generate gaussian random variables for signals and noises
%
K=length(frq);
L=length(tau);

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*frq.'*tau/1000)*cs.';

% add noise components
%
cx = cx + diag(sqrt(nv/snr0/2))*(randn(K,1)+j*randn(K,1));
%End make_Xin

%%
%%
function crs = ssp(cr, K)
%
% Spatial Smoothing Processing
%
[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

%%
%%
function crs = mssp(cr, K)
%
% Modified Spatial Smoothing Processing
%
[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
