Cubature Kalman Filter（CKF）の理論とPython実装

import numpy as np
import scipy.linalg
import matplotlib.pyplot as plt

# ---- 状態空間モデルの定義 ----
dt = 1.0
omega = 0.05  # 旋回角速度 [rad/s]

def f(x):
    """非線形状態遷移関数（等速旋回運動モデル）"""
    px, py, v, theta = x
    if abs(omega) < 1e-6:
        return np.array([px + v * np.cos(theta) * dt,
                         py + v * np.sin(theta) * dt,
                         v, theta])
    return np.array([
        px + v / omega * (np.sin(theta + omega * dt) - np.sin(theta)),
        py - v / omega * (np.cos(theta + omega * dt) - np.cos(theta)),
        v,
        theta + omega * dt
    ])

def h(x):
    """観測関数（位置のみ観測可能）"""
    return np.array([x[0], x[1]])

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

# プロセスノイズと観測ノイズ
Q = np.diag([0.1, 0.1, 0.01, 0.001])
R = np.diag([1.0, 1.0])

def generate_cubature_points(mu, P, n):
    """キュバチャポイントの生成（式2）"""
    num_points = 2 * n
    sqrt_P = scipy.linalg.cholesky(n * P, lower=True)

    points = np.zeros((n, num_points))
    for i in range(n):
        points[:, i] = mu + sqrt_P[:, i]       # +sqrt(nP) の列
        points[:, n + i] = mu - sqrt_P[:, i]   # -sqrt(nP) の列

    return points

def ckf_predict(x_est, P_est):
    """CKF予測ステップ"""
    # 式3: キュバチャポイント生成
    X = generate_cubature_points(x_est, P_est, n)
    num_points = 2 * n
    w = 1.0 / num_points  # 式1: 等しい重み

    # 式4: 非線形変換
    X_pred = np.zeros_like(X)
    for i in range(num_points):
        X_pred[:, i] = f(X[:, i])

    # 式5: 予測平均
    x_pred = w * np.sum(X_pred, axis=1)

    # 式6: 予測共分散
    P_pred = np.zeros((n, n))
    for i in range(num_points):
        diff = X_pred[:, i] - x_pred
        P_pred += w * np.outer(diff, diff)
    P_pred += Q

    return x_pred, P_pred

def ckf_update(x_pred, P_pred, y):
    """CKF更新ステップ"""
    # 式7: キュバチャポイント再生成
    X = generate_cubature_points(x_pred, P_pred, n)
    num_points = 2 * n
    w = 1.0 / num_points

    # 式8: 観測のキュバチャポイント
    Y = np.zeros((m, num_points))
    for i in range(num_points):
        Y[:, i] = h(X[:, i])

    # 式9: 予測観測平均
    y_pred = w * np.sum(Y, axis=1)

    # 式10: イノベーション共分散
    S = np.zeros((m, m))
    for i in range(num_points):
        dy = Y[:, i] - y_pred
        S += w * np.outer(dy, dy)
    S += R

    # 式11: 相互共分散
    C = np.zeros((n, m))
    for i in range(num_points):
        dx = X[:, i] - x_pred
        dy = Y[:, i] - y_pred
        C += w * np.outer(dx, dy)

    # 式12-14: カルマンゲインと状態更新
    K = C @ np.linalg.inv(S)
    x_est = x_pred + K @ (y - y_pred)
    P_est = P_pred - K @ S @ K.T

    return x_est, P_est

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

# 真の初期状態
x_true = np.array([0.0, 0.0, 1.0, np.pi / 2])

# CKFの初期推定
x_est = np.array([0.5, -0.5, 0.8, np.pi / 2 + 0.1])
P_est = np.diag([1.0, 1.0, 0.5, 0.1])

# 記録用配列
true_states = [x_true.copy()]
measurements = []
estimates = [x_est.copy()]

for k in range(T):
    # 真の状態の更新
    x_true = f(x_true) + np.random.multivariate_normal(np.zeros(n), Q)
    true_states.append(x_true.copy())

    # 観測の生成
    y = h(x_true) + np.random.multivariate_normal(np.zeros(m), R)
    measurements.append(y.copy())

    # CKF
    x_est, P_est = ckf_predict(x_est, P_est)
    x_est, P_est = ckf_update(x_est, P_est, y)
    estimates.append(x_est.copy())

true_states = np.array(true_states)
measurements = np.array(measurements)
estimates = np.array(estimates)

# ---- 結果のプロット ----
plt.figure(figsize=(10, 8))
plt.plot(true_states[:, 0], true_states[:, 1], "b-", label="True trajectory")
plt.scatter(measurements[:, 0], measurements[:, 1],
            c="gray", s=10, alpha=0.5, label="Measurements")
plt.plot(estimates[:, 0], estimates[:, 1], "r--", label="CKF estimate")
plt.xlabel("x")
plt.ylabel("y")
plt.title("Cubature Kalman Filter - Coordinated Turn Tracking")
plt.legend()
plt.axis("equal")
plt.grid(True)
plt.tight_layout()
plt.savefig("ckf_result.png", dpi=150)
plt.show()

# RMSE計算
rmse_x = np.sqrt(np.mean((true_states[1:, 0] - estimates[1:, 0])**2))
rmse_y = np.sqrt(np.mean((true_states[1:, 1] - estimates[1:, 1])**2))
print(f"Position RMSE: x={rmse_x:.4f}, y={rmse_y:.4f}")