Unscented Kalman Filter（UKF）：非線形状態推定の理論とPython実装

import numpy as np
import matplotlib.pyplot as plt

class UKF:
    def __init__(self, n, m, f, h, Q, R, alpha=1e-3, beta=2, kappa=0):
        self.n = n
        self.m = m
        self.f = f
        self.h = h
        self.Q = Q
        self.R = 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
            dx = sigmas_pred[i] - x_pred
            Pzz += self.Wc[i] * np.outer(dz, dz)
            Pxz += self.Wc[i] * np.outer(dx, dz)

        K = Pxz @ np.linalg.inv(Pzz)
        x_new = x_pred + K @ (z - z_pred)
        P_new = P_pred - K @ Pzz @ K.T

        return x_new, P_new

# --- 等速円運動モデル ---
def state_transition(x, dt):
    px, py, vx, vy = x
    return np.array([px + vx * dt, py + vy * dt, vx, vy])

def observation(x):
    return np.array([x[0], x[1]])

# --- シミュレーション ---
np.random.seed(42)
dt = 0.1
T = 10
steps = int(T / dt)
omega = 0.5  # 角速度

true_states = np.zeros((steps, 4))
true_states[0] = [5, 0, 0, 5 * omega]
for k in range(1, steps):
    t = k * dt
    true_states[k] = [5 * np.cos(omega * t), 5 * np.sin(omega * t),
                       -5 * omega * np.sin(omega * t),
                       5 * omega * np.cos(omega * t)]

R = np.diag([0.5**2, 0.5**2])
measurements = true_states[:, :2] + np.random.multivariate_normal(
    [0, 0], R, steps)

Q = np.diag([0.01, 0.01, 0.1, 0.1])
ukf = UKF(n=4, m=2, f=state_transition, h=observation, Q=Q, R=R)

x_est = np.array([measurements[0, 0], measurements[0, 1], 0, 2.5])
P_est = np.eye(4)
estimates = [x_est.copy()]

for k in range(1, steps):
    x_pred, P_pred, sigmas_pred = ukf.predict(x_est, P_est, dt)
    x_est, P_est = ukf.update(x_pred, P_pred, sigmas_pred, measurements[k])
    estimates.append(x_est.copy())

estimates = np.array(estimates)

# --- 可視化 ---
plt.figure(figsize=(8, 8))
plt.plot(true_states[:, 0], true_states[:, 1], 'b-', linewidth=2,
         label='True')
plt.scatter(measurements[:, 0], measurements[:, 1], c='gray', s=10,
            alpha=0.3, label='Measurements')
plt.plot(estimates[:, 0], estimates[:, 1], 'r--', linewidth=1.5,
         label='UKF estimate')
plt.xlabel('x')
plt.ylabel('y')
plt.title('UKF: Circular Motion Tracking')
plt.legend()
plt.axis('equal')
plt.grid(True, alpha=0.3)
plt.tight_layout()
plt.show()