粒子フィルタのPython実装：リサンプリング手法の比較

def effective_sample_size(weights):
    """有効サンプルサイズの計算"""
    return 1.0 / np.sum(weights ** 2)

def systematic_resampling(weights):
    """系統的リサンプリング"""
    N = len(weights)
    positions = (np.random.random() + np.arange(N)) / N
    cumsum = np.cumsum(weights)
    indices = np.zeros(N, dtype=int)
    i, j = 0, 0
    while i < N:
        if positions[i] < cumsum[j]:
            indices[i] = j
            i += 1
        else:
            j += 1
    return indices

def residual_resampling(weights):
    """残差リサンプリング"""
    N = len(weights)
    indices = []

    # 確定的な複製
    num_copies = (N * weights).astype(int)
    for i in range(N):
        indices.extend([i] * num_copies[i])

    # 残差の処理
    residual_weights = N * weights - num_copies
    residual_weights /= residual_weights.sum()
    num_remaining = N - len(indices)
    if num_remaining > 0:
        cumsum = np.cumsum(residual_weights)
        # 残差部分に系統的リサンプリングを適用
        positions = (np.random.random() + np.arange(num_remaining)) / num_remaining
        i, j = 0, 0
        while i < num_remaining:
            if positions[i] < cumsum[j]:
                indices.append(j)
                i += 1
            else:
                j += 1

    return np.array(indices)

def stratified_resampling(weights):
    """層化リサンプリング"""
    N = len(weights)
    positions = (np.random.random(N) + np.arange(N)) / N
    cumsum = np.cumsum(weights)
    indices = np.zeros(N, dtype=int)
    i, j = 0, 0
    while i < N:
        if positions[i] < cumsum[j]:
            indices[i] = j
            i += 1
        else:
            j += 1
    return indices

import numpy as np
import matplotlib.pyplot as plt

# ---- モデルの定義 ----
sigma_w = np.sqrt(10.0)  # プロセスノイズの標準偏差
sigma_v = np.sqrt(1.0)   # 観測ノイズの標準偏差

def state_transition(x, k):
    """状態遷移関数"""
    return x / 2.0 + 25.0 * x / (1.0 + x ** 2) + 8.0 * np.cos(1.2 * k)

def observation(x):
    """観測関数"""
    return x ** 2 / 20.0

def log_likelihood(y, x):
    """対数尤度 log p(y|x)"""
    diff = y - observation(x)
    return -0.5 * (diff ** 2) / (sigma_v ** 2)

# ---- 粒子フィルタ本体 ----
def particle_filter(y_obs, N_particles, resample_fn, resample_threshold=0.5):
    """
    粒子フィルタの実行

    Parameters
    ----------
    y_obs : array, 観測値の系列
    N_particles : int, 粒子数
    resample_fn : callable, リサンプリング関数
    resample_threshold : float, ESS閾値（N_particlesに対する比率）

    Returns
    -------
    x_est : array, 状態推定値の系列
    ess_history : array, ESSの履歴
    """
    T = len(y_obs)
    threshold = resample_threshold * N_particles

    # 初期化：事前分布からサンプリング
    particles = np.random.normal(0, np.sqrt(5.0), N_particles)
    weights = np.ones(N_particles) / N_particles

    x_est = np.zeros(T)
    ess_history = np.zeros(T)

    for k in range(T):
        # 予測：状態遷移 + プロセスノイズ
        particles = state_transition(particles, k + 1) \
                    + sigma_w * np.random.randn(N_particles)

        # 重み更新：対数尤度で計算し、オーバーフローを防ぐ
        log_w = log_likelihood(y_obs[k], particles)
        log_w -= np.max(log_w)  # 数値安定化
        weights = np.exp(log_w)
        weights /= np.sum(weights)

        # 状態推定
        x_est[k] = np.sum(weights * particles)

        # ESS計算
        ess = effective_sample_size(weights)
        ess_history[k] = ess

        # リサンプリング（ESS閾値ベース）
        if ess < threshold:
            indices = resample_fn(weights)
            particles = particles[indices]
            weights = np.ones(N_particles) / N_particles

    return x_est, ess_history

np.random.seed(42)
T = 100           # タイムステップ数
N_particles = 500  # 粒子数

# 真の状態と観測の生成
x_true = np.zeros(T)
y_obs = np.zeros(T)
x_true[0] = 0.1  # 初期状態

for k in range(1, T):
    x_true[k] = state_transition(x_true[k - 1], k) \
                + sigma_w * np.random.randn()

for k in range(T):
    y_obs[k] = observation(x_true[k]) + sigma_v * np.random.randn()

# 3種類のリサンプリング手法で粒子フィルタを実行
resamplers = {
    "Systematic": systematic_resampling,
    "Residual": residual_resampling,
    "Stratified": stratified_resampling,
}

results = {}
for name, fn in resamplers.items():
    np.random.seed(42)  # 公平な比較のため乱数シードを統一
    x_est, ess_hist = particle_filter(y_obs, N_particles, fn)
    rmse = np.sqrt(np.mean((x_true - x_est) ** 2))
    results[name] = {"estimate": x_est, "ess": ess_hist, "rmse": rmse}
    print(f"{name}: RMSE = {rmse:.4f}")

fig, axes = plt.subplots(2, 1, figsize=(12, 8), sharex=True)

# 状態推定の比較
axes[0].plot(x_true, "k-", linewidth=1.5, label="True state")
colors = {"Systematic": "tab:blue", "Residual": "tab:orange",
          "Stratified": "tab:green"}
for name, res in results.items():
    axes[0].plot(res["estimate"], "--", color=colors[name],
                 linewidth=1, label=f"{name} (RMSE={res['rmse']:.2f})")
axes[0].set_ylabel("State $x_k$")
axes[0].set_title("Particle Filter: Resampling Method Comparison")
axes[0].legend(loc="upper right", fontsize=9)
axes[0].grid(True, alpha=0.3)

# ESSの推移
for name, res in results.items():
    axes[1].plot(res["ess"], color=colors[name], linewidth=0.8, label=name)
axes[1].axhline(y=N_particles / 2, color="red", linestyle=":",
                label=f"Threshold (N/2={N_particles // 2})")
axes[1].set_xlabel("Time step $k$")
axes[1].set_ylabel("Effective Sample Size")
axes[1].legend(loc="upper right", fontsize=9)
axes[1].grid(True, alpha=0.3)

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