人工衛星の軌道を事後的に精密に決定する場面を想像してください。地上局のレーダーで12時間にわたって衛星を追跡し、観測データを蓄積しました。このデータを使って、午前6時（追跡開始から6時間後）の衛星の位置を最も正確に推定したいとします。
カルマンフィルタは、午前0時から順に処理を進め、午前6時の推定には「午前6時までの」観測データのみを使います。しかし私たちの手元には午前6時以降の観測データもあるのです。当然、その後のデータも使えば、午前6時の推定はさらに正確になるはずです。
このように全時刻の観測データを使って過去の各時刻の状態を最適に推定する処理をスムージング（smoothing）と呼びます。フィルタリングが「今この瞬間の最良推定」を求めるのに対し、スムージングは「全データを見渡した上での最良推定」を求めます。事後解析や軌道再構成のように、リアルタイム性が不要な場面で威力を発揮する手法です。
Rauch-Tung-Striebel（RTS）スムーザは、線形ガウスシステムに対するスムージングの最も標準的なアルゴリズムです。1965年にRauch、Tung、Striebelによって発表されました。RTSスムーザのアイデアは非常にエレガントで、カルマンフィルタを順方向に1パス走らせた後、逆方向に1パス走らせるだけで最適なスムージング推定が得られます。計算コストはカルマンフィルタの約2倍に過ぎません。
スムージングが有効な応用は多岐にわたります。
- 衛星軌道決定: 追跡データの事後処理による精密軌道暦の作成
- 慣性航法の事後補正: フライトデータの事後解析、テスト飛行の軌跡再構成
- 時系列分析: 経済指標のトレンド推定、気候データの平滑化
- 音声・信号処理: ノイズ除去、バイオメディカル信号の平滑化
- 機械学習: 状態空間モデルのEMアルゴリズムにおけるEステップ
本記事の内容
- フィルタリングとスムージングの数学的な違い
- RTSスムーザの再帰式の導出
- スムーザゲインの直感的解釈
- スムージング推定の性質（なぜフィルタリングより良いのか）
- Python実装とフィルタリングとの比較
- 非線形システムへの拡張（EKSとUKS）
前提知識
この記事を読む前に、以下の記事を読んでおくと理解が深まります。
- カルマンフィルタと最適推定の制御応用 — 線形カルマンフィルタの理論と実装
- カルマンフィルタの理論と導出 — カルマンフィルタの基礎
また、以下の概念に馴染みがあることを前提とします。
- カルマンフィルタの予測・更新ステップ
- 条件付きガウス分布の性質
- ベイズ推定の基本概念
フィルタリングとスムージング
3つの状態推定問題
時系列データの状態推定には、使う観測データの範囲によって3つの問題があります。時刻 $k$ の状態 $\bm{x}_k$ を、時刻 $1$ から $T$ までの全観測 $\bm{y}_{1:T}$ の中でどこまで使うかで分類します。
| 問題 | 使う観測 | 条件付き分布 | 用途 |
|---|---|---|---|
| 予測（Prediction） | $\bm{y}_{1:k-j}$（$j > 0$） | $p(\bm{x}_k \| \bm{y}_{1:k-j})$ | 将来の状態を予測 |
| フィルタリング（Filtering） | $\bm{y}_{1:k}$ | $p(\bm{x}_k \| \bm{y}_{1:k})$ | 現在の状態をリアルタイム推定 |
| スムージング（Smoothing） | $\bm{y}_{1:T}$（$T > k$） | $p(\bm{x}_k \| \bm{y}_{1:T})$ | 過去の状態を事後的に推定 |
カルマンフィルタは「フィルタリング」を行います。時刻 $k$ の推定値 $\hat{\bm{x}}_{k|k}$ は、時刻 $1$ から $k$ までの観測のみを使った最適推定です。しかし、$k+1$ 以降の観測も利用可能であれば、時刻 $k$ の状態についてもっと正確な推定ができるはずです。
なぜスムージングはフィルタリングより良いのか
直感的に理解するために、簡単な1次元の例を考えてみましょう。等速直線運動する物体の位置 $x$ をノイズの多いセンサで観測しています。時刻 $k = 50$ での推定を考えます。
フィルタリング（$k = 50$ まで）: 時刻50までの観測から位置を推定します。時刻50の直近の観測と、モデルによる予測を組み合わせた推定値が得られます。
スムージング（$k = 100$ まで）: 時刻100までの全観測を使います。時刻50以降の観測は「物体がその後どこに向かったか」の情報を含んでおり、この情報は「時刻50にどこにいたはず」かの推定を改善します。もし時刻51〜60の観測が、フィルタリング推定よりも少し右側を指しているなら、時刻50の推定も右にシフトすべきです。
数学的には、スムージング推定 $\hat{\bm{x}}_{k|T}$ は全観測 $\bm{y}_{1:T}$ の条件付き期待値であり、フィルタリング推定 $\hat{\bm{x}}_{k|k}$ は部分観測 $\bm{y}_{1:k}$ の条件付き期待値です。条件付ける情報が増えるほど（追加の観測が入るほど）、推定精度は必ず改善するか、少なくとも悪化しません。
具体的には、スムージング推定の誤差共分散 $\bm{P}_{k|T}$ は、フィルタリングの誤差共分散 $\bm{P}_{k|k}$ に対して次の不等式を満たします。
$$ \bm{P}_{k|T} \leq \bm{P}_{k|k} $$
ここで「$\leq$」は行列の意味で、$\bm{P}_{k|k} – \bm{P}_{k|T}$ が半正定値であることを意味します。つまり、スムージングはフィルタリングと比べて、すべての状態成分について推定精度が同等以上になることが保証されています。
この性質を理論的に保証する具体的なアルゴリズムが、次に紹介するRTSスムーザです。
RTSスムーザの導出
アルゴリズムの概要
RTSスムーザは2パスのアルゴリズムです。
-
順方向パス（Forward pass）: 時刻 $1$ から $T$ まで通常のカルマンフィルタを走らせ、各時刻のフィルタリング推定 $\hat{\bm{x}}_{k|k}$、$\bm{P}_{k|k}$、予測 $\hat{\bm{x}}_{k|k-1}$、$\bm{P}_{k|k-1}$ を保存する
-
逆方向パス（Backward pass）: 時刻 $T$ から $1$ に向かって逆方向に走り、スムージング推定 $\hat{\bm{x}}_{k|T}$、$\bm{P}_{k|T}$ を計算する
逆方向パスの再帰式が RTSスムーザの核心です。
逆方向パスの導出
時刻 $k$ のスムージング分布 $p(\bm{x}_k | \bm{y}_{1:T})$ を、フィルタリング分布 $p(\bm{x}_k | \bm{y}_{1:k})$ から求めることを考えます。
マルコフ性（$\bm{x}_{k+1}$ が与えられれば $\bm{x}_k$ と $\bm{y}_{k+1:T}$ は独立）を利用すると、次の関係が成り立ちます。
$$ p(\bm{x}_k | \bm{y}_{1:T}) = p(\bm{x}_k | \bm{y}_{1:k}) \int \frac{p(\bm{x}_{k+1} | \bm{x}_k) \, p(\bm{x}_{k+1} | \bm{y}_{1:T})}{p(\bm{x}_{k+1} | \bm{y}_{1:k})} \, d\bm{x}_{k+1} $$
この式は、スムージング分布がフィルタリング分布に「未来からの補正」を掛けたものであることを示しています。
線形ガウスモデルでは、すべての分布がガウス分布であるため、この積分は解析的に計算できます。結果として、次のRTS再帰式が得られます。
スムーザゲイン:
$$ \bm{G}_k = \bm{P}_{k|k} \bm{A}^T \bm{P}_{k+1|k}^{-1} $$
スムージング推定:
$$ \hat{\bm{x}}_{k|T} = \hat{\bm{x}}_{k|k} + \bm{G}_k(\hat{\bm{x}}_{k+1|T} – \hat{\bm{x}}_{k+1|k}) $$
スムージング共分散:
$$ \bm{P}_{k|T} = \bm{P}_{k|k} + \bm{G}_k(\bm{P}_{k+1|T} – \bm{P}_{k+1|k})\bm{G}_k^T $$
逆方向パスの初期値は、最終時刻 $T$ のフィルタリング推定です。
$$ \hat{\bm{x}}_{T|T} = \hat{\bm{x}}_{T|T}, \quad \bm{P}_{T|T} = \bm{P}_{T|T} $$
最終時刻ではスムージングとフィルタリングが一致します。これは当然です。最終時刻では「未来のデータ」が存在しないため、追加の情報はありません。
スムーザゲインの解釈
RTSスムーザの更新式を詳しく見てみましょう。
$$ \hat{\bm{x}}_{k|T} = \hat{\bm{x}}_{k|k} + \bm{G}_k \underbrace{(\hat{\bm{x}}_{k+1|T} – \hat{\bm{x}}_{k+1|k})}_{\text{未来からの修正量}} $$
この式は、フィルタリング推定 $\hat{\bm{x}}_{k|k}$ に「未来からの修正」を加えたものです。修正量 $\hat{\bm{x}}_{k+1|T} – \hat{\bm{x}}_{k+1|k}$ は、時刻 $k+1$ のスムージング推定とフィルタリング時の予測の差です。これは「未来のデータを見た結果、時刻 $k+1$ の予測がどれだけ間違っていたか」を表しています。
スムーザゲイン $\bm{G}_k$ は、この修正量を時刻 $k$ の状態にどの程度反映させるかを決めるゲインです。
$$ \bm{G}_k = \bm{P}_{k|k} \bm{A}^T \bm{P}_{k+1|k}^{-1} $$
$\bm{P}_{k|k}$ はフィルタリング推定の不確実性、$\bm{P}_{k+1|k}$ は1ステップ予測の不確実性です。フィルタリングの不確実性が大きい（$\bm{P}_{k|k}$ が大きい）ほど修正の余地が大きく、予測の不確実性が小さい（$\bm{P}_{k+1|k}$ が小さい）ほどモデルの予測が信頼できるため修正が控えめになります。
これはカルマンフィルタのゲインと対称的な構造を持っています。カルマンフィルタのゲインは「観測からの修正」を決めるのに対し、スムーザゲインは「未来からの修正」を決めます。
共分散の更新式の解釈
スムージング共分散の更新式にも重要な洞察が含まれています。
$$ \bm{P}_{k|T} = \bm{P}_{k|k} + \bm{G}_k \underbrace{(\bm{P}_{k+1|T} – \bm{P}_{k+1|k})}_{\leq \bm{0}} \bm{G}_k^T $$
$\bm{P}_{k+1|T} \leq \bm{P}_{k+1|k}$ であるため（スムージングはフィルタリングの予測より良い）、括弧内の行列は半負定値です。したがって $\bm{P}_{k|T} \leq \bm{P}_{k|k}$ が保証されます。これが「スムージングはフィルタリングと同等以上」という定理の証明です。
修正の大きさは $\bm{P}_{k+1|T} – \bm{P}_{k+1|k}$ の大きさに依存します。この差が大きいほど、未来のデータが大きな情報をもたらしたことを意味し、スムージングによる改善効果が大きくなります。
RTSスムーザの理論を理解したところで、次にPythonで実装し、フィルタリングとスムージングの違いを定量的に確認しましょう。
Pythonによる実装と比較
RTSスムーザの実装
位置と速度を持つ1次元の等速運動モデルで、カルマンフィルタとRTSスムーザを比較します。
import numpy as np
import matplotlib.pyplot as plt
np.random.seed(42)
# システムパラメータ
dt = 0.5
n_steps = 200
A = np.array([[1, dt], [0, 1]]) # 等速直線運動
B = np.array([[0.5*dt**2], [dt]])
C = np.array([[1, 0]]) # 位置のみ観測
Q = np.array([[0.1, 0], [0, 0.5]]) # プロセスノイズ
R = np.array([[4.0]]) # 観測ノイズ（標準偏差2m）
# 真の状態の生成（加速・減速を含む運動）
x_true = np.zeros((2, n_steps))
x_true[:, 0] = [0, 2]
for k in range(1, n_steps):
# 時変の加速度入力
u = np.array([[0.3 * np.sin(0.03 * k)]])
w = np.random.multivariate_normal([0, 0], Q)
x_true[:, k] = A @ x_true[:, k-1] + (B @ u).flatten() + w
# 観測の生成
y_obs = np.zeros(n_steps)
for k in range(n_steps):
y_obs[k] = C @ x_true[:, k] + np.random.normal(0, np.sqrt(R[0, 0]))
# ====== 順方向パス: カルマンフィルタ ======
x_filt = np.zeros((2, n_steps)) # フィルタリング推定
x_pred = np.zeros((2, n_steps)) # 予測（スムーザで使用）
P_filt = np.zeros((4, n_steps)) # フィルタリング共分散（flatten）
P_pred = np.zeros((4, n_steps)) # 予測共分散（flatten）
x_filt[:, 0] = [2, 0] # 初期推定（誤差あり）
P = np.diag([10, 5])
P_filt[:, 0] = P.flatten()
for k in range(1, n_steps):
u = np.array([[0.3 * np.sin(0.03 * k)]])
# 予測
xp = A @ x_filt[:, k-1] + (B @ u).flatten()
Pp = A @ P @ A.T + Q
x_pred[:, k] = xp
P_pred[:, k] = Pp.flatten()
# 更新
S = C @ Pp @ C.T + R
K = Pp @ C.T @ np.linalg.inv(S)
e = y_obs[k] - C @ xp
x_filt[:, k] = xp + (K @ e.reshape(-1, 1)).flatten()
P = (np.eye(2) - K @ C) @ Pp
P_filt[:, k] = P.flatten()
# ====== 逆方向パス: RTSスムーザ ======
x_smooth = np.zeros((2, n_steps)) # スムージング推定
P_smooth = np.zeros((4, n_steps)) # スムージング共分散
smoother_gains = np.zeros((4, n_steps))
# 最終時刻の初期化（フィルタリングと同じ）
x_smooth[:, -1] = x_filt[:, -1]
P_smooth[:, -1] = P_filt[:, -1]
for k in range(n_steps - 2, -1, -1):
Pk = P_filt[:, k].reshape(2, 2)
Ppk1 = P_pred[:, k+1].reshape(2, 2)
# スムーザゲイン
G = Pk @ A.T @ np.linalg.inv(Ppk1)
smoother_gains[:, k] = G.flatten()
# スムージング推定
x_smooth[:, k] = x_filt[:, k] + G @ (x_smooth[:, k+1] - x_pred[:, k+1])
# スムージング共分散
Psk1 = P_smooth[:, k+1].reshape(2, 2)
P_s = Pk + G @ (Psk1 - Ppk1) @ G.T
P_smooth[:, k] = P_s.flatten()
# 描画
t = np.arange(n_steps) * dt
fig, axes = plt.subplots(2, 2, figsize=(14, 10))
# 位置推定: フィルタ vs スムーザ
ax = axes[0, 0]
ax.plot(t, x_true[0], 'b-', linewidth=2, label='True position')
ax.scatter(t, y_obs, s=5, c='gray', alpha=0.3, label='Observations')
ax.plot(t, x_filt[0], 'r--', linewidth=1.5, alpha=0.7, label='KF (filter)')
ax.plot(t, x_smooth[0], 'g-', linewidth=2, label='RTS (smoother)')
ax.set_xlabel('Time [s]', fontsize=11)
ax.set_ylabel('Position', fontsize=11)
ax.set_title('Position: Filtering vs Smoothing', fontsize=13)
ax.legend(fontsize=9)
ax.grid(True, alpha=0.3)
# 速度推定: フィルタ vs スムーザ
ax = axes[0, 1]
ax.plot(t, x_true[1], 'b-', linewidth=2, label='True velocity')
ax.plot(t, x_filt[1], 'r--', linewidth=1.5, alpha=0.7, label='KF (filter)')
ax.plot(t, x_smooth[1], 'g-', linewidth=2, label='RTS (smoother)')
ax.set_xlabel('Time [s]', fontsize=11)
ax.set_ylabel('Velocity', fontsize=11)
ax.set_title('Velocity: Filtering vs Smoothing', fontsize=13)
ax.legend(fontsize=10)
ax.grid(True, alpha=0.3)
# 推定誤差の比較
ax = axes[1, 0]
err_pos_filt = np.abs(x_true[0] - x_filt[0])
err_pos_smooth = np.abs(x_true[0] - x_smooth[0])
err_vel_filt = np.abs(x_true[1] - x_filt[1])
err_vel_smooth = np.abs(x_true[1] - x_smooth[1])
ax.plot(t, err_pos_filt, 'r-', linewidth=1, alpha=0.6, label='KF pos. error')
ax.plot(t, err_pos_smooth, 'g-', linewidth=1, alpha=0.6, label='RTS pos. error')
ax.plot(t, err_vel_filt, 'r--', linewidth=1, alpha=0.6, label='KF vel. error')
ax.plot(t, err_vel_smooth, 'g--', linewidth=1, alpha=0.6, label='RTS vel. error')
ax.set_xlabel('Time [s]', fontsize=11)
ax.set_ylabel('Absolute Error', fontsize=11)
ax.set_title('Estimation Error Comparison', fontsize=13)
ax.legend(fontsize=8, ncol=2)
ax.grid(True, alpha=0.3)
# 共分散（不確実性）の比較
ax = axes[1, 1]
p_trace_filt = np.array([P_filt[0, k] + P_filt[3, k] for k in range(n_steps)])
p_trace_smooth = np.array([P_smooth[0, k] + P_smooth[3, k] for k in range(n_steps)])
# 位置の標準偏差のみ
sigma_pos_filt = np.sqrt(P_filt[0, :])
sigma_pos_smooth = np.sqrt(P_smooth[0, :])
sigma_vel_filt = np.sqrt(P_filt[3, :])
sigma_vel_smooth = np.sqrt(P_smooth[3, :])
ax.plot(t, sigma_pos_filt, 'r-', linewidth=1.5, label='KF $\\sigma_{pos}$')
ax.plot(t, sigma_pos_smooth, 'g-', linewidth=1.5, label='RTS $\\sigma_{pos}$')
ax.plot(t, sigma_vel_filt, 'r--', linewidth=1.5, label='KF $\\sigma_{vel}$')
ax.plot(t, sigma_vel_smooth, 'g--', linewidth=1.5, label='RTS $\\sigma_{vel}$')
ax.set_xlabel('Time [s]', fontsize=11)
ax.set_ylabel('Standard Deviation', fontsize=11)
ax.set_title('Estimation Uncertainty: KF vs RTS', fontsize=13)
ax.legend(fontsize=9)
ax.grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig('rts_smoother.png', dpi=150, bbox_inches='tight')
plt.show()
# 定量評価
rmse_pos_filt = np.sqrt(np.mean((x_true[0, 10:] - x_filt[0, 10:])**2))
rmse_pos_smooth = np.sqrt(np.mean((x_true[0, 10:] - x_smooth[0, 10:])**2))
rmse_vel_filt = np.sqrt(np.mean((x_true[1, 10:] - x_filt[1, 10:])**2))
rmse_vel_smooth = np.sqrt(np.mean((x_true[1, 10:] - x_smooth[1, 10:])**2))
print("=== RMSE Comparison ===")
print(f"Position - KF: {rmse_pos_filt:.3f}, RTS: {rmse_pos_smooth:.3f} "
f"({(1-rmse_pos_smooth/rmse_pos_filt)*100:.1f}% improvement)")
print(f"Velocity - KF: {rmse_vel_filt:.3f}, RTS: {rmse_vel_smooth:.3f} "
f"({(1-rmse_vel_smooth/rmse_vel_filt)*100:.1f}% improvement)")
print(f"\nMean uncertainty (tr(P)):")
print(f" KF: {np.mean(p_trace_filt[10:]):.3f}")
print(f" RTS: {np.mean(p_trace_smooth[10:]):.3f}")
この結果から、RTSスムーザの効果が明確に確認できます。
-
位置推定（左上）: カルマンフィルタの推定（赤い破線）とRTSスムーザの推定（緑の実線）を比較すると、スムーザの推定が明らかに滑らかで、真の軌跡により近いことが分かります。特にフィルタリング推定にある「ギザギザ」がスムーザでは大幅に軽減されています。これは、未来のデータが過去の推定を修正する効果です。
-
速度推定（右上）: 速度は直接観測されないため、フィルタリングではノイズの影響を強く受けます。スムーザは速度推定の精度を大幅に改善しています。速度のような「間接的に推定される量」に対して、スムージングの恩恵が特に大きいことが分かります。
-
推定誤差（左下）: 位置・速度とも、スムーザ（緑）の誤差がフィルタ（赤）の誤差を上回ることはほとんどなく、理論通りスムージングが一貫してフィルタリングより良い（または同等の）推定を提供しています。
-
推定不確実性（右下）: スムーザの不確実性（緑）がフィルタリング（赤）を常に下回っていることが確認できます。特徴的なのは、フィルタリングの不確実性がデータの端（系列の始まりと終わり）で大きくなるのに対し、スムーザは端の効果を「未来のデータ」で補正するため、系列の始まり付近でも不確実性が低く抑えられている点です。ただし、系列の終端ではフィルタリングとスムーザの不確実性が一致します（未来のデータがないため）。
共分散の時間変化の詳細解析
スムージングによる改善効果が系列のどの位置で最も大きいかを詳しく見てみましょう。
import numpy as np
import matplotlib.pyplot as plt
fig, axes = plt.subplots(1, 2, figsize=(14, 5))
# 不確実性の改善率
improvement_pos = 1 - P_smooth[0, :] / P_filt[0, :]
improvement_vel = 1 - P_smooth[3, :] / P_filt[3, :]
ax = axes[0]
ax.plot(t, improvement_pos * 100, 'b-', linewidth=2, label='Position')
ax.plot(t, improvement_vel * 100, 'r-', linewidth=2, label='Velocity')
ax.set_xlabel('Time [s]', fontsize=11)
ax.set_ylabel('Variance Reduction [%]', fontsize=11)
ax.set_title('Smoothing Improvement over Filtering', fontsize=13)
ax.legend(fontsize=10)
ax.grid(True, alpha=0.3)
# スムーザゲインの大きさ
G_norm = np.sqrt(smoother_gains[0, :]**2 + smoother_gains[1, :]**2 +
smoother_gains[2, :]**2 + smoother_gains[3, :]**2)
ax = axes[1]
ax.plot(t[:-1], G_norm[:-1], 'k-', linewidth=1.5)
ax.set_xlabel('Time [s]', fontsize=11)
ax.set_ylabel('$\\|G_k\\|_F$', fontsize=11)
ax.set_title('Smoother Gain Magnitude', fontsize=13)
ax.grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig('rts_improvement.png', dpi=150, bbox_inches='tight')
plt.show()
print(f"\nImprovement range:")
print(f" Position: {np.min(improvement_pos[5:-5])*100:.1f}% "
f"to {np.max(improvement_pos[5:-5])*100:.1f}%")
print(f" Velocity: {np.min(improvement_vel[5:-5])*100:.1f}% "
f"to {np.max(improvement_vel[5:-5])*100:.1f}%")
この解析から、スムージングの改善効果の構造が見えます。
左のグラフは、フィルタリングに対するスムージングの分散低減率を示しています。系列の中央部では位置・速度ともに一定の改善が得られています。系列の両端では挙動が異なります。始端付近ではフィルタが初期の不確実性を抱えているため、スムーザが大きな改善をもたらします。終端付近では未来のデータがなくなるため、改善率はゼロに近づきます。
右のグラフはスムーザゲインのフロベニウスノルムで、逆方向パスで「未来の情報がどの程度伝播されるか」を示しています。ゲインが大きいほど、未来の修正が過去に強く伝播されます。定常状態（系列の中央部）では一定値に収束しており、これはカルマンフィルタのゲインが定常値に収束するのと同様の挙動です。
Q/Rの比とスムージング効果の関係
プロセスノイズ $\bm{Q}$ と観測ノイズ $\bm{R}$ の比が、スムージングの改善効果にどう影響するかを調べます。
import numpy as np
import matplotlib.pyplot as plt
# Q/R比を変えたときのスムージング効果
qr_ratios = np.logspace(-2, 2, 20)
improvement_rates = []
for qr in qr_ratios:
np.random.seed(42)
Q_test = np.array([[0.1*qr, 0], [0, 0.5*qr]])
R_test = np.array([[4.0]])
# 真の状態
xt = np.zeros((2, n_steps))
xt[:, 0] = [0, 2]
for k in range(1, n_steps):
u = np.array([[0.3*np.sin(0.03*k)]])
w = np.random.multivariate_normal([0,0], Q_test)
xt[:, k] = A @ xt[:, k-1] + (B @ u).flatten() + w
yo = np.array([C @ xt[:, k] + np.random.normal(0, np.sqrt(R_test[0,0]))
for k in range(n_steps)])
# カルマンフィルタ
xf = np.zeros((2, n_steps))
xp_arr = np.zeros((2, n_steps))
Pf = np.zeros((2, 2, n_steps))
Pp_arr = np.zeros((2, 2, n_steps))
xf[:, 0] = [2, 0]
P_run = np.diag([10, 5])
Pf[:, :, 0] = P_run
for k in range(1, n_steps):
u = np.array([[0.3*np.sin(0.03*k)]])
xp = A @ xf[:, k-1] + (B @ u).flatten()
Pp = A @ P_run @ A.T + Q_test
xp_arr[:, k] = xp
Pp_arr[:, :, k] = Pp
S = C @ Pp @ C.T + R_test
K = Pp @ C.T @ np.linalg.inv(S)
e = yo[k] - C @ xp
xf[:, k] = xp + (K * e).flatten()
P_run = (np.eye(2) - K @ C) @ Pp
Pf[:, :, k] = P_run
# RTSスムーザ
xs = np.zeros((2, n_steps))
Ps = np.zeros((2, 2, n_steps))
xs[:, -1] = xf[:, -1]
Ps[:, :, -1] = Pf[:, :, -1]
for k in range(n_steps-2, -1, -1):
G = Pf[:, :, k] @ A.T @ np.linalg.inv(Pp_arr[:, :, k+1])
xs[:, k] = xf[:, k] + G @ (xs[:, k+1] - xp_arr[:, k+1])
Ps[:, :, k] = Pf[:, :, k] + G @ (Ps[:, :, k+1] - Pp_arr[:, :, k+1]) @ G.T
# RMSE計算
skip = 20
rmse_f = np.sqrt(np.mean((xt[0, skip:]-xf[0, skip:])**2))
rmse_s = np.sqrt(np.mean((xt[0, skip:]-xs[0, skip:])**2))
improvement_rates.append((1 - rmse_s/rmse_f) * 100)
# 描画
fig, ax = plt.subplots(figsize=(10, 6))
ax.semilogx(qr_ratios, improvement_rates, 'bo-', linewidth=2, markersize=6)
ax.set_xlabel('Q/R Ratio (Process Noise / Observation Noise)', fontsize=12)
ax.set_ylabel('Position RMSE Improvement [%]', fontsize=12)
ax.set_title('Smoothing Improvement vs Q/R Ratio', fontsize=13)
ax.grid(True, alpha=0.3)
ax.axhline(y=0, color='gray', linestyle='--', alpha=0.5)
# 注釈
ax.annotate('Model accurate\n(Q small)', xy=(0.02, improvement_rates[1]),
fontsize=10, ha='center', color='green')
ax.annotate('Model uncertain\n(Q large)', xy=(50, improvement_rates[-2]),
fontsize=10, ha='center', color='red')
plt.tight_layout()
plt.savefig('rts_qr_effect.png', dpi=150, bbox_inches='tight')
plt.show()
この結果は、スムージングの改善効果がQ/R比に強く依存することを示しています。
Q/R比が小さい場合（モデルが正確で観測がノイジー）、スムーザの改善効果が最も大きくなります。モデルが正確であれば、各時刻間の状態遷移の「つながり」が強く、未来のデータの情報が過去に効果的に伝播されるからです。逆にQ/R比が大きい場合（モデルが不正確）、各時刻の状態がほぼ独立に振る舞い、未来の情報が過去の推定にほとんど寄与しません。極端にQが大きいと、各時刻の観測値がほぼそのまま最良の推定であり、スムーザの出番はありません。
RTSスムーザの計算コストとメモリ
RTSスムーザの実装に当たって、計算コストとメモリの要件を整理しておくことは重要です。
計算コスト
RTSスムーザは順方向パス（カルマンフィルタ）と逆方向パスの2パスで構成されます。状態次元を $n$、観測次元を $p$、系列長を $T$ とすると、各パスの計算量は以下の通りです。
| 処理 | 計算量 |
|---|---|
| 順方向パス（KF） | $O(T \cdot n^3)$ |
| 逆方向パス（RTS） | $O(T \cdot n^3)$ |
| 合計 | $O(T \cdot n^3)$ |
逆方向パスの主な計算は、各時刻でのスムーザゲイン $\bm{G}_k = \bm{P}_{k|k}\bm{A}^T\bm{P}_{k+1|k}^{-1}$ の計算（逆行列を含む $O(n^3)$）です。したがって、RTSスムーザの総計算量はカルマンフィルタの約2倍です。「たった2倍のコストで推定精度が大幅に向上する」という意味で、RTSスムーザはコストパフォーマンスに優れた手法です。
メモリ要件
逆方向パスを実行するためには、順方向パスの結果を保存しておく必要があります。各時刻で保存すべき量は以下の通りです。
- フィルタリング推定 $\hat{\bm{x}}_{k|k}$: $n$ 個の浮動小数点数
- フィルタリング共分散 $\bm{P}_{k|k}$: $n^2$ 個（対称性を利用すれば $n(n+1)/2$ 個）
- 予測推定 $\hat{\bm{x}}_{k+1|k}$: $n$ 個
- 予測共分散 $\bm{P}_{k+1|k}$: $n^2$ 個
合計で $T \times O(n^2)$ のメモリが必要です。状態次元 $n = 10$、系列長 $T = 10^6$（1 MHzサンプリングで1秒間のデータ）の場合、倍精度浮動小数点で約800 MBのメモリが必要になります。非常に長い系列では、固定区間ごとに分割してスムージングを行う分割スムーザ（block smoother）や、メモリ効率の良いアルゴリズム（two-filter smoother）が用いられることもあります。
非線形システムへの拡張
RTSスムーザは線形ガウスシステムを前提としていますが、非線形システムにも拡張できます。
拡張カルマンスムーザ（EKS）
EKFをベースにしたスムーザです。順方向パスでEKFを走らせ、逆方向パスではEKFが計算したヤコビアン $\bm{F}_k$ を使ってスムーザゲインを計算します。
$$ \bm{G}_k = \bm{P}_{k|k} \bm{F}_k^T \bm{P}_{k+1|k}^{-1} $$
$$ \hat{\bm{x}}_{k|T} = \hat{\bm{x}}_{k|k} + \bm{G}_k(\hat{\bm{x}}_{k+1|T} – \hat{\bm{x}}_{k+1|k}) $$
ここで $\hat{\bm{x}}_{k+1|k} = f(\hat{\bm{x}}_{k|k}, \bm{u}_k)$ は非線形関数による予測です。EKSの精度はEKFの精度に依存するため、非線形性が強い場合は限界があります。
無香カルマンスムーザ（UKS）
UKFをベースにしたスムーザで、スムーザゲインの計算にもシグマポイントを用います。順方向パスで保存したUKFの予測シグマポイントから、状態と予測状態の相互共分散を計算し、スムーザゲインに使います。
$$ \bm{D}_k = \sum_{i=0}^{2n} W_i^{(c)} (\mathcal{X}_i^{(k)} – \hat{\bm{x}}_{k|k})(\mathcal{X}_i^{(k+1|k)} – \hat{\bm{x}}_{k+1|k})^T $$
$$ \bm{G}_k = \bm{D}_k \bm{P}_{k+1|k}^{-1} $$
UKSは EKSよりも非線形性に対して頑健で、ヤコビアンの計算も不要です。
適用上の注意
非線形スムーザを適用する際には、以下の点に注意が必要です。
-
メモリ要件: 順方向パスの全結果を保存する必要があるため、長い系列ではメモリ使用量が問題になることがあります。$T$ ステップの系列で $n$ 次元状態の場合、$O(Tn^2)$ のメモリが必要です。
-
反復スムージング: 非線形性が強い場合、1パスのスムーザでは不十分なことがあります。スムージング結果を初期値として再度フィルタリング→スムージングを繰り返す反復スムーザ（IEKS, IUKS）が有効な場合があります。
-
EMアルゴリズムとの組み合わせ: スムーザは状態空間モデルのパラメータ推定（EMアルゴリズムのEステップ）に不可欠です。Eステップでスムーザを走らせて状態の期待十分統計量を計算し、Mステップでパラメータを更新します。
まとめ
本記事では、全時刻の観測データを使って過去の状態を最適に推定するRauch-Tung-Striebel（RTS）スムーザを解説しました。
- フィルタリング vs スムージング: フィルタリングは現在までの観測のみを使い、スムージングは全期間の観測を使う。スムージングの誤差共分散は必ずフィルタリング以下（$\bm{P}_{k|T} \leq \bm{P}_{k|k}$）
- RTSの2パス構造: 順方向パス（カルマンフィルタ）で中間結果を保存し、逆方向パスでスムーザゲインを使って推定を修正する
- スムーザゲイン: $\bm{G}_k = \bm{P}_{k|k}\bm{A}^T\bm{P}_{k+1|k}^{-1}$ は「未来の情報をどの程度過去に伝播させるか」を決定する。カルマンゲインが「観測からの修正」を決めるのと対称的な役割
- 改善効果: Q/R比が小さい（モデルが正確で観測がノイジー）ほど、スムージングの改善が大きい。直接観測されない状態変数（速度など）で改善が特に顕著
- 非線形への拡張: EKF/UKFをベースとしたEKS/UKSが利用可能。スムーザゲインの計算にヤコビアン（EKS）またはシグマポイント（UKS）を使用
RTSスムーザは、リアルタイム性が不要な全ての状態推定問題で、フィルタリングの結果を「無料で」改善できる強力な手法です。軌道決定、信号処理、時系列分析において、データの事後処理にはスムーザの適用を常に検討すべきです。
次のステップとして、以下の記事も参考にしてください。
- カルマンフィルタと最適推定の制御応用 — 線形カルマンフィルタの理論
- 拡張カルマンフィルタ（EKF） — 非線形フィルタリング
- 無香カルマンフィルタ（UKF） — シグマポイントによる非線形フィルタリング
- パーティクルフィルタ — 一般的な非線形・非ガウスフィルタリング
- 状態空間モデルと時系列分析 — 時系列への応用