拡張カルマンフィルタ（EKF）の理論とPython実装

import numpy as np
import matplotlib.pyplot as plt

dt = 1.0  # タイムステップ

def f(x):
    """状態遷移関数（等速直線運動モデル）"""
    px, py, vx, vy = x
    return np.array([px + vx * dt, py + vy * dt, vx, vy])

def jacobian_F(x):
    """状態遷移関数のヤコビ行列"""
    return np.array([
        [1, 0, dt, 0],
        [0, 1, 0, dt],
        [0, 0, 1,  0],
        [0, 0, 0,  1]
    ])

def h(x):
    """観測関数（距離と方位角）"""
    px, py = x[0], x[1]
    r = np.sqrt(px**2 + py**2)
    theta = np.arctan2(py, px)
    return np.array([r, theta])

def jacobian_H(x):
    """観測関数のヤコビ行列"""
    px, py = x[0], x[1]
    r = np.sqrt(px**2 + py**2)
    return np.array([
        [px / r,      py / r,  0, 0],
        [-py / r**2,  px / r**2, 0, 0]
    ])

class EKF:
    def __init__(self, x0, P0, Q, R):
        """
        Parameters
        ----------
        x0 : array, 初期状態推定値
        P0 : array, 初期共分散行列
        Q  : array, プロセスノイズ共分散
        R  : array, 観測ノイズ共分散
        """
        self.x = x0.copy()
        self.P = P0.copy()
        self.Q = Q
        self.R = R

    def predict(self):
        """予測ステップ（式5, 6）"""
        F = jacobian_F(self.x)
        self.x = f(self.x)           # 非線形関数fで状態を予測
        self.P = F @ self.P @ F.T + self.Q  # ヤコビFで共分散を伝播
        return self.x.copy()

    def update(self, z):
        """更新ステップ（式7-11）"""
        H = jacobian_H(self.x)
        y = z - h(self.x)            # 非線形関数hで観測残差を計算
        # 方位角の差を [-pi, pi] に正規化
        y[1] = (y[1] + np.pi) % (2 * np.pi) - np.pi

        S = H @ self.P @ H.T + self.R
        K = self.P @ H.T @ np.linalg.inv(S)
        self.x = self.x + K @ y
        self.P = (np.eye(len(self.x)) - K @ H) @ self.P
        return self.x.copy()

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

# ノイズパラメータ
Q = np.diag([0.1, 0.1, 0.01, 0.01])
R = np.diag([0.5, 0.01])  # 距離ノイズ, 方位角ノイズ

# 真の初期状態（円弧状の軌跡を描くように速度を設定）
x_true = np.array([10.0, 0.0, 0.5, 1.0])

# 真の軌跡と観測を生成
true_states = [x_true.copy()]
measurements = []

for k in range(T):
    # 速度方向を少しずつ回転させる（緩やかなカーブ）
    omega_true = 0.03
    vx, vy = x_true[2], x_true[3]
    x_true[2] = vx * np.cos(omega_true) - vy * np.sin(omega_true)
    x_true[3] = vx * np.sin(omega_true) + vy * np.cos(omega_true)

    x_true = f(x_true) + np.random.multivariate_normal(np.zeros(4), Q)
    true_states.append(x_true.copy())

    z = h(x_true) + np.random.multivariate_normal(np.zeros(2), R)
    measurements.append(z.copy())

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

# EKFの実行
x0 = np.array([10.5, -0.5, 0.0, 0.0])
P0 = np.diag([2.0, 2.0, 1.0, 1.0])
ekf = EKF(x0, P0, Q, R)

estimates = [x0.copy()]
for k in range(T):
    ekf.predict()
    ekf.update(measurements[k])
    estimates.append(ekf.x.copy())

estimates = np.array(estimates)

# 観測値をデカルト座標に変換（可視化用）
obs_x = measurements[:, 0] * np.cos(measurements[:, 1])
obs_y = measurements[:, 0] * np.sin(measurements[:, 1])

plt.figure(figsize=(10, 8))
plt.plot(true_states[:, 0], true_states[:, 1], "b-",
         linewidth=1.5, label="True trajectory")
plt.scatter(obs_x, obs_y, c="gray", s=10, alpha=0.5,
            label="Measurements (Cartesian)")
plt.plot(estimates[:, 0], estimates[:, 1], "r--",
         linewidth=1.5, label="EKF estimate")
plt.plot(true_states[0, 0], true_states[0, 1], "go",
         markersize=10, label="Start")
plt.xlabel("x")
plt.ylabel("y")
plt.title("Extended Kalman Filter - Range-Bearing Tracking")
plt.legend()
plt.axis("equal")
plt.grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig("ekf_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
import matplotlib.pyplot as plt

def f(x, k):
    """強い非線形性を持つ状態遷移関数（式17）"""
    return 0.5 * x + 25 * x / (1 + x**2) + 8 * np.cos(1.2 * k)

def df_dx(x, k):
    """f のヤコビアン（スカラー）"""
    return 0.5 + 25 * (1 - x**2) / (1 + x**2)**2

def h(x):
    """偶関数の観測関数（式18）"""
    return x**2 / 20.0

def dh_dx(x):
    """h のヤコビアン。x=0 近傍でゼロに近づく"""
    return x / 10.0

np.random.seed(3)
T = 50
Q = 10.0  # プロセスノイズ分散
R = 1.0   # 観測ノイズ分散

# 真の軌跡と観測を生成
x_true = 0.1
true_states = [x_true]
zs = []
for k in range(T):
    x_true = f(x_true, k) + np.random.normal(0, np.sqrt(Q))
    true_states.append(x_true)
    zs.append(h(x_true) + np.random.normal(0, np.sqrt(R)))
true_states = np.array(true_states)
zs = np.array(zs)

# スカラーEKFの実行
x_est = 0.1
P = 5.0
ekf_est = [x_est]
for k in range(T):
    F = df_dx(x_est, k)
    x_pred = f(x_est, k)
    P_pred = F * P * F + Q

    H = dh_dx(x_pred)
    y = zs[k] - h(x_pred)
    S = H * P_pred * H + R
    K = P_pred * H / S
    x_est = x_pred + K * y
    P = (1 - K * H) * P_pred
    ekf_est.append(x_est)
ekf_est = np.array(ekf_est)

rmse = np.sqrt(np.mean((true_states - ekf_est)**2))
max_err = np.abs(true_states - ekf_est).max()
print(f"RMSE: {rmse:.4f}, max abs error: {max_err:.4f}")

import numpy as np

def state_transition(x, dt):
    """状態遷移関数（等速直線運動モデル）"""
    px, py, vx, vy = x
    return np.array([px + vx * dt, py + vy * dt, vx, vy])

def jacobian_F(dt):
    return np.array([
        [1, 0, dt, 0],
        [0, 1, 0, dt],
        [0, 0, 1, 0],
        [0, 0, 0, 1],
    ])

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])

def jacobian_H(x):
    px, py = x[0], x[1]
    r = np.sqrt(px**2 + py**2)
    return np.array([
        [px / r,      py / r,    0, 0],
        [-py / r**2,  px / r**2, 0, 0],
    ])

def wrap(y):
    """方位角の残差を [-pi, pi] に正規化"""
    y = y.copy()
    y[1] = (y[1] + np.pi) % (2 * np.pi) - np.pi
    return y

# --- シミュレーション設定（UKF記事・CKF記事と同一） ---
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)
])

Q = np.diag([0.05, 0.05, 0.01, 0.01])
R = np.diag([0.3, 0.2])

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])

class EKF:
    def __init__(self, x0, P0, Q, R, dt):
        self.x = x0.copy(); self.P = P0.copy()
        self.Q, self.R, self.dt = Q, R, dt

    def predict(self):
        F = jacobian_F(self.dt)
        self.x = state_transition(self.x, self.dt)
        self.P = F @ self.P @ F.T + self.Q

    def update(self, z):
        H = jacobian_H(self.x)
        y = wrap(z - observation(self.x))
        S = H @ self.P @ H.T + self.R
        K = self.P @ H.T @ np.linalg.inv(S)
        self.x = self.x + K @ y
        self.P = (np.eye(4) - K @ H) @ self.P


class IEKF:
    """Iterated EKF: 更新ステップ内でHを反復点etaで逐次再評価する（式19-21）"""
    def __init__(self, x0, P0, Q, R, dt, n_iter=5):
        self.x = x0.copy(); self.P = P0.copy()
        self.Q, self.R, self.dt, self.n_iter = Q, R, dt, n_iter

    def predict(self):
        F = jacobian_F(self.dt)
        self.x_pred = state_transition(self.x, self.dt)
        self.P_pred = F @ self.P @ F.T + self.Q
        self.x, self.P = self.x_pred.copy(), self.P_pred

    def update(self, z):
        eta = self.x_pred.copy()
        for _ in range(self.n_iter):
            H = jacobian_H(eta)
            S = H @ self.P_pred @ H.T + self.R
            K = self.P_pred @ H.T @ np.linalg.inv(S)
            innovation = wrap(z - observation(eta)) - H @ (self.x_pred - eta)
            eta = self.x_pred + K @ innovation  # 式21
        self.x = eta
        H = jacobian_H(self.x)
        S = H @ self.P_pred @ H.T + self.R
        K = self.P_pred @ H.T @ np.linalg.inv(S)
        self.P = (np.eye(4) - K @ H) @ self.P_pred

ekf = EKF(x0.copy(), P0.copy(), Q, R, dt)
ekf_estimates = [x0.copy()]
for k in range(T):
    ekf.predict()
    ekf.update(measurements[k].copy())
    ekf_estimates.append(ekf.x.copy())
ekf_estimates = np.array(ekf_estimates)

iekf = IEKF(x0.copy(), P0.copy(), Q, R, dt, n_iter=5)
iekf_estimates = [x0.copy()]
for k in range(T):
    iekf.predict()
    iekf.update(measurements[k].copy())
    iekf_estimates.append(iekf.x.copy())
iekf_estimates = np.array(iekf_estimates)

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

print(f"EKF Position RMSE: {rmse(ekf_estimates):.4f}")
print(f"IEKF Position RMSE: {rmse(iekf_estimates):.4f}")

import time

class 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 = wrap(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)
        y = wrap(z - z_pred)
        K = Pxz @ np.linalg.inv(Pzz)
        return x_pred + K @ y, P_pred - K @ Pzz @ K.T


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)
        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 = wrap(Y[i] - y_pred)
            dx = X[i] - x_pred
            S += self.w * np.outer(dy, dy)
            C += self.w * np.outer(dx, dy)
        y = wrap(z - y_pred)
        K = C @ np.linalg.inv(S)
        return x_pred + K @ y, P_pred - K @ S @ K.T

def run_ekf():
    ekf = EKF(x0.copy(), P0.copy(), Q, R, dt)
    est = [x0.copy()]
    for k in range(T):
        ekf.predict(); ekf.update(measurements[k].copy())
        est.append(ekf.x.copy())
    return np.array(est)

def run_ukf():
    ukf = UKF(4, 2, state_transition, observation, Q, R)
    x_est, P_est = x0.copy(), P0.copy()
    est = [x_est.copy()]
    for k in range(T):
        x_pred, P_pred, sig = ukf.predict(x_est, P_est, dt)
        x_est, P_est = ukf.update(x_pred, P_pred, sig, measurements[k].copy())
        est.append(x_est.copy())
    return np.array(est)

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

ekf_est, ukf_est, ckf_est = run_ekf(), run_ukf(), run_ckf()
print(f"EKF RMSE: {rmse(ekf_est):.4f}")
print(f"UKF RMSE: {rmse(ukf_est):.4f}")
print(f"CKF RMSE: {rmse(ckf_est):.4f}")

n_repeat = 200
for name, fn in [("EKF", run_ekf), ("UKF", run_ukf), ("CKF", run_ckf)]:
    t0 = time.perf_counter()
    for _ in range(n_repeat):
        fn()
    t1 = time.perf_counter()
    print(f"{name}: {(t1 - t0) / n_repeat * 1000:.4f} ms/run "
          f"({(t1 - t0) / n_repeat / T * 1e6:.2f} us/step)")