非線形カルマンスムーザ：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\) を計算します。

導出：ガウス仮定密度スムーザとしての統一公式

EKS・URTSは一見別々の手法に見えますが、実際には「非線形変換の平均・共分散・交差共分散をどう近似するか」という1点だけが異なり、後ろ向き漸化式の骨格そのものは共通のガウス近似から導かれます。ここでは Särkkä (2013) の一般化ガウス（assumed density）スムーザの枠組みに沿って、天下り的に式を提示するのではなく、EKS・URTSに共通する漸化式を最初から導出します。

ステップ1：時刻 \(k, k+1\) の同時分布の近似

フィルタリングパスで時刻 \(k\) の事後分布 \(p(x_k \mid y_{1:k}) \approx \mathcal{N}(m_k, P_k)\) が得られているとします。状態遷移 \(x_{k+1} = f(x_k) + w_k\) を通じて、\((x_k, x_{k+1})\) の同時分布を次のガウス近似で置き換えます。

\[ p(x_k, x_{k+1} \mid y_{1:k}) \approx \mathcal{N}\!\left( \begin{bmatrix} m_k \\ m_{k+1}^- \end{bmatrix},\ \begin{bmatrix} P_k & C_{k+1} \\ C_{k+1}^\top & P_{k+1}^- \end{bmatrix} \right) \]

ここで

\[ m_{k+1}^- = \mathrm{E}[f(x_k) \mid y_{1:k}], \qquad P_{k+1}^- = \mathrm{Cov}[f(x_k) \mid y_{1:k}] + Q, \qquad C_{k+1} = \mathrm{Cov}[x_k, f(x_k) \mid y_{1:k}] \]

は \(p(x_k\mid y_{1:k}) \approx \mathcal{N}(m_k, P_k)\) のもとでの \(f\) による非線形変換の統計量です。これらはEKF/UKFの前向きパスの予測ステップで一度計算済みの量そのものです。

ステップ2：ガウス条件付けによる後ろ向き遷移

同時分布がガウスであれば、条件付き分布も厳密にガウスになり、多変量ガウスの条件付け公式から

\[ p(x_k \mid x_{k+1}, y_{1:k}) = \mathcal{N}\!\left(x_k;\ m_k + G_k(x_{k+1} - m_{k+1}^-),\ P_k - G_k P_{k+1}^- G_k^\top\right), \qquad G_k = C_{k+1} (P_{k+1}^-)^{-1} \]

が成り立ちます。

ステップ3：マルコフ近似と周辺化

RTSスムーザの中心的な近似は、

\[ p(x_k \mid x_{k+1}, y_{1:T}) \approx p(x_k \mid x_{k+1}, y_{1:k}) \]

というマルコフ性の仮定です（線形ガウス系ではこれは近似ではなく厳密に成立します。非線形系では \(p(x_{k+1}\mid y_{1:T})\) 自体が近似的なガウス分布であるため、この等式も近似になります）。この仮定のもとで \(p(x_{k+1} \mid y_{1:T}) \approx \mathcal{N}(m_{k+1}^s, P_{k+1}^s)\) を用いて \(x_{k+1}\) を周辺化すると、

\[ 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 \]

が得られます。これが線形・非線形を問わず共通する後ろ向き漸化式で、\(k=T\) で \(m_T^s = m_T,\ P_T^s = P_T\) から初期化し、\(k=T-1,\dots,0\) の順に逆再帰的に計算します。

EKS：1次テイラー展開によるモーメント近似

\(f\) を \(m_k\) の周りで1次テイラー展開 \(f(x_k) \approx f(m_k) + F_k(x_k - m_k)\) 、\(F_k = \left.\partial f/\partial x\right|_{x=m_k}\) で線形近似すると、ステップ1のモーメントは

\[ m_{k+1}^- \approx f(m_k), \qquad P_{k+1}^- \approx F_k P_k F_k^\top + Q, \qquad C_{k+1} \approx P_k F_k^\top \]

と解析的に計算できます。これをステップ2・3の一般公式に代入すると、ゲインは \(G_k = P_k F_k^\top (P_{k+1}^-)^{-1}\) となり、EKFが各時刻で計算したヤコビアン \(F_{k+1}\) をそのまま線形RTS漸化式に流用する**Extended RTS Smoother（EKS）**の漸化式が得られます。フィルタリング時に保存しておいた \(m_k, P_k, m_k^-, P_k^-, F_k\) を使い、\(k=T-1\) から \(k=0\) へ逆順に計算します。

URTS：シグマポイントによるモーメント近似

Särkkä（2008）が示したUnscented RTS Smootherは、\(f\) を線形近似せず、\(m_k, P_k\) から生成したシグマポイント \(\mathcal{X}_k^{(i)}\) を使って上記3つのモーメントをUnscented変換で数値的に近似します。\(\mathcal{X}_{k+1|k}^{(i)} = f(\mathcal{X}_k^{(i)})\) とおくと、

\[ m_{k+1}^- \approx \sum_i w_m^{(i)} \mathcal{X}_{k+1|k}^{(i)}, \qquad P_{k+1}^- \approx \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} \approx \sum_i w_c^{(i)} (\mathcal{X}_k^{(i)} - m_k)(\mathcal{X}_{k+1|k}^{(i)} - m_{k+1}^-)^\top \]

となり、これをステップ2・3の一般公式に代入したものが**Unscented RTS Smoother（URTS）**の漸化式です。

統一的な見方から分かる精度差の理由

EKSの誤差は「\(f\) の1次テイラー近似が無視する2次以上の曲率項」に由来し、URTSの誤差は「有限個（\(2n+1\) 個）のシグマ点による数値積分の誤差」に由来します。EKSは \(f\) の曲率を完全に無視するのに対し、URTSはUnscented変換により3次のモーメントまで（ガウス入力に対して）厳密に捉えるため、強い非線形性を持つ系ではURTSがEKSを一貫して上回る傾向があります。この理由は、後述するモンテカルロ実験の結果（UKF系がEKF系を大幅に上回る）とも整合します。

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/ で確認した線形ガウス系での傾向が非線形系でも成り立つことを示しています。

フィルタリング推定 vs スムージング推定の可視化

平均値だけでは見えない挙動を確認するため、乱数シード42の1試行を取り出し、EKF/EKSとUKF/URTSそれぞれの推定軌道を真値と重ねてプロットしました。

非線形カルマンスムーザ：フィルタリング推定とスムージング推定の比較

左図（EKF vs EKS）では、\(k=36\) 付近で真値が \(x=\pm1\) の高曲率領域を通過した直後、EKFのヤコビアン線形化が破綻して推定値が一時的に \(-190\) 近くまで暴走している様子が確認できます（この1試行でのRMSEはEKFが28.04、EKSが21.54）。EKSはEKFの推定をそのまま後ろ向きに補正する構造上、フィルタが暴走した時刻の誤差を完全には吸収しきれませんが、それでも他の時刻の推定を滑らかにすることでRMSEを約23%改善しています。中央図（UKF vs URTS）では同じ観測データに対してUKF・URTSともに真値の振動によく追従しており、フィルタ（RMSE 8.04）とスムーザ（RMSE 7.59）の差は相対的に小さいものの、真値の谷・山の折り返し付近でURTSがUKFよりわずかに真値に近づいていることが読み取れます。右図は200試行のRMSE分布の箱ひげ図で、EKF/EKSがUKF/URTSより中央値・ばらつきの両方で劣ることが一目で分かります。とりわけEKFの箱ひげ図は上ヒゲが長く、左図で見たような散発的な暴走がRMSEの裾を重くしていることが伺えます。

注意点・エッジケース

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

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

マルコフ近似の限界と非ガウス性

前節の導出で使った \(p(x_k \mid x_{k+1}, y_{1:T}) \approx p(x_k \mid x_{k+1}, y_{1:k})\) という近似は、\(p(x_{k+1}\mid y_{1:T})\) が実際にガウス分布に従う場合にのみ厳密です。強い非線形性のもとでは真の平滑化事後分布が多峰性・非対称性を持つことがあり、この場合EKS・URTSはどちらも単峰のガウス分布で近似するため、平均・共分散の1次・2次モーメントしか捉えられません。図の左図で見たEKFの暴走のように、フィルタリング推定が大きく外れた時刻では、後ろ向きの補正だけでは真の分布の多峰性を復元できないことに注意が必要です。多峰性が疑われる場合は https://yuhi-sa.github.io/posts/20260223_particle_filter/1/ の粒子スムーザとの併用を検討すべきです。

微分可能性の要件

EKF/EKSはヤコビアン \(F_k = \partial f/\partial x\) の解析的な計算（または数値微分）を必要とします。\(f\) が区分的関数（飽和・ReLU的な非線形性）や非連続な場合、ヤコビアンが定義できない・不連続になる点で線形化が破綻します。一方UKF/URTSは \(f\) を関数値のみで評価するため、微分不可能な \(f\) に対しても（連続でありさえすれば）そのまま適用できるという実務上の利点があります。

共分散の数値的な悪条件性

\(G_k = C_{k+1}(P_{k+1}^-)^{-1}\) の逆行列計算は、\(Q\) が非常に小さい・観測が非常に精密といった悪条件な設定で数値的に不安定になり得ます。https://yuhi-sa.github.io/posts/20260223_rts_smoother/1/ で確認したように、np.linalg.inv の代わりに np.linalg.solve を使う、\(P \leftarrow (P+P^\top)/2\) で明示的に対称化する、といった対策は非線形版のEKS/URTSにもそのまま有効です。

メモリコスト

EKS・URTSはいずれもフィルタリングパスで計算した中間量（EKSは \(m_k, P_k, m_k^-, P_k^-, F_k\) 、URTSはシグマ点とその重み・伝播先）を全時刻分保存しておく必要があり、状態次元 \(n\) ・時刻数 \(T\) に対して \(O(Tn^2)\) （URTSはさらにシグマ点数倍）のメモリを要します。オンライン・低メモリ環境では固定ラグスムーザ（https://yuhi-sa.github.io/posts/20260704_kalman_smoother/1/ 参照）への置き換えを検討する価値があります。

発展：反復スムーザとその後の研究動向

本記事のEKS・URTSはいずれも「フィルタリングパスを1回だけ実行し、その結果を後ろ向きに1回だけ補正する」という非反復的な手法です。線形化・シグマ点の基準点がフィルタリング時の推定値 \(m_k\) に固定されるため、非線形性が強い区間では線形化誤差が補正しきれずに残ります。この限界に対し、線形化点をスムージング後の推定値で更新して収束するまで繰り返す**反復拡張カルマンスムーザ（Iterated EKS, IEKS）や、統計的線形化を反復する反復事後線形化スムーザ（Iterated Posterior Linearization Smoother, IPLS）**が、EKS/URTSの精度限界を補う手法として研究されています。

Kullberg, Skoglund, Skog, & Hendeby (2024) の “Dynamically Iterated Filters”（arXiv:2404.15359）は、従来の反復拡張/Unscentedカルマンフィルタ（IEKF/IUKF）が観測モデルの線形化のみを反復更新していたのに対し、状態遷移モデルの線形化も同時に反復更新する統一的枠組みを提案し、RTSスムーザとの関係を明示した上でGauss-Newton法に基づくステップサイズ補正（damped iteration）を導入することで、標準的な反復フィルタよりも高い精度・頑健性を達成できることを示しました。また Lindqvist, Särkkä, García-Fernández, Raitoharju, & Svensson の “Posterior linearisation smoothing with robust iterations”（arXiv:2112.03969、2025年2月改訂版）は、IPLSにLevenberg-Marquardt法や直線探索法（Armijo-Wolfe条件）を組み込むことで、反復が発散しやすい強非線形設定でも収束性を改善できることを報告しています。これらの反復法は、本記事で導出したEKS/URTSの後ろ向き漸化式そのものを内側のループとして繰り返し呼び出す構造を持ち、非反復のEKS/URTSが抱える「線形化点が1回のフィルタリング推定に固定される」という弱点を、追加の計算コストと引き換えに緩和するアプローチです。

まとめ

EKS・URTSの後ろ向き漸化式は、\((x_k, x_{k+1})\) の同時ガウス近似とガウス条件付け公式・マルコフ近似から統一的に導出でき、両者の違いは非線形変換の平均・共分散・交差共分散を1次テイラー展開（EKS）で近似するかシグマ点によるUnscented変換（URTS）で近似するかだけである
強非線形なKitagawaベンチマークで200試行モンテカルロ検証した結果、UKF系（RMSE 8.88→8.26）がEKF系（19.39→17.86）を大幅に上回り、かつ両方式ともスムーザがフィルタを一貫して上回ることを確認した
1試行の軌道を可視化すると、EKFは高曲率領域通過直後に推定が暴走し得る（RMSE 28.04）のに対し、UKF/URTSはそのような暴走なく真値に追従する（RMSE 8.04/7.59）ことが視覚的に確認できた
シグマ点パラメータalphaは非線形性の強さに応じた調整が必須で、小さすぎるとUKFが発散する。またマルコフ近似の限界（多峰性の非対応）・微分可能性の要件・共分散の悪条件性・メモリコストといったエッジケースにも注意が必要
非反復のEKS/URTSは線形化・シグマ点の基準点が1回のフィルタリング推定に固定される限界を持ち、これを補う反復スムーザ（IEKS, IPLS）の研究が2024年以降も継続している

https://yuhi-sa.github.io/posts/20260224_kalman_filter/1/ — 線形カルマンフィルタの理論（カルマンゲインのMMSE導出・可観測性・フィルタ発散を含む）。本記事のEKS/URTSが前向きパスとして利用する基礎編
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/ — 非ガウス系向けの粒子フィルタ・粒子スムーザ
https://yuhi-sa.github.io/posts/20260715_rls_adaptive_filter/1/ — RLS適応フィルタとカルマンフィルタの数値的等価性（線形の特殊ケース）

参考文献

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.
Kullberg, A., Skoglund, M. A., Skog, I., & Hendeby, G. (2024). “Dynamically Iterated Filters: A unified framework for improved iterated filtering and smoothing.” arXiv:2404.15359.
Lindqvist, J., Särkkä, S., García-Fernández, Á. F., Raitoharju, M., & Svensson, L. (2025). “Posterior linearisation smoothing with robust iterations.” arXiv:2112.03969 (revised Feb. 2025).