カルマンスムーザ3種比較（RTS固定区間・固定ラグ・固定点）：理論・遅延と計算量のトレードオフ・Python実装

import numpy as np

# ---- 状態空間モデル：ランダムウォーク + 観測ノイズ ----
T = 300  # タイムステップ数

F = np.array([[1.0]])   # 状態遷移（ランダムウォーク）
H = np.array([[1.0]])   # 観測行列
Q = np.array([[0.05]])  # プロセスノイズ分散
R = np.array([[1.0]])   # 観測ノイズ分散

n, m = 1, 1
x0 = np.array([0.0])
P0 = np.diag([1.0])

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], P_filt[0] = x0, 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)
        x_filt[k + 1] = x_pred[k] + K @ (y_obs[k] - H @ x_pred[k])
        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], P_smooth[T] = x_filt[T], P_filt[T]
    for k in range(T - 1, -1, -1):
        G = P_filt[k] @ F.T @ np.linalg.inv(P_pred[k])  # 式8
        x_smooth[k] = x_filt[k] + G @ (x_smooth[k + 1] - x_pred[k])  # 式9
        P_smooth[k] = P_filt[k] + G @ (P_smooth[k + 1] - P_pred[k]) @ G.T  # 式10
    return x_smooth, P_smooth

def fixed_lag_smoother(y_obs, F, H, Q, R, x0, P0, N):
    """固定ラグスムーザ：各時刻 k で直近 N ステップ分だけ後向き再帰を回し
    x_{k-N|k} を出力する（窓付きRTSによる実装）"""
    T = len(y_obs)
    n = len(x0)
    x_filt, P_filt, x_pred, P_pred = kalman_filter(y_obs, F, H, Q, R, x0, P0)
    x_lag = np.zeros((T + 1, n))
    x_lag[0] = x0
    for k in range(1, T + 1):
        j0 = max(0, k - N)  # 出力する時刻 k-N
        xs = x_filt[k].copy()
        for j in range(k - 1, j0 - 1, -1):  # 窓 [j0, k] で後向き再帰
            G = P_filt[j] @ F.T @ np.linalg.inv(P_pred[j])
            xs = x_filt[j] + G @ (xs - x_pred[j])
        x_lag[j0] = xs
    # 末尾 N 点はラグ分の未来観測が無いので、最終窓の後向き再帰で埋める
    xs = x_filt[T].copy()
    x_lag[T] = xs
    for j in range(T - 1, max(0, T - N) - 1, -1):
        G = P_filt[j] @ F.T @ np.linalg.inv(P_pred[j])
        xs = x_filt[j] + G @ (xs - x_pred[j])
        x_lag[j] = xs
    return x_lag

def fixed_lag_augmented(y_obs, F, H, Q, R, x0, P0, N):
    """Biswas-Mahalanabis 型：拡大状態 z_k = [x_k; x_{k-1}; ...; x_{k-N}] に
    対して通常のカルマンフィルタを回し、末尾ブロック（= x_{k-N|k}）を読み出す"""
    n = len(x0)
    T = len(y_obs)
    B = N + 1
    A = np.zeros((B * n, B * n))
    A[0:n, 0:n] = F                                    # 先頭ブロックだけ状態遷移
    for i in range(1, B):
        A[i * n:(i + 1) * n, (i - 1) * n:i * n] = np.eye(n)  # 残りはシフト（1つ前のブロックをコピー）
    Q_aug = np.zeros((B * n, B * n))
    Q_aug[0:n, 0:n] = Q                                 # プロセスノイズは先頭ブロックにのみ入る
    H_aug = np.zeros((H.shape[0], B * n))
    H_aug[:, 0:n] = H                                   # 観測は先頭ブロック（x_k）のみ
    z0 = np.tile(x0, B)
    P0_aug = np.tile(P0, (B, B))                        # 初期時刻は全ブロックが x0 に完全相関

    z_filt = np.zeros((T + 1, B * n))
    Pz_filt = np.zeros((T + 1, B * n, B * n))
    z_filt[0], Pz_filt[0] = z0, P0_aug
    for k in range(T):
        z_pred = A @ z_filt[k]
        P_pred_aug = A @ Pz_filt[k] @ A.T + Q_aug
        S = H_aug @ P_pred_aug @ H_aug.T + R
        K = P_pred_aug @ H_aug.T @ np.linalg.inv(S)
        z_filt[k + 1] = z_pred + K @ (y_obs[k] - H_aug @ z_pred)
        Pz_filt[k + 1] = (np.eye(B * n) - K @ H_aug) @ P_pred_aug

    x_lag = np.zeros((T + 1, n))
    for k in range(T + 1):
        j0 = max(0, k - N)
        depth = k - j0                                  # 時刻 k における x_{j0} の格納ブロック
        x_lag[j0] = z_filt[k, depth * n:(depth + 1) * n]
    for i in range(B):                                  # 末尾 N 点は最終フィルタ結果から直接読み出す
        x_lag[T - i] = z_filt[T, i * n:(i + 1) * n]
    return x_lag


rng_eq = np.random.default_rng(99)
x_true_eq = np.zeros((T + 1, n))
y_obs_eq = np.zeros((T, m))
for k in range(T):
    x_true_eq[k + 1] = F @ x_true_eq[k] + rng_eq.multivariate_normal(np.zeros(n), Q)
    y_obs_eq[k] = H @ x_true_eq[k + 1] + rng_eq.multivariate_normal(np.zeros(m), R)

for N in [1, 2, 3, 5]:
    x_lag_win = fixed_lag_smoother(y_obs_eq, F, H, Q, R, x0, P0, N)
    x_lag_aug = fixed_lag_augmented(y_obs_eq, F, H, Q, R, x0, P0, N)
    # 起動直後は拡大状態側の初期パディング（全ブロックをx0で埋める近似）が
    # 尾を引くため、k >= N+5 で比較する
    diff = np.abs(x_lag_win[N + 5:, 0] - x_lag_aug[N + 5:, 0])
    print(f"N={N}: 窓付きRTSと拡大状態の最大差 = {diff.max():.3e}, 平均差 = {diff.mean():.3e}")

N=1: 窓付きRTSと拡大状態の最大差 = 4.441e-16, 平均差 = 3.536e-17
N=2: 窓付きRTSと拡大状態の最大差 = 4.441e-16, 平均差 = 5.581e-17
N=3: 窓付きRTSと拡大状態の最大差 = 4.441e-16, 平均差 = 8.235e-17
N=5: 窓付きRTSと拡大状態の最大差 = 8.882e-16, 平均差 = 9.623e-17

def rmse(a, b):
    return np.sqrt(np.mean((a - b) ** 2))

n_trials = 50
lags = [1, 2, 3, 5, 10, 20]
rmse_kf_list, rmse_rts_list = [], []
rmse_lag_lists = {N: [] for N in lags}

rng = np.random.default_rng(42)
for trial in range(n_trials):
    # 真の状態と観測の生成
    x_true = np.zeros((T + 1, n))
    y_obs = np.zeros((T, m))
    for k in range(T):
        x_true[k + 1] = F @ x_true[k] + rng.multivariate_normal(np.zeros(n), Q)
        y_obs[k] = H @ x_true[k + 1] + rng.multivariate_normal(np.zeros(m), R)

    x_filt, P_filt, x_pred, P_pred = kalman_filter(y_obs, F, H, Q, R, x0, P0)
    x_rts, P_rts = rts_smoother(x_filt, P_filt, x_pred, P_pred, F)

    rmse_kf_list.append(rmse(x_true[1:, 0], x_filt[1:, 0]))
    rmse_rts_list.append(rmse(x_true[1:, 0], x_rts[1:, 0]))
    for N in lags:
        x_lag = fixed_lag_smoother(y_obs, F, H, Q, R, x0, P0, N)
        rmse_lag_lists[N].append(rmse(x_true[1:, 0], x_lag[1:, 0]))

rmse_kf = np.mean(rmse_kf_list)
rmse_rts = np.mean(rmse_rts_list)
print(f"KF  filter RMSE : {rmse_kf:.4f}")
for N in lags:
    r = np.mean(rmse_lag_lists[N])
    print(f"Fixed-lag N={N:2d}   : {r:.4f}  ({(1 - r / rmse_kf) * 100:.1f}% improvement)")
print(f"RTS smoother    : {rmse_rts:.4f}  ({(1 - rmse_rts / rmse_kf) * 100:.1f}% improvement)")

KF  filter RMSE : 0.4519
Fixed-lag N= 1   : 0.4145  (8.3% improvement)
Fixed-lag N= 2   : 0.3890  (13.9% improvement)
Fixed-lag N= 3   : 0.3717  (17.7% improvement)
Fixed-lag N= 5   : 0.3521  (22.1% improvement)
Fixed-lag N=10   : 0.3397  (24.8% improvement)
Fixed-lag N=20   : 0.3387  (25.0% improvement)
RTS smoother    : 0.3388  (25.0% improvement)

G_bar = 0.8  # 前段で導出した定常スムーザゲイン P_filt_ss/P_pred_ss = 0.2/0.25
mse_kf, mse_rts = rmse_kf**2, rmse_rts**2
print(f"{'N':>3s} | {'measured RMSE':>14s} | {'predicted RMSE':>15s} | {'abs diff':>9s}")
for N in lags:
    measured = np.mean(rmse_lag_lists[N])
    predicted = np.sqrt(mse_rts + (mse_kf - mse_rts) * G_bar ** (2 * N))
    print(f"{N:3d} | {measured:14.4f} | {predicted:15.4f} | {abs(measured-predicted):9.4f}")

  N |  measured RMSE |  predicted RMSE |  abs diff
  1 |         0.4145 |          0.4147 |    0.0002
  2 |         0.3890 |          0.3891 |    0.0001
  3 |         0.3717 |          0.3718 |    0.0000
  5 |         0.3521 |          0.3526 |    0.0005
 10 |         0.3397 |          0.3403 |    0.0006
 20 |         0.3387 |          0.3388 |    0.0000

import matplotlib.pyplot as plt

rng_viz = np.random.default_rng(2024)
x_true = np.zeros((T + 1, n))
y_obs = np.zeros((T, m))
for k in range(T):
    x_true[k + 1] = F @ x_true[k] + rng_viz.multivariate_normal(np.zeros(n), Q)
    y_obs[k] = H @ x_true[k + 1] + rng_viz.multivariate_normal(np.zeros(m), R)
x_filt, P_filt, x_pred, P_pred = kalman_filter(y_obs, F, H, Q, R, x0, P0)
x_rts, P_rts = rts_smoother(x_filt, P_filt, x_pred, P_pred, F)
x_lag5 = fixed_lag_smoother(y_obs, F, H, Q, R, x0, P0, N=5)

fig, axes = plt.subplots(1, 2, figsize=(13, 4.6))

# 左：時系列でのフィルタ・固定ラグ(N=5)・RTSの比較（区間 k=150..220）
ax = axes[0]
window = slice(150, 221)
tt = np.arange(T + 1)
ax.scatter(tt[window], y_obs[149:220, 0], s=12, color="#898781", alpha=0.55, label="Observations")
ax.plot(tt[window], x_true[window, 0], color="#0b0b0b", lw=2.2, label="True state")
ax.plot(tt[window], x_filt[window, 0], color="#2a78d6", lw=1.6, ls="--", label="KF filter")
ax.plot(tt[window], x_lag5[window, 0], color="#eb6834", lw=1.6, ls="-.", label="Fixed-lag (N=5)")
ax.plot(tt[window], x_rts[window, 0], color="#008300", lw=1.8, label="RTS smoother")
ax.set_xlabel("Time step $k$"); ax.set_ylabel("State $x_k$")
ax.set_title("Filtering vs. fixed-lag vs. fixed-interval (RTS) smoothing")
ax.legend(loc="upper left", fontsize=8)

# 右：ラグ長Nに対するRMSEの飽和と式(14)の理論曲線
ax = axes[1]
N_curve = np.linspace(0, 22, 200)
predicted_curve = np.sqrt(mse_rts + (mse_kf - mse_rts) * G_bar ** (2 * N_curve))
ax.axhline(rmse_kf, color="#2a78d6", lw=1.4, ls="--", label="KF filter (N=0)")
ax.axhline(rmse_rts, color="#008300", lw=1.4, label="RTS (N=inf)")
ax.plot(N_curve, predicted_curve, color="#52514e", lw=1.4, ls=":", label="eq.(14) model")
ax.scatter(lags, [np.mean(rmse_lag_lists[N]) for N in lags], color="#eb6834", s=36, label="Fixed-lag (measured)")
ax.set_xlabel("Lag length $N$"); ax.set_ylabel("RMSE (position)")
ax.set_title("Lag-length saturation: measured vs. decay-law model")
ax.legend(loc="upper right", fontsize=7.5)

fig.tight_layout()
fig.savefig("kalman_smoother_comparison.png", dpi=150)

x_lag_full = fixed_lag_smoother(y_obs, F, H, Q, R, x0, P0, N=T)
x_lag_over = fixed_lag_smoother(y_obs, F, H, Q, R, x0, P0, N=T + 100)
print(f"max |x_lag(N=T)   - x_rts| = {np.max(np.abs(x_lag_full[:, 0] - x_rts[:, 0])):.3e}")
print(f"max |x_lag(N=T+100) - x_rts| = {np.max(np.abs(x_lag_over[:, 0] - x_rts[:, 0])):.3e}")

max |x_lag(N=T)   - x_rts| = 0.000e+00
max |x_lag(N=T+100) - x_rts| = 0.000e+00

F_ar, H_ar, R_ar = 0.9, 1.0, 1.0
P = 1.0  # 恣意的な初期値（定常状態である必要はない）
for k in range(8):
    P_pred_val = F_ar**2 * P + 0.0  # Q = 0 を厳密に代入
    G_val = P * F_ar / P_pred_val
    print(f"k={k}: P_filt={P:.6e}, G_k={G_val:.6f}  (1/F = {1/F_ar:.6f})")
    S = H_ar**2 * P_pred_val + R_ar
    K = P_pred_val * H_ar / S
    P = (1 - K * H_ar) * P_pred_val

k=0: P_filt=1.000000e+00, G_k=1.111111  (1/F = 1.111111)
k=1: P_filt=4.475138e-01, G_k=1.111111  (1/F = 1.111111)
k=2: P_filt=2.660476e-01, G_k=1.111111  (1/F = 1.111111)
k=3: P_filt=1.772923e-01, G_k=1.111111  (1/F = 1.111111)
k=4: P_filt=1.255736e-01, G_k=1.111111  (1/F = 1.111111)
k=5: P_filt=9.232390e-02, G_k=1.111111  (1/F = 1.111111)
k=6: P_filt=6.957907e-02, G_k=1.111111  (1/F = 1.111111)
k=7: P_filt=5.335217e-02, G_k=1.111111  (1/F = 1.111111)

for Qval in [1e-1, 1e-3, 1e-6, 1e-9, 1e-12]:
    P = 1.0
    for _ in range(2000):  # 定常状態まで反復
        P_pred_val = F_ar**2 * P + Qval
        S = H_ar**2 * P_pred_val + R_ar
        K = P_pred_val * H_ar / S
        P = (1 - K * H_ar) * P_pred_val
    P_pred_val = F_ar**2 * P + Qval
    G_val = P * F_ar / P_pred_val
    print(f"Q={Qval:.0e}: steady-state P_filt={P:.3e}, G_bar={G_val:.8f}")

Q=1e-01: steady-state P_filt=2.153e-01, G_bar=0.70620719
Q=1e-03: steady-state P_filt=5.124e-03, G_bar=0.89538818
Q=1e-06: steady-state P_filt=5.263e-06, G_bar=0.89999526
Q=1e-09: steady-state P_filt=5.263e-09, G_bar=0.90000000
Q=1e-12: steady-state P_filt=5.263e-12, G_bar=0.90000000