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

import numpy as np

def cubature_points_std(n):
    """標準座標系（mu=0, P=I）でのキュバチャ点"""
    pts = np.zeros((2 * n, n))
    for i in range(n):
        pts[i, i] = np.sqrt(n)
        pts[n + i, i] = -np.sqrt(n)
    return pts

for n in [2, 3, 5, 8]:
    pts = cubature_points_std(n)
    w = 1.0 / (2 * n)

    cov_approx = w * (pts.T @ pts)          # 2次モーメント（単位行列に一致するはず）
    m4_approx = w * np.sum(pts[:, 0] ** 4)  # 4次モーメント E[z_1^4]

    print(f"n={n}: max|Cov - I| = {np.abs(cov_approx - np.eye(n)).max():.2e}, "
          f"cubature E[z1^4] = {m4_approx:.4f}  (exact Gaussian value = 3.0)")

n=2: max|Cov - I| = 2.22e-16, cubature E[z1^4] = 2.0000  (exact Gaussian value = 3.0)
n=3: max|Cov - I| = 2.22e-16, cubature E[z1^4] = 3.0000  (exact Gaussian value = 3.0)
n=5: max|Cov - I| = 2.22e-16, cubature E[z1^4] = 5.0000  (exact Gaussian value = 3.0)
n=8: max|Cov - I| = 2.22e-16, cubature E[z1^4] = 8.0000  (exact Gaussian value = 3.0)

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}")

import numpy as np

# --- レンジ・ベアリング観測モデル（UKFの記事と同一） ---
def state_transition(x, dt):
    px, py, vx, vy = x
    return np.array([px + vx * dt, py + vy * dt, vx, vy])

def observation(x):
    px, py = x[0], x[1]
    r = np.sqrt(px**2 + py**2)
    theta = np.arctan2(py, px)
    return np.array([r, theta])

class CKF:
    def __init__(self, n, m, f, h, Q, R):
        self.n, self.m, self.f, self.h = n, m, f, h
        self.Q, self.R = Q, R
        self.w = 1.0 / (2 * n)

    def cubature_points(self, x, P):
        L = np.linalg.cholesky(self.n * P)
        pts = np.zeros((2 * self.n, self.n))
        for i in range(self.n):
            pts[i] = x + L[:, i]
            pts[self.n + i] = x - L[:, i]
        return pts

    def predict(self, x, P, dt):
        X = self.cubature_points(x, P)
        X_pred = np.array([self.f(s, dt) for s in X])
        x_pred = self.w * X_pred.sum(axis=0)
        P_pred = self.Q.copy()
        for i in range(2 * self.n):
            d = X_pred[i] - x_pred
            P_pred += self.w * np.outer(d, d)
        return x_pred, P_pred

    def update(self, x_pred, P_pred, z):
        X = self.cubature_points(x_pred, P_pred)  # 式7: 予測共分散から再生成
        Y = np.array([self.h(s) for s in X])
        y_pred = self.w * Y.sum(axis=0)
        S = self.R.copy()
        C = np.zeros((self.n, self.m))
        for i in range(2 * self.n):
            dy = Y[i] - y_pred
            dy[1] = (dy[1] + np.pi) % (2 * np.pi) - np.pi
            dx = X[i] - x_pred
            S += self.w * np.outer(dy, dy)
            C += self.w * np.outer(dx, dy)
        y = z - y_pred
        y[1] = (y[1] + np.pi) % (2 * np.pi) - np.pi
        K = C @ np.linalg.inv(S)
        return x_pred + K @ y, P_pred - K @ S @ K.T


class UKF:
    """UKFの記事と同一の実装（更新ステップは予測済みシグマ点を再利用）"""
    def __init__(self, n, m, f, h, Q, R, alpha=1.0, beta=2, kappa=0):
        self.n, self.m, self.f, self.h = n, m, f, h
        self.Q, self.R = Q, R
        self.lam = alpha**2 * (n + kappa) - n
        self.gamma = np.sqrt(n + self.lam)
        self.Wm = np.full(2 * n + 1, 1 / (2 * (n + self.lam)))
        self.Wc = np.full(2 * n + 1, 1 / (2 * (n + self.lam)))
        self.Wm[0] = self.lam / (n + self.lam)
        self.Wc[0] = self.lam / (n + self.lam) + (1 - alpha**2 + beta)

    def sigma_points(self, x, P):
        L = np.linalg.cholesky(P)
        sigmas = np.zeros((2 * self.n + 1, self.n))
        sigmas[0] = x
        for i in range(self.n):
            sigmas[i + 1] = x + self.gamma * L[:, i]
            sigmas[i + 1 + self.n] = x - self.gamma * L[:, i]
        return sigmas

    def predict(self, x, P, dt):
        sigmas = self.sigma_points(x, P)
        sigmas_pred = np.array([self.f(s, dt) for s in sigmas])
        x_pred = np.dot(self.Wm, sigmas_pred)
        P_pred = self.Q.copy()
        for i in range(2 * self.n + 1):
            d = sigmas_pred[i] - x_pred
            P_pred += self.Wc[i] * np.outer(d, d)
        return x_pred, P_pred, sigmas_pred

    def update(self, x_pred, P_pred, sigmas_pred, z):
        sigmas_z = np.array([self.h(s) for s in sigmas_pred])
        z_pred = np.dot(self.Wm, sigmas_z)
        Pzz = self.R.copy()
        Pxz = np.zeros((self.n, self.m))
        for i in range(2 * self.n + 1):
            dz = sigmas_z[i] - z_pred
            dz[1] = (dz[1] + np.pi) % (2 * np.pi) - np.pi
            dx = sigmas_pred[i] - x_pred
            Pzz += self.Wc[i] * np.outer(dz, dz)
            Pxz += self.Wc[i] * np.outer(dx, dz)
        y = z - z_pred
        y[1] = (y[1] + np.pi) % (2 * np.pi) - np.pi
        K = Pxz @ np.linalg.inv(Pzz)
        return x_pred + K @ y, P_pred - K @ Pzz @ K.T


np.random.seed(42)
dt = 1.0
T = 80
t = np.arange(0, T + 1) * dt
px_true = np.full(T + 1, 1.0)
py_true = -10.0 + 0.25 * t
true_states = np.column_stack([px_true, py_true, np.zeros(T + 1), np.full(T + 1, 0.25)])
R = np.diag([0.3, 0.2])
Q = np.diag([0.05, 0.05, 0.01, 0.01])
measurements = np.array([
    observation(true_states[k]) + np.random.multivariate_normal([0, 0], R)
    for k in range(1, T + 1)
])
x0 = np.array([6.0, -3.0, 0.0, 0.1])
P0 = np.diag([16.0, 16.0, 1.0, 1.0])

def run_ukf(alpha=1.0, beta=2, kappa=0):
    ukf = UKF(4, 2, state_transition, observation, Q, R, alpha, beta, kappa)
    x, P = x0.copy(), P0.copy()
    est = [x.copy()]
    for k in range(T):
        x_pred, P_pred, sigmas_pred = ukf.predict(x, P, dt)
        x, P = ukf.update(x_pred, P_pred, sigmas_pred, measurements[k].copy())
        est.append(x.copy())
    return np.array(est)

def run_ckf():
    ckf = CKF(4, 2, state_transition, observation, Q, R)
    x, P = x0.copy(), P0.copy()
    est = [x.copy()]
    for k in range(T):
        x_pred, P_pred = ckf.predict(x, P, dt)
        x, P = ckf.update(x_pred, P_pred, measurements[k].copy())
        est.append(x.copy())
    return np.array(est)

def rmse(est):
    return np.sqrt(np.mean(np.sum((true_states[1:, :2] - est[1:, :2])**2, axis=1)))

ukf_est = run_ukf(alpha=1.0, beta=2, kappa=0)
ckf_est = run_ckf()
print(f"UKF Position RMSE: {rmse(ukf_est):.4f}")
print(f"CKF Position RMSE: {rmse(ckf_est):.4f}")
print(f"Relative difference: {(rmse(ckf_est) - rmse(ukf_est)) / rmse(ukf_est) * 100:.2f}%")

UKF Position RMSE: 1.5489
CKF Position RMSE: 1.5673
Relative difference: 1.19%

# beta=0 にすると理論的にはCKFに一致するはずだが、
# UKFの更新ステップは予測済みシグマ点を再利用する規約のままなので、
# CKF（キュバチャ点を再生成する規約）とは依然として規約が食い違う
ukf_b0_est = run_ukf(alpha=1.0, beta=0, kappa=0)
print(f"[規約が不一致] UKF(a=1,b=0,k=0, 再利用) vs CKF(再生成): "
      f"最大絶対誤差 = {np.abs(ukf_b0_est - ckf_est).max():.4f}")


class UKF_regen(UKF):
    """更新ステップでも予測共分散からシグマ点を再生成する変種（CKFの式7と同じ規約）"""
    def update(self, x_pred, P_pred, sigmas_pred_unused, z):
        sigmas = self.sigma_points(x_pred, P_pred)
        sigmas_z = np.array([self.h(s) for s in sigmas])
        z_pred = np.dot(self.Wm, sigmas_z)
        Pzz = self.R.copy()
        Pxz = np.zeros((self.n, self.m))
        for i in range(2 * self.n + 1):
            dz = sigmas_z[i] - z_pred
            dz[1] = (dz[1] + np.pi) % (2 * np.pi) - np.pi
            dx = sigmas[i] - x_pred
            Pzz += self.Wc[i] * np.outer(dz, dz)
            Pxz += self.Wc[i] * np.outer(dx, dz)
        y = z - z_pred
        y[1] = (y[1] + np.pi) % (2 * np.pi) - np.pi
        K = Pxz @ np.linalg.inv(Pzz)
        return x_pred + K @ y, P_pred - K @ Pzz @ K.T


def run_ukf_regen(alpha=1.0, beta=0, kappa=0):
    ukf = UKF_regen(4, 2, state_transition, observation, Q, R, alpha, beta, kappa)
    x, P = x0.copy(), P0.copy()
    est = [x.copy()]
    for k in range(T):
        x_pred, P_pred, sigmas_pred = ukf.predict(x, P, dt)
        x, P = ukf.update(x_pred, P_pred, sigmas_pred, measurements[k].copy())
        est.append(x.copy())
    return np.array(est)

ukf_regen_est = run_ukf_regen(alpha=1.0, beta=0, kappa=0)
print(f"[規約が一致]   UKF(a=1,b=0,k=0, 再生成) vs CKF(再生成): "
      f"最大絶対誤差 = {np.abs(ukf_regen_est - ckf_est).max():.10f}")

[規約が不一致] UKF(a=1,b=0,k=0, 再利用) vs CKF(再生成): 最大絶対誤差 = 0.1419
[規約が一致]   UKF(a=1,b=0,k=0, 再生成) vs CKF(再生成): 最大絶対誤差 = 0.0000000000

def ukf_weight0_cov(n, alpha=1.0, beta=2.0):
    kappa = 3 - n
    lam = alpha**2 * (n + kappa) - n
    W0_cov = lam / (n + lam) + (1 - alpha**2 + beta)
    Wi = 1.0 / (2 * (n + lam))
    return W0_cov, Wi

print("kappa=3-n, alpha=1, beta=2 の場合:")
for n in [2, 4, 6, 8, 9, 10, 15, 20, 30, 50]:
    W0_cov, Wi = ukf_weight0_cov(n)
    print(f"  n={n:3d}: W0_cov={W0_cov:8.4f}  Wi={Wi:.5f}  CKFの重み=1/(2n)={1/(2*n):.5f}")

kappa=3-n, alpha=1, beta=2 の場合:
  n=  2: W0_cov=  2.3333  Wi=0.16667  CKFの重み=1/(2n)=0.25000
  n=  4: W0_cov=  1.6667  Wi=0.16667  CKFの重み=1/(2n)=0.12500
  n=  6: W0_cov=  1.0000  Wi=0.16667  CKFの重み=1/(2n)=0.08333
  n=  8: W0_cov=  0.3333  Wi=0.16667  CKFの重み=1/(2n)=0.06250
  n=  9: W0_cov=  0.0000  Wi=0.16667  CKFの重み=1/(2n)=0.05556
  n= 10: W0_cov= -0.3333  Wi=0.16667  CKFの重み=1/(2n)=0.05000
  n= 15: W0_cov= -2.0000  Wi=0.16667  CKFの重み=1/(2n)=0.03333
  n= 20: W0_cov= -3.6667  Wi=0.16667  CKFの重み=1/(2n)=0.02500
  n= 30: W0_cov= -7.0000  Wi=0.16667  CKFの重み=1/(2n)=0.01667
  n= 50: W0_cov=-13.6667  Wi=0.16667  CKFの重み=1/(2n)=0.01000

import numpy as np

np.random.seed(0)
n = 20
kappa = -(n + 5)
alpha, beta = 1.0, 2.0
lam = alpha**2 * (n + kappa) - n
print(f"n={n}, kappa={kappa}: n+kappa={n + kappa}, lambda={lam}, n+lambda={n + lam}")

# ランダムだが正定値な共分散行列を用意
A = np.random.randn(n, n) * 0.3
P = A @ A.T + np.eye(n) * 0.5
print("P は正定値か:", np.all(np.linalg.eigvalsh(P) > 0))

# UKF: sqrt((n+lambda) P) が必要
M_ukf = (n + lam) * P
eigs_ukf = np.linalg.eigvalsh(M_ukf)
print(f"(n+lambda)P の固有値範囲: [{eigs_ukf.min():.3f}, {eigs_ukf.max():.3f}]")
try:
    np.linalg.cholesky(M_ukf)
    print("UKF: コレスキー分解に成功")
except np.linalg.LinAlgError as e:
    print(f"UKF: コレスキー分解が失敗 -> {e}")

# CKF: sqrt(n P) は P が正定値である限り常に存在
M_ckf = n * P
eigs_ckf = np.linalg.eigvalsh(M_ckf)
print(f"nP の固有値範囲: [{eigs_ckf.min():.3f}, {eigs_ckf.max():.3f}]")
L_ckf = np.linalg.cholesky(M_ckf)
print(f"CKF: コレスキー分解に成功, 形状={L_ckf.shape}")

n=20, kappa=-25: n+kappa=-5, lambda=-25.0, n+lambda=-5.0
P は正定値か: True
(n+lambda)P の固有値範囲: [-35.120, -2.597]
UKF: コレスキー分解が失敗 -> Matrix is not positive definite
nP の固有値範囲: [10.387, 140.480]
CKF: コレスキー分解に成功, 形状=(20, 20)