%%
%% 例3.2  
%% 図3.6 RLSアルゴリズムによる2乗誤差の変化
%%

% 初期設定
clear all

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

M=100;                      % イタレーション数
ndata=M+1;                  % データ数

kelm=2;                              % 素子数
dspc=0.5;                            % 素子間隔(*λ)
dx=linspace(0,dspc*(kelm-1),kelm);   % 素子位置

theta = [30,  -60]*derad;    % 所望波（第一成分），不要波の到来角
Pw    = [1.0, 1.0];          % 所望波（第一成分），不要波の入力電力
Pn=0.01;                     % 雑音電力
L=length(theta);             % % 所望波を含む到来波数
             
V=exp(-j*twpi*dx.'*sin(theta));
% V = [Vs, Vu1, Vu2,...] 所望波，不要波のアレー応答ベクトルから成る行列

rand('state',0)                 
randn('state',0)

for iL=1: L
    pha=rand(1,ndata)*twpi;
    sv(iL,:) = sqrt(Pw(iL))*exp(j*pha);
end

% 入力ベクトルのサンプル値：データ行列 (kelm x ndata)
X=V*sv+sqrt(Pn/2)*(randn(kelm,ndata)+j*randn(kelm,ndata));

delta=1;       % 相関行列の初期値（擬似雑音電力）

for alpha=0.9:0.1:1     % 忘却係数

% ウエイトの計算：RLSアルゴリズム
W(:,1)=[1 zeros(1,kelm-1)].';         % 初期ウエイト

    for m=0:M
        mp = m+1;
        if m == 0
            InvRxx = eye(kelm)/delta;
        else
            RX= InvRxx*X(:,m);
            InvRxx=(1/alpha)*InvRxx-RX*RX'/(alpha^2+alpha*X(:,m)'*RX);
            InvRxx = (InvRxx+InvRxx')/2;  % エルミート化
        end
        gamma=1/(alpha+X(:,m+1)'*InvRxx*X(:,m+1));
        e(m+1)=sv(1,m+1)-W(:,m+1)'*X(:,m+1);
        W(:,mp+1)=W(:,mp)+gamma*InvRxx*X(:,m+1)*conj(e(m+1));
     end    

% 相関行列の理論値計算
Rxx=V*diag(Pw)*V'+Pn*eye(kelm);

% 相関ベクトルの理論値計算
rxr=Pw(1)*exp(-j*twpi*dx.'*sin(theta(1)));

% 2乗誤差の期待値の計算
    for m=1:M+1
        n=round((alpha-0.9)/0.1+1);  % alpha=0.9:0.1:1のカウンタ
        E(n,m)=Pw(1)-W(:,m).'*conj(rxr)-W(:,m)'*rxr+W(:,m)'*Rxx*W(:,m);
    end
    
end

% グラフプロット
semilogy(0:M,abs(E))
axis([0 M 0.001 10])
xlabel('イタレーション回数');
ylabel('E[|e(m)|^2]');
title('RLSアルゴリズムによる2乗誤差の変化')
h=legend('\alpha=0.9','\alpha=1');
grid on
hold on