近似直線位相IIRフィルタの時間領域設計（カルマンフィルタ利用）

こんな感じのカルマンフィルタで設計する。

計算したところ、あんまりうまくいかなかった。あとカルマンフィルタでやる必要あるのか？という点がいまいち納得できていない。

clear; close all;

%Parks-McCllelan法による低域FIRフィルタの設計
wc = pi/2;
wd = pi/2+pi/10;
omega = [0 wc wd pi];
Href = [1 1 0 0];
N = 40;
subplot(211);
plot(omega, Href,'o-',"DisplayName",'理想低域フィルタ');
hk = firpm(N, omega/max(omega), Href);
[h, w] = freqz(hk,1,512);
hold on;
plot(w,abs(h),'DisplayName','参照FIRフィルタ(N=40)');
title('理想フィルタと参照FIRフィルタ');
legend;

subplot(212);
plot(smoother(angle(h)));
title('FIRフィルタの位相特性（＝直線位相）');
big;


%FIRフィルタのインパルス応答yf(t)の計算
%入力x(t)をxkとする
xk = zeros(N+1,1);
xk(1) = 1;%インパルス入力なので
%出力yf(t)
yk = zeros(size(xk));
%畳み込みでyfkを計算する
for t=0:N-1
    yf = 0;
    for k=0:t
        yf = yf +  hk(k+1)*xk(t-k+1);
    end
    yk(t+1) = yf;
end
%上記よりyk=hkが確かめられたので、以降yk=hkとする.
yfk = hk;

%IIRディジタルフィルタの次数
n = 20;
theta = zeros(2*n+1,N);%縦ベクトル
z = zeros(2*n+1,N);%縦ベクトル
gamma = 1;
R = gamma*eye(2*n+1);
%N秒間シミュレーションするとする
k = zeros(2*n+1,N);
v = zeros(1,N);%vは一次元
y = [0];%縦ベクトル
rho = 0;
for idx = 1:N-1
    %誤差vの更新
    v(idx+1) = yfk(idx)-z(:,idx)'*theta(:,idx);
    %ρの更新
    rho = rho + 1/idx*(v(idx+1)-rho);
    %カルマンゲインの更新
    k(:,idx+1) = R*z(:,idx)/(1+z(:,idx)'*R*z(:,idx));
    %共分散行列の更新
    R = (eye(2*n+1)-k(:,idx+1)*z(:,idx)')*R;
    %パラメータの更新
    theta(:,idx+1) = theta(:,idx)+k(:,idx+1)*v(idx+1);
    %y(t)の更新
    y(end+1) = z(:,idx)'*theta(:,idx+1);
    %z(t)の更新
    z_tmp = [];
    for j=1:n
        %-y(t-N)の追加
        if idx-j<1
            z_tmp(end+1) = 0;
        else
            z_tmp(end+1) = -y(idx-j);
        end
    end
    for j=0:n
        %x(t-N)の追加
        if idx-j<1
            z_tmp(end+1) = 0;
        else
            z_tmp(end+1) = xk(idx-j);
        end
    end
    z(:,idx+1) = z_tmp';
end

theta_opt = theta(:,end);

syms z;
a = theta_opt(1:n);
b = theta_opt(n+1:end);
Ts = 1;
num = b;
den = [1; a];
G = tf(num', den', Ts);
figure();
bode(G);
title('IIRフィルタの振幅特性・位相特性');
big;

figure();
plot(yk,'DisplayName','Reference FIR (N=40)');
hold on;
plot(y,'DisplayName','IIR (N=20)');
legend;
title('インパルス応答');
big;

function ys=smoother(xs)
ys = xs;
for idx = 2:length(xs)
    if (xs(idx) - xs(idx-1)) > 0
        disp('curernt value increased!');
        ys(idx:end) = ys(idx:end) - abs(xs(idx)-xs(idx-1));
    end
end
end