%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 適応フィルタ (LMSアルゴリズム)
%
%                                                      2003.01.16 Katto Lab
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear all;

% 未知システムのパラメータ
%% w_unknown(n): 未知システムのインパルス応答
w_unknown = [1.0 0.2790 -1.4488 -1.0700 0.4678 0.8100 -0.2034 0.4532 0.2356 -0.3456 -0.0123];
% w_unknown = [1 0.2790 -1.4488 -1.0700 0.4678 0.8100];
% w_unknown = [1.2 0.5];
len = length(w_unknown);

% 未知システムへの入力、未知システムからの出力
%% N: 信号数
%% x(n): 未知システム入力
%% d(n): 未知システム出力 (教師信号)

%% input (1) Random Noise
N = 1000;
x = zeros(N,1);
for n=1:N
    x(n) = randn;
end;

%% input (2) Chirp with delayed signal
%t = 0:1/(N-1):1;
%sigma = 0.5;
%sig = chirp(t,220,5,880)';
%noise = randn(size(sig));
%x = sig + sigma*noise;
%delay = 10;
%for n=1:N-delay
%    tmp(n+delay) = x(n) + 0.1*x(n+delay);
%end;
%for n=delay:N
%    x(n) = tmp(n);
%end;

d = conv(w_unknown,x);


% Wiener-Hoff Equation (1) 行列計算
%% Wiener-Hoff equation: w = inv(R)*p
%% R: 入力信号 x(n) の自己相関行列
%% p(k): 教師信号 d(n) と入力信号 x(n) の相互相関ベクトル
%% w0(k): 推定されたインパルス応答
[X,R] = corrmtx(x,len-1);
p = zeros(len,1);
for n=len:N
    for k=1:len
        p(k) = p(k) + d(n)*x(n-k+1);
    end;
end;
p = p / (N-len+1);
w0 = inv(R)*p;
figure(1);
plot([1:len], w_unknown, [1:len], w0);


% Wiener-Hoff Equation (2) 最急降下法 (Steepest descent method)
%% Wiener-Hoff Equation: w(m+1) = (I-2*myu*R) w(m) + 2*myu*p
%% loop: ループ回数
%% myu1: 定数
%% w1(k): 推定されたインパルス応答
loop = 500;
myu1 = 0.01;
I = eye(len,len);
w1 = zeros(len,loop);
for k=1:len
%    w1(k,1) = 1.0;
    w1(k,1) = 0.5 + 0.5 * randn;
end;
for m=1:loop-1
    w1(:,m+1) = (I - 2.0 * myu1 * R) * w1(:,m) + 2.0 * myu1 * p;
end;
figure(2);
plot([1:loop], w1(1,:), [1:loop], w1(2,:));


% LMS アルゴリズム
%% LMSアルゴリズム: w(n+1) = w(n) + 2.0 * myu * e(n) * x(n)
%% myu2: 定数
%% w2(k): 適応フィルタのインパルス応答
%% y2(n): 適応フィルタ出力
%% e2(n): 予測誤差 = d(n) - y2(n)
myu2 = 0.01;
w2 = zeros(len,N);
for k=1:len
%    w2(k,1) = 1.0;
    w2(k,1) = 0.5 + 0.5 * randn;
end;
y2 = zeros(N,1);
e2 = zeros(N,1);
for n=1:N-len
    y2(n) = 0;
    x_inv = zeros(len,1);
    for k=1:len
        if (n-k+1) > 0 
            x_inv(k) = x(n-k+1);
            y2(n) = y2(n) + w2(k,n) * x_inv(k);
        end;
    end;
    e2(n) = d(n) - y2(n);
    w2(:,n+1) = w2(:,n) + 2.0 * myu2 * e2(n) * x_inv;
end;
figure(3);
gr_max = N-len;
plot([1:gr_max], w2(1,1:gr_max), [1:gr_max], w2(2,1:gr_max));


% 正規化 LMS アルゴリズム
%% 正規化LMSアルゴリズム: w(n+1) = w(n) + 2.0 * myu * e(n) * x(n) / E[x(n)^{2}]
%% myu3: 定数
%% w3(k): 適応フィルタのインパルス応答
%% y3(n): 適応フィルタ出力
%% e3(n): 予測誤差 = d(n) - y3(n)
myu3 = 0.01;
w3 = zeros(len,N);
for k=1:len
%    w3(k,1) = 1.0;
    w3(k,1) = 0.5 + 0.5 * randn;
end;
y3 = zeros(N,1);
e3 = zeros(N,1);
x_inv = zeros(len,1);
x_pow = 0.0;
for n=1:N
    y3(n) = 0;
    x_inv = zeros(len,1);
    for k=1:len
        if (n-k+1) > 0 
            x_inv(k) = x(n-k+1);
            y3(n) = y3(n) + w3(k,n) * x_inv(k);
        end;
    end;
    e3(n) = d(n) - y3(n);
    x_pow = x_pow + x(n)*x(n);
    x_pow_normalized = x_pow / n;
    w3(:,n+1) = w3(:,n) + 2.0 * myu3 * e3(n) * x_inv / x_pow_normalized;
end;
figure(4);
gr_max = N-len;
plot([1:gr_max], w3(1,1:gr_max), [1:gr_max], w3(2,1:gr_max));


% 慣性項付き LMS アルゴリズム
%% 慣性項付きLMSアルゴリズム: w(n+1) = w(n) + 2.0 * myu * e(n) * x(n) 
%%                                          + alpha * (w(n) - w(n-1))
%% myu4: 定数
%% w4(k): 適応フィルタのインパルス応答
%% y4(n): 適応フィルタ出力
%% e4(n): 予測誤差 = d(n) - y4(n)
myu4 = 0.01;
alpha = 0.1;
w4 = zeros(len,N);
for k=1:len
%    w4(k,1) = 1.0;
    w4(k,1) = 0.5 + 0.5 * randn;
end;
y4 = zeros(N,1);
e4 = zeros(N,1);
x_inv = zeros(len,1);
for n=1:N
    y4(n) = 0;
    x_inv = zeros(len,1);
    for k=1:len
        if (n-k+1) > 0 
            x_inv(k) = x(n-k+1);
            y4(n) = y4(n) + w4(k,n) * x_inv(k);
        end;
    end;
    e4(n) = d(n) - y4(n);
    if (n == 1)
        w4(:,n+1) = w4(:,n) + 2.0 * myu4 * e4(n) * x_inv;
    else
        w4(:,n+1) = w4(:,n) + 2.0 * myu4 * e4(n) * x_inv + alpha * (w4(:,n) - w4(:,n-1));
    end;
end;
figure(5);
gr_max = N-len;
plot([1:gr_max], w4(1,1:gr_max), [1:gr_max], w4(2,1:gr_max));


% 最小二乗法
%% 最小二乗法: w = (X^{t}X)^{-1}X^{t}d
%% w5(k): 適応フィルタのインパルス応答
%% y5(n): 適応フィルタ出力
%% e5(n): 予測誤差 = d(n) - y5(n)
w5 = zeros(len,N);
y5 = zeros(N,1);
e5 = zeros(N,1);
for n=1:N-len
    m = n+len-1;
    X = zeros(m, len);
    for ky=1:m
        for kx=1:len
            if ( ky - (kx-1) ) > 0
                X(ky,kx) = x( ky - (kx-1) );
            end;
        end;
    end;
    D = d(1:m);
    w5(:,n) = inv(X'*X) * X' * D;

    y5(n) = 0;
    x_inv = zeros(len,1);
    for k=1:len
        if (m-k+1) > 0 
            x_inv(k) = x(m-k+1);
            y5(n) = y5(n) + w5(k,n) * x_inv(k);
        end;
    end;
    e5(n) = d(m) - y5(n);
    if (e5(n) == 0.0)
        e5(n) = exp(-40);
    end;
end;
figure(6);
gr_max = N-len;
plot([1:gr_max], w5(1,1:gr_max), [1:gr_max], w5(2,1:gr_max));

figure(7);
gr_max = N-len;
plot([1:gr_max], log10(abs(e2(1:gr_max))), [1:gr_max], log10(abs(e3(1:gr_max))), [1:gr_max], log10(abs(e4(1:gr_max))), [1:gr_max], log10(abs(e5(1:gr_max))) );


