カルマンスムーザ（RTS Smoother）の理論とPython実装

import numpy as np
import matplotlib.pyplot as plt

# ---- 状態空間モデルの定義 ----
dt = 1.0  # サンプリング間隔

# 状態遷移行列（等速直線運動モデル）
F = np.array([[1, dt],
              [0, 1]])

# 観測行列（位置のみ観測）
H = np.array([[1, 0]])

# プロセスノイズ共分散
q = 0.1
Q = q * np.array([[dt**3/3, dt**2/2],
                   [dt**2/2, dt]])

# 観測ノイズ共分散
R = np.array([[1.0]])

n = 2  # 状態次元
m = 1  # 観測次元

def kalman_filter(y_obs, F, H, Q, R, x0, P0):
    """カルマンフィルタのフォワードパス（中間結果をすべて保存）"""
    T = len(y_obs)
    n = len(x0)

    # フィルタ推定値の保存
    x_filt = np.zeros((T + 1, n))  # x_{k|k}
    P_filt = np.zeros((T + 1, n, n))  # P_{k|k}

    # 予測値の保存（スムーザで使用）
    x_pred = np.zeros((T, n))  # x_{k|k-1}
    P_pred = np.zeros((T, n, n))  # P_{k|k-1}

    # 初期化
    x_filt[0] = x0
    P_filt[0] = P0

    for k in range(T):
        # 予測ステップ（式3, 4）
        x_pred[k] = F @ x_filt[k]
        P_pred[k] = F @ P_filt[k] @ F.T + Q

        # 更新ステップ（式5, 6, 7）
        S = H @ P_pred[k] @ H.T + R
        K = P_pred[k] @ H.T @ np.linalg.inv(S)
        innovation = y_obs[k] - H @ x_pred[k]
        x_filt[k + 1] = x_pred[k] + K @ innovation
        P_filt[k + 1] = (np.eye(n) - K @ H) @ P_pred[k]

    return x_filt, P_filt, x_pred, P_pred

def rts_smoother(x_filt, P_filt, x_pred, P_pred, F):
    """RTSスムーザのバックワードパス"""
    T = len(x_pred)
    n = x_filt.shape[1]

    # 平滑化推定値の保存
    x_smooth = np.zeros((T + 1, n))
    P_smooth = np.zeros((T + 1, n, n))

    # 初期条件：最終時刻のフィルタ推定値
    x_smooth[T] = x_filt[T]
    P_smooth[T] = P_filt[T]

    # バックワード再帰
    for k in range(T - 1, -1, -1):
        # スムーザゲイン（式8）
        G = P_filt[k] @ F.T @ np.linalg.inv(P_pred[k])

        # 平滑化状態推定（式9）
        x_smooth[k] = x_filt[k] + G @ (x_smooth[k + 1] - x_pred[k])

        # 平滑化共分散（式10）
        P_smooth[k] = P_filt[k] + G @ (P_smooth[k + 1] - P_pred[k]) @ G.T

    return x_smooth, P_smooth

np.random.seed(42)
T = 50  # タイムステップ数

# 真の初期状態
x_true_init = np.array([0.0, 1.0])  # 位置0, 速度1

# 真の状態の生成
true_states = np.zeros((T + 1, n))
true_states[0] = x_true_init
for k in range(T):
    true_states[k + 1] = F @ true_states[k] + \
        np.random.multivariate_normal(np.zeros(n), Q)

# 観測の生成
y_obs = np.zeros((T, m))
for k in range(T):
    y_obs[k] = H @ true_states[k + 1] + \
        np.random.multivariate_normal(np.zeros(m), R)

# フィルタの初期推定
x0 = np.array([0.0, 0.0])
P0 = np.diag([1.0, 1.0])

# カルマンフィルタの実行
x_filt, P_filt, x_pred, P_pred = kalman_filter(y_obs, F, H, Q, R, x0, P0)

# RTSスムーザの実行
x_smooth, P_smooth = rts_smoother(x_filt, P_filt, x_pred, P_pred, F)

time = np.arange(T + 1)

# ---- 位置の推定結果 ----
plt.figure(figsize=(12, 8))

plt.subplot(2, 1, 1)
plt.plot(time, true_states[:, 0], "b-", linewidth=2, label="True state")
plt.scatter(np.arange(1, T + 1), y_obs[:, 0],
            c="gray", s=15, alpha=0.5, label="Measurements")
plt.plot(time, x_filt[:, 0], "r--", linewidth=1.5, label="KF estimate")
plt.plot(time, x_smooth[:, 0], "g-", linewidth=1.5, label="RTS estimate")
plt.xlabel("Time step")
plt.ylabel("Position")
plt.title("Position Estimation: Kalman Filter vs RTS Smoother")
plt.legend()
plt.grid(True)

# ---- 位置推定の誤差共分散 ----
plt.subplot(2, 1, 2)
kf_pos_var = np.array([P_filt[k, 0, 0] for k in range(T + 1)])
rts_pos_var = np.array([P_smooth[k, 0, 0] for k in range(T + 1)])
plt.plot(time, kf_pos_var, "r--", linewidth=1.5, label="KF variance")
plt.plot(time, rts_pos_var, "g-", linewidth=1.5, label="RTS variance")
plt.xlabel("Time step")
plt.ylabel("Position variance")
plt.title("Estimation Error Covariance Comparison")
plt.legend()
plt.grid(True)

plt.tight_layout()
plt.savefig("rts_smoother_result.png", dpi=150)
plt.show()

# 位置のRMSE
rmse_kf = np.sqrt(np.mean((true_states[1:, 0] - x_filt[1:, 0])**2))
rmse_rts = np.sqrt(np.mean((true_states[1:, 0] - x_smooth[1:, 0])**2))

# 速度のRMSE
rmse_kf_vel = np.sqrt(np.mean((true_states[1:, 1] - x_filt[1:, 1])**2))
rmse_rts_vel = np.sqrt(np.mean((true_states[1:, 1] - x_smooth[1:, 1])**2))

print(f"Position RMSE - KF: {rmse_kf:.4f}, RTS: {rmse_rts:.4f}")
print(f"Velocity RMSE - KF: {rmse_kf_vel:.4f}, RTS: {rmse_rts_vel:.4f}")
print(f"Position improvement: {(1 - rmse_rts / rmse_kf) * 100:.1f}%")
print(f"Velocity improvement: {(1 - rmse_rts_vel / rmse_kf_vel) * 100:.1f}%")