カルマンフィルタの理論とPython実装

import numpy as np
import matplotlib.pyplot as plt

# ---- パラメータの設定 ----
dt = 1.0  # タイムステップ [s]

# 状態遷移行列（等速運動モデル）
A = np.array([[1, dt],
              [0,  1]])

# 観測行列（位置のみ観測可能）
H = np.array([[1, 0]])

# プロセスノイズ共分散（加速度ノイズから導出）
q = 0.1  # 加速度ノイズの分散
Q = q * np.array([[dt**3 / 3, dt**2 / 2],
                  [dt**2 / 2, dt]])

# 観測ノイズ共分散
R = np.array([[1.0]])

class KalmanFilter:
    """線形カルマンフィルタ"""

    def __init__(self, A, H, Q, R, x0, P0):
        self.A = A
        self.H = H
        self.Q = Q
        self.R = R
        self.x = x0.copy()
        self.P = P0.copy()

    def predict(self, u=None, B=None):
        """予測ステップ（式3, 4）"""
        self.x = self.A @ self.x
        if u is not None and B is not None:
            self.x += B @ u
        self.P = self.A @ self.P @ self.A.T + self.Q
        return self.x.copy(), self.P.copy()

    def update(self, z):
        """更新ステップ（式5-9）"""
        # イノベーション
        y = z - self.H @ self.x
        S = self.H @ self.P @ self.H.T + self.R

        # カルマンゲイン
        K = self.P @ self.H.T @ np.linalg.inv(S)

        # 状態と共分散の更新
        self.x = self.x + K @ y
        I = np.eye(len(self.x))
        self.P = (I - K @ self.H) @ self.P
        return self.x.copy(), self.P.copy()

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

# 真の初期状態
x_true = np.array([0.0, 1.0])  # 位置0, 速度1

# カルマンフィルタの初期化
x0 = np.array([0.0, 0.0])     # 初期推定（速度を知らないと仮定）
P0 = np.diag([1.0, 1.0])      # 初期共分散
kf = KalmanFilter(A, H, Q, R, x0, P0)

# 記録用配列
true_positions = [x_true[0]]
true_velocities = [x_true[1]]
measurements = []
est_positions = [x0[0]]
est_velocities = [x0[1]]
P_history = [P0.copy()]

for k in range(T):
    # 真の状態の更新
    x_true = A @ x_true + np.random.multivariate_normal([0, 0], Q)
    true_positions.append(x_true[0])
    true_velocities.append(x_true[1])

    # 観測の生成
    z = H @ x_true + np.random.multivariate_normal([0], R)
    measurements.append(z[0])

    # カルマンフィルタ
    kf.predict()
    x_est, P_est = kf.update(z)
    est_positions.append(x_est[0])
    est_velocities.append(x_est[1])
    P_history.append(P_est.copy())

true_positions = np.array(true_positions)
true_velocities = np.array(true_velocities)
measurements = np.array(measurements)
est_positions = np.array(est_positions)
est_velocities = np.array(est_velocities)
P_history = np.array(P_history)

# ---- 結果のプロット ----
time = np.arange(T + 1)
sigma_pos = np.sqrt(P_history[:, 0, 0])

fig, axes = plt.subplots(2, 1, figsize=(12, 8), sharex=True)

# 位置の推定
axes[0].plot(time, true_positions, "b-", linewidth=1.5, label="True position")
axes[0].scatter(time[1:], measurements, c="gray", s=15, alpha=0.5,
                label="Measurements", zorder=3)
axes[0].plot(time, est_positions, "r--", linewidth=1.2, label="KF estimate")
axes[0].fill_between(time,
                      est_positions - 2 * sigma_pos,
                      est_positions + 2 * sigma_pos,
                      color="red", alpha=0.15, label="$\pm 2\sigma$")
axes[0].set_ylabel("Position")
axes[0].set_title("Kalman Filter - 1D Constant Velocity Tracking")
axes[0].legend(loc="upper left", fontsize=9)
axes[0].grid(True, alpha=0.3)

# 速度の推定
sigma_vel = np.sqrt(P_history[:, 1, 1])
axes[1].plot(time, true_velocities, "b-", linewidth=1.5, label="True velocity")
axes[1].plot(time, est_velocities, "r--", linewidth=1.2, label="KF estimate")
axes[1].fill_between(time,
                      est_velocities - 2 * sigma_vel,
                      est_velocities + 2 * sigma_vel,
                      color="red", alpha=0.15, label="$\pm 2\sigma$")
axes[1].set_xlabel("Time step $k$")
axes[1].set_ylabel("Velocity")
axes[1].legend(loc="upper left", fontsize=9)
axes[1].grid(True, alpha=0.3)

plt.tight_layout()
plt.savefig("kalman_filter_result.png", dpi=150)
plt.show()

# RMSE計算
rmse_pos = np.sqrt(np.mean((true_positions[1:] - est_positions[1:]) ** 2))
rmse_vel = np.sqrt(np.mean((true_velocities[1:] - est_velocities[1:]) ** 2))
print(f"Position RMSE: {rmse_pos:.4f}")
print(f"Velocity RMSE: {rmse_vel:.4f}")