拡張カルマンフィルタ（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}")