非線形カルマンスムーザ：Extended RTS SmootherとUnscented RTS Smootherの理論とPython実装

はじめに

https://yuhi-sa.github.io/posts/20260704_kalman_smoother/1/ では線形ガウス系を対象に、RTS・固定ラグ・固定点の3種スムーザを比較しました。同記事の末尾では「同じ3分類は非線形系にも持ち上がる」と述べるにとどめ、実装には踏み込んでいませんでした。本記事ではその続きとして、https://yuhi-sa.github.io/posts/20260224_ekf/1/ と https://yuhi-sa.github.io/posts/20260226_ukf/1/ の後向きパスにあたる**Extended RTS Smoother（EKS）とUnscented RTS Smoother（URTS）**を導出・実装し、強い非線形性を持つベンチマーク問題で精度を比較します。

復習：EKF・UKFフィルタの前向きパス

非線形状態空間モデル

\[ x_k = f(x_{k-1}) + w_{k-1}, \qquad w_{k-1} \sim \mathcal{N}(0, Q) \] \[ y_k = h(x_k) + v_k, \qquad v_k \sim \mathcal{N}(0, R) \]

に対して、EKFは \(f, h\) をヤコビアン \(F_k, H_k\) で線形近似し、UKFはシグマポイントで非線形変換の平均・分散を数値的に近似します（詳細は https://yuhi-sa.github.io/posts/20260224_ekf/1/ ・https://yuhi-sa.github.io/posts/20260226_ukf/1/ を参照）。どちらも前向き（フィルタリング）パスで、時刻 \(k\) までの観測から \(x_k\) を推定する予測 \(m_k^-, P_k^-\) と更新 \(m_k, P_k\) を計算します。

Extended RTS Smoother（EKS）

線形RTSスムーザの後向き再帰式

\[ G_k = P_k F_{k+1}^\top (P_{k+1}^-)^{-1}, \qquad m_k^s = m_k + G_k(m_{k+1}^s - m_{k+1}^-), \qquad P_k^s = P_k + G_k(P_{k+1}^s - P_{k+1}^-)G_k^\top \]

において、\(F_{k+1}\) を定数の遷移行列ではなくEKFが各時刻で計算したヤコビアン \(F_{k+1} = \left.\frac{\partial f}{\partial x}\right|_{x=m_k}\) に置き換えるだけで、非線形系への拡張が得られます。フィルタリング時に保存しておいた \(m_k, P_k, m_k^-, P_k^-, F_k\) を使い、\(k=T-1\) から \(k=0\) へ逆順に計算します。

Unscented RTS Smoother（URTS）

Särkkä（2008）が示したUnscented RTS Smootherは、ヤコビアンの代わりにフィルタリング時と同じシグマポイントを使って状態遷移の交差共分散を数値的に求めます。時刻 \(k\) の平均・共分散 \(m_k, P_k\) からシグマポイント \(\mathcal{X}_{k}^{(i)}\) を生成し、\(f\) で伝播させた \(\mathcal{X}_{k+1|k}^{(i)} = f(\mathcal{X}_k^{(i)})\) から、

\[ m_{k+1}^- = \sum_i w_m^{(i)} \mathcal{X}_{k+1|k}^{(i)}, \qquad P_{k+1}^- = \sum_i w_c^{(i)} (\mathcal{X}_{k+1|k}^{(i)} - m_{k+1}^-)(\mathcal{X}_{k+1|k}^{(i)} - m_{k+1}^-)^\top + Q \] \[ C_{k+1} = \sum_i w_c^{(i)} (\mathcal{X}_k^{(i)} - m_k)(\mathcal{X}_{k+1|k}^{(i)} - m_{k+1}^-)^\top \]

を計算し、ゲイン \(G_k = C_{k+1} (P_{k+1}^-)^{-1}\) を使って線形RTSと同じ更新式を適用します。EKSがヤコビアンで局所線形近似するのに対し、URTSはシグマポイントによる非線形変換の統計量をそのまま使う点が異なります。

Pythonによる実装

ベンチマークモデル

強い非線形性を持つ古典的ベンチマーク（Kitagawa, 1987; Julier & Uhlmann, 1997のUKF論文でも使用）を使います。

\[ x_k = 0.5x_{k-1} + \frac{25x_{k-1}}{1+x_{k-1}^2} + 8\cos(1.2(k-1)) + w_{k-1} \] \[ y_k = \frac{x_k^2}{20} + v_k \]

\(Q=10,\ R=1\) とし、状態遷移関数は \(x=\pm 1\) 付近で強い非線形性（山型のピーク）を持ちます。

import numpy as np

Q, R, T = 10.0, 1.0, 60


def f(x, k):
    return 0.5 * x + 25 * x / (1 + x**2) + 8 * np.cos(1.2 * k)


def fprime(x, k):
    return 0.5 + 25 * (1 - x**2) / (1 + x**2) ** 2


def h(x):
    return x**2 / 20.0


def hprime(x):
    return x / 10.0

EKF フィルタ + EKS スムーザ

def ekf_filter(ys):
    m, P = np.zeros(T), np.zeros(T)
    m_pred, P_pred, F_hist = np.zeros(T), np.zeros(T), np.zeros(T)
    m[0], P[0] = 0.1, 5.0
    m_pred[0], P_pred[0] = m[0], P[0]
    for k in range(1, T):
        F = fprime(m[k - 1], k - 1)
        F_hist[k] = F
        m_pred[k] = f(m[k - 1], k - 1)
        P_pred[k] = F * P[k - 1] * F + Q
        Hk = hprime(m_pred[k])
        S = Hk * P_pred[k] * Hk + R
        K = P_pred[k] * Hk / S
        m[k] = m_pred[k] + K * (ys[k] - h(m_pred[k]))
        P[k] = (1 - K * Hk) * P_pred[k]
    return m, P, m_pred, P_pred, F_hist


def eks_smoother(m, P, m_pred, P_pred, F_hist):
    ms, Ps = m.copy(), P.copy()
    for k in range(T - 2, -1, -1):
        F = F_hist[k + 1]
        G = P[k] * F / P_pred[k + 1]
        ms[k] = m[k] + G * (ms[k + 1] - m_pred[k + 1])
        Ps[k] = P[k] + G * (Ps[k + 1] - P_pred[k + 1]) * G
    return ms, Ps

UKF フィルタ + URTS スムーザ

def sigma_points(mean, cov, alpha=1.0, beta=2.0, kappa=2.0):
    n = 1
    lam = alpha**2 * (n + kappa) - n
    c = n + lam
    sqrt_c = np.sqrt(c * cov)
    pts = np.array([mean, mean + sqrt_c, mean - sqrt_c])
    wm = np.array([lam / c, 1 / (2 * c), 1 / (2 * c)])
    wc = wm.copy()
    wc[0] += 1 - alpha**2 + beta
    return pts, wm, wc


def ukf_filter(ys):
    m, P = np.zeros(T), np.zeros(T)
    m_pred, P_pred = np.zeros(T), np.zeros(T)
    sig_store = [None] * T
    m[0], P[0] = 0.1, 5.0
    m_pred[0], P_pred[0] = m[0], P[0]
    for k in range(1, T):
        pts, wm, wc = sigma_points(m[k - 1], P[k - 1])
        pts_pred = f(pts, k - 1)
        mp = np.sum(wm * pts_pred)
        Pp = np.sum(wc * (pts_pred - mp) ** 2) + Q
        m_pred[k], P_pred[k] = mp, Pp
        sig_store[k] = (pts, wc, pts_pred, mp)

        pts2, wm2, wc2 = sigma_points(mp, Pp)
        y_pred = h(pts2)
        y_mean = np.sum(wm2 * y_pred)
        Pyy = np.sum(wc2 * (y_pred - y_mean) ** 2) + R
        Pxy = np.sum(wc2 * (pts2 - mp) * (y_pred - y_mean))
        K = Pxy / Pyy
        m[k] = mp + K * (ys[k] - y_mean)
        P[k] = Pp - K * Pyy * K
    return m, P, m_pred, P_pred, sig_store


def urts_smoother(m, P, m_pred, P_pred, sig_store):
    ms, Ps = m.copy(), P.copy()
    for k in range(T - 2, -1, -1):
        pts, wc, pts_pred, mp = sig_store[k + 1]
        C = np.sum(wc * (pts - m[k]) * (pts_pred - mp))
        G = C / P_pred[k + 1]
        ms[k] = m[k] + G * (ms[k + 1] - m_pred[k + 1])
        Ps[k] = P[k] + G * (Ps[k + 1] - P_pred[k + 1]) * G
    return ms, Ps

alpha=1.0, beta=2.0, kappa=2.0 を採用しています。標準的な alpha=1e-3（高次元系向けの狭いシグマ点配置）をこの強非線形ベンチマークにそのまま使うと、シグマ点が局所領域に集中しすぎて非線形性を捉えきれず、UKFが発散することを確認しました（本記事末尾の注意点を参照）。

モンテカルロ評価（200試行）

rng = np.random.default_rng(0)


def simulate(x0=0.1):
    xs, ys = np.zeros(T), np.zeros(T)
    x = x0
    for k in range(T):
        if k > 0:
            x = f(x, k - 1) + rng.normal(0, np.sqrt(Q))
        xs[k] = x
        ys[k] = h(x) + rng.normal(0, np.sqrt(R))
    return xs, ys


rmse = lambda a, b: np.sqrt(np.mean((a - b) ** 2))
res = {"ekf": [], "eks": [], "ukf": [], "uks": []}
for _ in range(200):
    xs, ys = simulate()
    m, P, m_pred, P_pred, F_hist = ekf_filter(ys)
    ms, Ps = eks_smoother(m, P, m_pred, P_pred, F_hist)
    mu, Pu, mu_pred, Pu_pred, sig_store = ukf_filter(ys)
    msu, Psu = urts_smoother(mu, Pu, mu_pred, Pu_pred, sig_store)
    res["ekf"].append(rmse(m, xs))
    res["eks"].append(rmse(ms, xs))
    res["ukf"].append(rmse(mu, xs))
    res["uks"].append(rmse(msu, xs))

for k, v in res.items():
    print(f"{k}: mean RMSE = {np.mean(v):.4f} (std {np.std(v):.4f})")

実行結果（200試行モンテカルロ、平均RMSE）:

手法	平均RMSE	標準偏差
EKF（フィルタ）	19.39	9.06
EKS（スムーザ）	17.86	6.14
UKF（フィルタ）	8.88	1.99
URTS（スムーザ）	8.26	2.14

2つの傾向が確認できます。

UKFはEKFを大幅に上回る（RMSE 8.88 対 19.39）。このベンチマークは \(x=\pm1\) 付近で \(\frac{25x}{1+x^2}\) の曲率が非常に大きく、EKFのヤコビアンによる1次線形近似が破綻しやすい一方、UKFはシグマ点で実際の非線形変換を数値的に捉えるため頑健です。
スムーザはフィルタより一貫して精度が改善する（EKF→EKSで約7.9%、UKF→URTSで約7.0%のRMSE改善）。これは https://yuhi-sa.github.io/posts/20260704_kalman_smoother/1/ で確認した線形ガウス系での傾向が非線形系でも成り立つことを示しています。

注意点：シグマ点パラメータの選び方

alpha を小さくしすぎる（alpha=1e-3 など）と、シグマ点がほぼ平均値に密集し、強い非線形領域をサンプリングできずにUKFが発散します（本記事のベンチマークで実際に発散を確認しました）。alpha は状態次元・非線形性の強さに応じて調整が必要で、次元数が大きい実システムでは小さめの値、本記事のような低次元で強非線形な系ではやや大きめの値（1.0前後）が安定します。

まとめ

Extended RTS Smoother（EKS）はEKFのヤコビアンをそのまま線形RTS漸化式に用いる素直な拡張で、線形系のRTSと同じ再帰式で実装できる
Unscented RTS Smoother（URTS）はシグマ点による交差共分散を使い、ヤコビアンなしで非線形系のスムージングを実現する
強非線形なKitagawaベンチマークで200試行モンテカルロ検証した結果、UKF系（RMSE 8.88→8.26）がEKF系（19.39→17.86）を大幅に上回り、かつ両方式ともスムーザがフィルタを一貫して上回ることを確認した
シグマ点パラメータalphaは非線形性の強さに応じた調整が必須で、小さすぎるとUKFが発散する

https://yuhi-sa.github.io/posts/20260704_kalman_smoother/1/ — 線形ガウス系でのRTS・固定ラグ・固定点スムーザ比較。本記事の前提となる基礎編
https://yuhi-sa.github.io/posts/20260224_ekf/1/ — 拡張カルマンフィルタの理論とヤコビアンの導出
https://yuhi-sa.github.io/posts/20260226_ukf/1/ — Unscented Kalman Filterとシグマポイントの理論
https://yuhi-sa.github.io/posts/20260223_rts_smoother/1/ — 線形RTSスムーザの前向き後向き再帰の基礎
https://yuhi-sa.github.io/posts/20260223_particle_filter/1/ — 非ガウス系向けの粒子フィルタ・粒子スムーザ

参考文献

Särkkä, S. (2008). “Unscented Rauch-Tung-Striebel Smoother.” IEEE Transactions on Automatic Control, 53(3), 845-849.
Särkkä, S. (2013). Bayesian Filtering and Smoothing. Cambridge University Press.
Julier, S. J., & Uhlmann, J. K. (1997). “New extension of the Kalman filter to nonlinear systems.” Proceedings of SPIE, 3068.
Kitagawa, G. (1987). “Non-Gaussian state-space modeling of nonstationary time series.” Journal of the American Statistical Association, 82(400), 1032-1041.