書記が数学やるだけ#937 カルマンフィルタと平滑化
カルマンフィルタと平滑化について，具体例を見ておく。
問題
説明
カルマンフィルタは、動的システムの各時刻において、直前の推定値と現時刻の観測情報のみを用いて状態を逐次的に更新する、オンライン推定アルゴリズムである。推定軌跡は真の状態に対して追従的な挙動を示す特性を持つ。
Rauch-Tung-Striebel（RTS）平滑化は、全観測期間 t∈[1,T] のデータを得た後、最終時刻から過去方向へ遡及的に計算を行うオフライン推定手法である。前方フィルタリングによって算出された各時刻の推定値に対し、将来時刻からの残差情報をフィードバックすることで、状態の事後分布を再構成する。
解答
カルマンフィルタによる推定は以下の手順で行われる：
(1)予測ステップ: 予測推定値 と予測誤差共分散行列
(2)更新ステップ（中間指標）: 観測残差、残差共分散、カルマンゲイン
(3)更新ステップ（最終結果）: 更新された状態推定値と誤差共分散行列
特に最適カルマンゲインは以下のようにして表される。
一方で平滑化では，時刻k+1の平滑化結果から，時刻kの平滑化利得行列および平滑化推定値を求める。
Pythonによるコードを以下に示しておく。
import numpy as np
import matplotlib.pyplot as plt
# --- 1. 設定 ---
dt = 1.0
F = np.array([[1, dt], [0, 1]])
H = np.array([[1, 0]])
Q = np.array([[0.05, 0], [0, 0.05]])
R = np.array([[2.0]])
steps = 50
x_true = np.array([[0], [1]]) # 初期位置0, 速度1
x_est = np.array([[0], [1]])
P = np.eye(2)
# データを蓄積するリスト
true_hist = []
obs_hist = []
kf_est_hist = [] # 前方フィルタのみ
kf_P_hist = [] # 誤差共分散
kf_pred_hist = [] # 予測値 (平滑化で使用)
kf_P_pred_hist = [] # 予測誤差共分散 (平滑化で使用)
# --- 2. 前方フィルタリング (Kalman Filter) ---
for k in range(steps):
# 真の値を生成
x_true = F @ x_true + np.random.multivariate_normal([0, 0], Q).reshape(2, 1)
z = H @ x_true + np.random.normal(0, np.sqrt(R))
# 予測
x_pred = F @ x_est
P_pred = F @ P @ F.T + Q
# 保存 (平滑化のために予測値を記録しておく)
kf_pred_hist.append(x_pred)
kf_P_pred_hist.append(P_pred)
# 更新
y = z - H @ x_pred
S = H @ P_pred @ H.T + R
K = P_pred @ H.T @ np.linalg.inv(S)
x_est = x_pred + K @ y
P = (np.eye(2) - K @ H) @ P_pred
# 保存
true_hist.append(x_true.flatten())
obs_hist.append(z.flatten())
kf_est_hist.append(x_est.flatten())
kf_P_hist.append(P)
# --- 3. RTS平滑化 (RTS Smoothing) ---
s_est_hist = [None] * steps
s_est_hist[-1] = kf_est_hist[-1] # 最終時刻はKFと同じ
s_P_hist = [None] * steps
s_P_hist[-1] = kf_P_hist[-1]
for k in range(steps - 2, -1, -1):
# 平滑化利得 C_k = P_k|k * F.T * P_k+1|k^-1
C = kf_P_hist[k] @ F.T @ np.linalg.inv(kf_P_pred_hist[k+1])
# 推定値の更新: x_k|T = x_k|k + C * (x_k+1|T - x_k+1|k)
s_est = kf_est_hist[k].reshape(2,1) + C @ (s_est_hist[k+1].reshape(2,1) - kf_pred_hist[k+1])
s_est_hist[k] = s_est.flatten()
# 誤差共分散の更新 (オプション)
s_P = kf_P_hist[k] + C @ (s_P_hist[k+1] - kf_P_pred_hist[k+1]) @ C.T
s_P_hist[k] = s_P
s_est_hist = np.array(s_est_hist)
true_hist = np.array(true_hist)
kf_est_hist = np.array(kf_est_hist)
# --- 4. 可視化 ---
plt.figure(figsize=(12, 6))
plt.plot(true_hist[:, 0], 'g--', label='True Position', alpha=0.5)
plt.scatter(range(steps), obs_hist, color='red', s=10, alpha=0.3, label='Observed')
plt.plot(kf_est_hist[:, 0], 'b', label='Kalman Filter (Forward)')
plt.plot(s_est_hist[:, 0], 'm', linewidth=2, label='RTS Smoother (Backward)')
plt.title('Kalman Filter vs RTS Smoother')
plt.legend()
plt.grid(True)
plt.show()
本記事のもくじはこちら：
