自動車が交差点を曲がるとき、その運動は直線運動とはまったく異なります。ハンドルの切れ角に応じて曲率が変わり、速度と方向が同時に変化します。GPSの位置をそのまま使えば大まかな位置は分かりますが、車線レベルの精度が必要な自動運転では、ジャイロセンサや車輪速センサの情報をリアルタイムに統合して、滑らかで正確な位置・姿勢の推定を行わなければなりません。
ここで問題になるのが、車両の運動モデルが非線形であるという点です。前の記事で学んだ標準的なカルマンフィルタは、状態方程式と観測方程式がいずれも線形であることを前提としていました。しかし、車両の旋回運動、人工衛星の軌道力学、ロボットアームの関節運動など、現実の多くのシステムは本質的に非線形です。線形カルマンフィルタをそのまま適用することはできません。
この壁を乗り越えるために考案されたのが拡張カルマンフィルタ（Extended Kalman Filter, EKF）です。EKFのアイデアは驚くほどシンプルです。非線形関数を現在の推定値の周りでテイラー展開し、1次の項（ヤコビアン）で近似することで、各時刻で「局所的に線形化された」カルマンフィルタを適用します。
EKFは1960年代にNASAのアポロ計画で初めて実用化され、月への航行における宇宙船の軌道決定に使われました。以来60年以上にわたり、EKFは非線形状態推定の標準的な手法であり続けています。現代でも以下のような分野で広く使われています。
- 自動運転・ロボティクス: GPS/IMU統合航法、SLAM（自己位置推定と地図作成の同時実行）
- 航空宇宙: 衛星の軌道決定、ロケットの誘導制御、姿勢推定（MEKF）
- 通信: PLLの位相追跡、チャネル推定
- 金融工学: 確率的ボラティリティモデルのパラメータ推定
本記事の内容
- 非線形状態空間モデルの定式化
- テイラー展開による線形化の数学的根拠
- ヤコビアン行列の計算方法
- EKFアルゴリズムの全ステップ
- EKFの近似精度と限界
- Pythonによるレーダー追跡シミュレーション
- 標準カルマンフィルタとの比較検証
前提知識
この記事を読む前に、以下の記事を読んでおくと理解が深まります。
- カルマンフィルタと最適推定の制御応用 — 線形カルマンフィルタの理論と実装
- カルマンフィルタの理論と導出 — カルマンフィルタの基礎
また、以下の概念に馴染みがあることを前提とします。
- 多変数関数の偏微分とヤコビアン行列
- テイラー展開（1次近似）
- 線形カルマンフィルタの予測・更新ステップ
非線形状態空間モデル
線形モデルの限界
線形カルマンフィルタが扱う状態空間モデルは、次の形でした。
$$ \begin{align} \bm{x}_{k+1} &= \bm{A}\bm{x}_k + \bm{B}\bm{u}_k + \bm{w}_k \\ \bm{y}_k &= \bm{C}\bm{x}_k + \bm{v}_k \end{align} $$
ここで $\bm{A}$、$\bm{B}$、$\bm{C}$ は定数行列です。しかし、レーダーで航空機を追跡する場面を考えてみましょう。航空機の状態を直交座標 $\bm{x} = [x, y, v_x, v_y]^T$ で表すとき、レーダーが観測するのは距離 $r$ と方位角 $\theta$ です。
$$ \begin{align} r &= \sqrt{x^2 + y^2} \\ \theta &= \arctan\left(\frac{y}{x}\right) \end{align} $$
この観測方程式は $\bm{x}$ の非線形関数であり、$\bm{y} = \bm{C}\bm{x}$ の形には書けません。同様に、車両の旋回運動の状態方程式も三角関数を含む非線形方程式です。
一般的な非線形モデル
非線形システムの離散時間状態空間モデルは、次のように表されます。
$$ \begin{align} \bm{x}_{k+1} &= f(\bm{x}_k, \bm{u}_k) + \bm{w}_k \\ \bm{y}_k &= h(\bm{x}_k) + \bm{v}_k \end{align} $$
ここで、$f(\cdot)$ は状態遷移関数、$h(\cdot)$ は観測関数で、いずれも非線形関数です。ノイズの仮定は線形カルマンフィルタと同じです。
- $\bm{w}_k \sim \mathcal{N}(\bm{0}, \bm{Q})$: プロセスノイズ（白色ガウスノイズ）
- $\bm{v}_k \sim \mathcal{N}(\bm{0}, \bm{R})$: 観測ノイズ（白色ガウスノイズ）
線形カルマンフィルタでは $f(\bm{x}, \bm{u}) = \bm{A}\bm{x} + \bm{B}\bm{u}$、$h(\bm{x}) = \bm{C}\bm{x}$ という特殊ケースに相当します。
問題は、非線形関数を通した確率分布の変換が解析的に計算できないことにあります。ガウス分布を非線形関数に通すと、一般にはガウス分布ではなくなります。線形カルマンフィルタが美しく閉じた形で更新式を持てたのは、ガウス分布の線形変換がガウス分布のままであるという性質に依存していました。
この問題を近似的に解決するのがEKFのアプローチです。次のセクションで、その数学的基盤であるテイラー展開による線形化を見ていきましょう。
テイラー展開による線形化
1次テイラー展開の復習
多変数関数 $f(\bm{x})$ の点 $\bm{a}$ まわりの1次テイラー展開は、次のように書けます。
$$ f(\bm{x}) \approx f(\bm{a}) + \frac{\partial f}{\partial \bm{x}}\bigg|_{\bm{x}=\bm{a}} (\bm{x} – \bm{a}) $$
ここで $\frac{\partial f}{\partial \bm{x}}$ はヤコビアン行列です。この近似は $\bm{x}$ が $\bm{a}$ に近いほど正確です。
地図のアナロジーで考えてみましょう。地球の表面は球面（非線形）ですが、あなたが立っている場所の周辺だけを見れば、ほぼ平面（線形）に見えます。これがテイラー展開の1次近似です。遠くに行くほど（曲率が効いてくるほど）平面近似のずれは大きくなりますが、局所的には非常に良い近似です。
ヤコビアン行列
EKFで必要なヤコビアン行列は2つあります。
状態遷移のヤコビアン $\bm{F}_k$:
$$ \bm{F}_k = \frac{\partial f}{\partial \bm{x}}\bigg|_{\bm{x}=\hat{\bm{x}}_{k|k}} = \begin{bmatrix} \frac{\partial f_1}{\partial x_1} & \cdots & \frac{\partial f_1}{\partial x_n} \\ \vdots & \ddots & \vdots \\ \frac{\partial f_n}{\partial x_1} & \cdots & \frac{\partial f_n}{\partial x_n} \end{bmatrix}_{\bm{x}=\hat{\bm{x}}_{k|k}} $$
$\bm{F}_k$ は $n \times n$ の行列で、状態遷移関数 $f$ の各成分を状態ベクトルの各成分で偏微分したものです。重要なのは、このヤコビアンが現在の推定値 $\hat{\bm{x}}_{k|k}$ で評価されることです。つまり、毎時刻異なるヤコビアンが得られます。
観測関数のヤコビアン $\bm{H}_k$:
$$ \bm{H}_k = \frac{\partial h}{\partial \bm{x}}\bigg|_{\bm{x}=\hat{\bm{x}}_{k+1|k}} = \begin{bmatrix} \frac{\partial h_1}{\partial x_1} & \cdots & \frac{\partial h_1}{\partial x_n} \\ \vdots & \ddots & \vdots \\ \frac{\partial h_p}{\partial x_1} & \cdots & \frac{\partial h_p}{\partial x_n} \end{bmatrix}_{\bm{x}=\hat{\bm{x}}_{k+1|k}} $$
$\bm{H}_k$ は $p \times n$ の行列で、観測関数 $h$ の各成分を状態ベクトルの各成分で偏微分したものです。こちらは予測値 $\hat{\bm{x}}_{k+1|k}$ で評価されます。
線形化の具体例：レーダー観測
先ほどのレーダー観測の例で、ヤコビアン $\bm{H}$ を計算してみましょう。状態を $\bm{x} = [x, y, v_x, v_y]^T$、観測を $h(\bm{x}) = [r, \theta]^T = [\sqrt{x^2+y^2}, \arctan(y/x)]^T$ とすると、ヤコビアンの各成分は次のようになります。
距離 $r = \sqrt{x^2 + y^2}$ について:
$$ \frac{\partial r}{\partial x} = \frac{x}{\sqrt{x^2+y^2}}, \quad \frac{\partial r}{\partial y} = \frac{y}{\sqrt{x^2+y^2}}, \quad \frac{\partial r}{\partial v_x} = 0, \quad \frac{\partial r}{\partial v_y} = 0 $$
方位角 $\theta = \arctan(y/x)$ について:
$$ \frac{\partial \theta}{\partial x} = \frac{-y}{x^2+y^2}, \quad \frac{\partial \theta}{\partial y} = \frac{x}{x^2+y^2}, \quad \frac{\partial \theta}{\partial v_x} = 0, \quad \frac{\partial \theta}{\partial v_y} = 0 $$
まとめると、ヤコビアンは次のようになります。
$$ \bm{H} = \begin{bmatrix} \frac{x}{r} & \frac{y}{r} & 0 & 0 \\ \frac{-y}{r^2} & \frac{x}{r^2} & 0 & 0 \end{bmatrix} $$
ここで $r = \sqrt{x^2+y^2}$ です。このヤコビアンは状態 $\bm{x}$ の値に依存する（つまり時変）ことに注意してください。線形カルマンフィルタの $\bm{C}$ 行列は定数でしたが、EKFでは毎時刻ヤコビアンを再計算する必要があります。
テイラー展開による線形化の数学的基盤を理解したところで、次にEKFのアルゴリズム全体を整理しましょう。
EKFアルゴリズム
アルゴリズムの全体像
EKFのアルゴリズムは、線形カルマンフィルタと同じ「予測→更新」の2ステップ構造を持ちます。違いは、線形の行列演算の代わりに、非線形関数による伝播とヤコビアンによる共分散の伝播を組み合わせる点です。
初期化:
$$ \hat{\bm{x}}_{0|0} = \bm{x}_0, \quad \bm{P}_{0|0} = \bm{P}_0 $$
予測ステップ
状態の予測 — 非線形関数 $f$ をそのまま使います:
$$ \hat{\bm{x}}_{k+1|k} = f(\hat{\bm{x}}_{k|k}, \bm{u}_k) $$
ヤコビアンの計算 — 現在の推定値でヤコビアンを評価します:
$$ \bm{F}_k = \frac{\partial f}{\partial \bm{x}}\bigg|_{\bm{x}=\hat{\bm{x}}_{k|k}} $$
誤差共分散の予測 — ヤコビアンを使って線形近似的に伝播させます:
$$ \bm{P}_{k+1|k} = \bm{F}_k \bm{P}_{k|k} \bm{F}_k^T + \bm{Q} $$
ここで注目すべきは、状態の予測では非線形関数 $f$ をそのまま使いますが、共分散の伝播にはヤコビアン $\bm{F}_k$ を使うという非対称性です。状態の予測値（平均値）は非線形関数を正確に通すことができますが、共分散（分布の広がり）の伝播は非線形関数を正確に扱えないため、線形近似を用います。
更新ステップ
観測のヤコビアンの計算:
$$ \bm{H}_{k+1} = \frac{\partial h}{\partial \bm{x}}\bigg|_{\bm{x}=\hat{\bm{x}}_{k+1|k}} $$
イノベーション（残差）の計算 — 非線形関数 $h$ をそのまま使います:
$$ \bm{e}_{k+1} = \bm{y}_{k+1} – h(\hat{\bm{x}}_{k+1|k}) $$
イノベーション共分散:
$$ \bm{S}_{k+1} = \bm{H}_{k+1}\bm{P}_{k+1|k}\bm{H}_{k+1}^T + \bm{R} $$
カルマンゲイン:
$$ \bm{K}_{k+1} = \bm{P}_{k+1|k}\bm{H}_{k+1}^T \bm{S}_{k+1}^{-1} $$
状態の更新:
$$ \hat{\bm{x}}_{k+1|k+1} = \hat{\bm{x}}_{k+1|k} + \bm{K}_{k+1}\bm{e}_{k+1} $$
誤差共分散の更新:
$$ \bm{P}_{k+1|k+1} = (\bm{I} – \bm{K}_{k+1}\bm{H}_{k+1})\bm{P}_{k+1|k} $$
線形カルマンフィルタとの対比
EKFと線形KFの違いを表にまとめます。
| 項目 | 線形KF | EKF |
|---|---|---|
| 状態遷移 | $\bm{A}\hat{\bm{x}} + \bm{B}\bm{u}$ | $f(\hat{\bm{x}}, \bm{u})$ |
| 観測予測 | $\bm{C}\hat{\bm{x}}$ | $h(\hat{\bm{x}})$ |
| 共分散伝播 | $\bm{A}\bm{P}\bm{A}^T + \bm{Q}$ | $\bm{F}\bm{P}\bm{F}^T + \bm{Q}$ |
| 行列 $\bm{A}$, $\bm{C}$ | 定数 | ヤコビアン $\bm{F}_k$, $\bm{H}_k$（時変） |
| 最適性 | 線形ガウスで最適 | 近似的（1次近似精度） |
重要な点は、EKFは最適ではないということです。線形カルマンフィルタは線形ガウスモデルにおいて推定誤差の分散を最小化する最適推定器でしたが、EKFは線形化による近似を含んでいるため、一般には最適性の保証がありません。それでも多くの実用的な問題で十分な推定精度を提供し、計算コストが比較的小さいことから、最も広く使われる非線形フィルタとなっています。
EKFの近似精度と限界
EKFの近似精度は、テイラー展開の打ち切り誤差に依存します。1次テイラー展開の打ち切り誤差は $O(\|\bm{x} – \hat{\bm{x}}\|^2)$ です。つまり、推定誤差が小さいほどEKFの近似は正確であり、推定誤差が大きいと近似が破綻します。
EKFが問題を起こしやすい状況は以下の通りです。
- 初期推定値が真値から大きく離れている場合: ヤコビアンを評価する点が真値から遠いため、線形近似の精度が悪い
- 非線形性が強い場合: 関数の曲率が大きいほど1次近似のずれが大きい
- サンプリング間隔が大きい場合: 1ステップでの状態変化が大きくなり、線形化点と実際の状態の乖離が増す
これらの限界を克服するために、2次近似を用いる手法や、線形化を使わずに分布を直接近似する無香カルマンフィルタ（UKF）が開発されました。UKFについては次の記事で詳しく解説します。
理論を理解したところで、次にPythonでEKFを実装し、具体的な追跡問題で動作を確認しましょう。
Pythonによるレーダー追跡シミュレーション
問題設定
ここでは、2次元平面を等速旋回する航空機をレーダーで追跡する問題を考えます。航空機の状態ベクトルは $\bm{x} = [x, y, v_x, v_y]^T$（位置と速度）です。レーダーは原点に設置され、航空機までの距離 $r$ と方位角 $\theta$ を観測します。
状態遷移モデル（等速直線運動 + プロセスノイズ）:
$$ f(\bm{x}_k) = \begin{bmatrix} x_k + v_{x,k} \Delta t \\ y_k + v_{y,k} \Delta t \\ v_{x,k} \\ v_{y,k} \end{bmatrix} $$
この問題では状態遷移自体は線形ですが、観測モデルが非線形であるため、標準カルマンフィルタは直接適用できません。
観測モデル（距離と方位角）:
$$ h(\bm{x}_k) = \begin{bmatrix} \sqrt{x_k^2 + y_k^2} \\ \arctan(y_k / x_k) \end{bmatrix} $$
EKFの実装
import numpy as np
import matplotlib.pyplot as plt
np.random.seed(42)
# シミュレーションパラメータ
dt = 1.0 # サンプリング間隔 [s]
n_steps = 100 # ステップ数
omega = 0.03 # 旋回角速度 [rad/s]
# 真の軌跡の生成（等速旋回運動）
x_true = np.zeros((4, n_steps))
x_true[:, 0] = [1000, 500, 10, 5] # 初期位置と速度
for k in range(1, n_steps):
cos_w = np.cos(omega * dt)
sin_w = np.sin(omega * dt)
# 旋回運動モデル（正確な非線形ダイナミクス）
x_true[0, k] = x_true[0, k-1] + x_true[2, k-1] * np.sin(omega*dt)/omega \
- x_true[3, k-1] * (1 - np.cos(omega*dt))/omega
x_true[1, k] = x_true[1, k-1] + x_true[2, k-1] * (1 - np.cos(omega*dt))/omega \
+ x_true[3, k-1] * np.sin(omega*dt)/omega
x_true[2, k] = x_true[2, k-1] * cos_w - x_true[3, k-1] * sin_w
x_true[3, k] = x_true[2, k-1] * sin_w + x_true[3, k-1] * cos_w
# プロセスノイズを加える
x_true[:, k] += np.random.multivariate_normal(
np.zeros(4), np.diag([0.5, 0.5, 0.1, 0.1]))
# 観測の生成
sigma_r = 50.0 # 距離の観測ノイズ [m]
sigma_theta = 0.02 # 方位角の観測ノイズ [rad]（約1.1度）
R = np.diag([sigma_r**2, sigma_theta**2])
y_obs = np.zeros((2, n_steps))
for k in range(n_steps):
r = np.sqrt(x_true[0, k]**2 + x_true[1, k]**2)
theta = np.arctan2(x_true[1, k], x_true[0, k])
y_obs[0, k] = r + np.random.normal(0, sigma_r)
y_obs[1, k] = theta + np.random.normal(0, sigma_theta)
# EKFの実装
def ekf_predict(x_est, P, Q, dt):
"""EKF予測ステップ（等速直線運動モデル）"""
# 状態遷移（非線形関数をそのまま使用）
x_pred = np.array([
x_est[0] + x_est[2] * dt,
x_est[1] + x_est[3] * dt,
x_est[2],
x_est[3]
])
# ヤコビアン F
F = np.array([
[1, 0, dt, 0],
[0, 1, 0, dt],
[0, 0, 1, 0],
[0, 0, 0, 1]
])
# 共分散の予測
P_pred = F @ P @ F.T + Q
return x_pred, P_pred
def ekf_update(x_pred, P_pred, y, R):
"""EKF更新ステップ（距離・方位角観測）"""
# 観測予測（非線形関数をそのまま使用）
r_pred = np.sqrt(x_pred[0]**2 + x_pred[1]**2)
theta_pred = np.arctan2(x_pred[1], x_pred[0])
y_pred = np.array([r_pred, theta_pred])
# 観測ヤコビアン H
r = r_pred
H = np.array([
[x_pred[0]/r, x_pred[1]/r, 0, 0],
[-x_pred[1]/r**2, x_pred[0]/r**2, 0, 0]
])
# イノベーション
e = y - y_pred
# 角度の差を[-pi, pi]に正規化
e[1] = (e[1] + np.pi) % (2 * np.pi) - np.pi
# カルマンゲイン
S = H @ P_pred @ H.T + R
K = P_pred @ H.T @ np.linalg.inv(S)
# 状態と共分散の更新
x_upd = x_pred + K @ e
P_upd = (np.eye(4) - K @ H) @ P_pred
return x_upd, P_upd
# EKF実行
Q = np.diag([1.0, 1.0, 0.5, 0.5]) # プロセスノイズ共分散
x_ekf = np.zeros((4, n_steps))
P_ekf = np.zeros((4, 4, n_steps))
# 初期化（真の初期値から少しずれた値）
x_ekf[:, 0] = [950, 550, 8, 3]
P_ekf[:, :, 0] = np.diag([200, 200, 10, 10])
for k in range(1, n_steps):
# 予測
x_pred, P_pred = ekf_predict(x_ekf[:, k-1], P_ekf[:, :, k-1], Q, dt)
# 更新
x_ekf[:, k], P_ekf[:, :, k] = ekf_update(x_pred, P_pred, y_obs[:, k], R)
# 観測値を直交座標に変換（比較用）
y_cart = np.zeros((2, n_steps))
y_cart[0] = y_obs[0] * np.cos(y_obs[1])
y_cart[1] = y_obs[0] * np.sin(y_obs[1])
# 描画
fig, axes = plt.subplots(2, 2, figsize=(14, 11))
# 2D軌跡
ax = axes[0, 0]
ax.plot(x_true[0], x_true[1], 'b-', linewidth=2, label='True trajectory')
ax.scatter(y_cart[0], y_cart[1], s=8, c='gray', alpha=0.4, label='Radar obs. (converted)')
ax.plot(x_ekf[0], x_ekf[1], 'r--', linewidth=2, label='EKF estimate')
ax.plot(0, 0, 'k^', markersize=12, label='Radar')
ax.set_xlabel('x [m]', fontsize=11)
ax.set_ylabel('y [m]', fontsize=11)
ax.set_title('2D Tracking', fontsize=13)
ax.legend(fontsize=9)
ax.grid(True, alpha=0.3)
ax.set_aspect('equal')
# 位置推定誤差
ax = axes[0, 1]
pos_err = np.sqrt((x_true[0] - x_ekf[0])**2 + (x_true[1] - x_ekf[1])**2)
sigma_pos = np.sqrt(P_ekf[0, 0, :] + P_ekf[1, 1, :])
t = np.arange(n_steps) * dt
ax.plot(t, pos_err, 'r-', linewidth=1.5, label='Position error')
ax.plot(t, 2*sigma_pos, 'b--', linewidth=1, label='$2\\sigma$ bound')
ax.set_xlabel('Time [s]', fontsize=11)
ax.set_ylabel('Position Error [m]', fontsize=11)
ax.set_title('Position Estimation Error', fontsize=13)
ax.legend(fontsize=10)
ax.grid(True, alpha=0.3)
# 速度推定
ax = axes[1, 0]
ax.plot(t, x_true[2], 'b-', linewidth=2, label='True $v_x$')
ax.plot(t, x_true[3], 'g-', linewidth=2, label='True $v_y$')
ax.plot(t, x_ekf[2], 'r--', linewidth=2, label='EKF $v_x$')
ax.plot(t, x_ekf[3], 'm--', linewidth=2, label='EKF $v_y$')
ax.set_xlabel('Time [s]', fontsize=11)
ax.set_ylabel('Velocity [m/s]', fontsize=11)
ax.set_title('Velocity Estimation', fontsize=13)
ax.legend(fontsize=9)
ax.grid(True, alpha=0.3)
# 共分散のトレース
ax = axes[1, 1]
p_trace = np.array([np.trace(P_ekf[:, :, k]) for k in range(n_steps)])
ax.plot(t, p_trace, 'k-', linewidth=2)
ax.set_xlabel('Time [s]', fontsize=11)
ax.set_ylabel('tr(P)', fontsize=11)
ax.set_title('Covariance Trace (Total Uncertainty)', fontsize=13)
ax.grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig('ekf_radar_tracking.png', dpi=150, bbox_inches='tight')
plt.show()
# 数値結果の表示
print("=== EKF Radar Tracking Results ===")
print(f"Mean position error: {np.mean(pos_err[10:]):.1f} m")
print(f"Max position error: {np.max(pos_err[10:]):.1f} m")
print(f"Final P trace: {p_trace[-1]:.1f}")
この結果から、EKFのレーダー追跡性能を多面的に確認できます。
-
2D軌跡（左上）: 灰色の点はレーダーの距離・方位角観測を直交座標に変換したもので、特に距離方向に大きな散らばりがあります。EKFの推定（赤い破線）は、このノイズだらけの極座標観測から真の軌跡（青い実線）を滑らかに復元しています。旋回による進行方向の変化にも追従できているのが分かります。
-
位置推定誤差（右上）: 初期の数ステップで誤差が急速に収束し、その後は安定した推定を維持しています。青い破線（$2\sigma$ 境界）は推定共分散から計算した95%信頼区間であり、実際の誤差がこの範囲内に収まっていることから、EKFの共分散推定が整合的であることが確認できます。
-
速度推定（左下）: 距離と方位角しか観測していないにもかかわらず、$v_x$ と $v_y$ の両成分を正確に推定できています。旋回運動により速度成分が正弦波状に変化していますが、EKFがこの変化に追従しています。これは、位置の観測から運動学的に速度を推論できることを示しています。
-
共分散のトレース（右下）: 初期の大きな不確実性（$\text{tr}(\bm{P}_0) = 420$）が数ステップで急速に減少し、定常値に収束しています。これは線形カルマンフィルタと同様の収束挙動であり、EKFが安定に動作していることを示しています。
EKFと単純な極座標→直交座標変換の比較
EKFの効果を定量的に評価するため、「観測値を単純に直交座標に変換しただけ」の場合と比較してみましょう。
import numpy as np
import matplotlib.pyplot as plt
# 単純変換 vs EKF の位置誤差比較
pos_err_raw = np.sqrt((x_true[0] - y_cart[0])**2 +
(x_true[1] - y_cart[1])**2)
pos_err_ekf = np.sqrt((x_true[0] - x_ekf[0])**2 +
(x_true[1] - x_ekf[1])**2)
fig, axes = plt.subplots(1, 2, figsize=(14, 5))
# 誤差の時系列比較
ax = axes[0]
ax.plot(t, pos_err_raw, 'gray', linewidth=1, alpha=0.7, label='Raw conversion')
ax.plot(t, pos_err_ekf, 'r-', linewidth=2, label='EKF')
ax.set_xlabel('Time [s]', fontsize=11)
ax.set_ylabel('Position Error [m]', fontsize=11)
ax.set_title('Position Error: Raw vs EKF', fontsize=13)
ax.legend(fontsize=10)
ax.grid(True, alpha=0.3)
# RMSE比較
rmse_raw = np.sqrt(np.mean(pos_err_raw[10:]**2))
rmse_ekf = np.sqrt(np.mean(pos_err_ekf[10:]**2))
improvement = (1 - rmse_ekf / rmse_raw) * 100
ax = axes[1]
bars = ax.bar(['Raw Conversion', 'EKF'], [rmse_raw, rmse_ekf],
color=['gray', 'red'], alpha=0.8, width=0.5)
ax.set_ylabel('RMSE [m]', fontsize=11)
ax.set_title(f'RMSE Comparison (EKF: {improvement:.0f}% improvement)', fontsize=13)
ax.grid(True, alpha=0.3, axis='y')
for bar, val in zip(bars, [rmse_raw, rmse_ekf]):
ax.text(bar.get_x() + bar.get_width()/2, bar.get_height() + 1,
f'{val:.1f} m', ha='center', fontsize=12, fontweight='bold')
plt.tight_layout()
plt.savefig('ekf_vs_raw.png', dpi=150, bbox_inches='tight')
plt.show()
print(f"\nRMSE (raw conversion): {rmse_raw:.1f} m")
print(f"RMSE (EKF): {rmse_ekf:.1f} m")
print(f"Improvement: {improvement:.1f}%")
この比較から、EKFの推定精度の優位性が明確に分かります。単純に極座標の観測値を直交座標に変換しただけでは、各時刻の観測ノイズがそのまま位置推定に反映され、大きな誤差が残ります。一方EKFは、運動モデルの予測と観測データを最適に統合することで、ノイズを大幅に低減しています。特に速度成分の推定は観測だけからは不可能であり、EKFの「モデルベース推定」の真価が発揮されている部分です。
EKFの線形化誤差の可視化
最後に、EKFの線形化がどの程度の近似誤差を生むかを可視化してみましょう。非線形な観測関数 $h(\bm{x})$ を真の値の周りで線形化したときの近似精度を確認します。
import numpy as np
import matplotlib.pyplot as plt
# 観測関数 h(x,y) = [sqrt(x^2+y^2), atan2(y,x)] の非線形性の可視化
fig, axes = plt.subplots(1, 2, figsize=(14, 6))
# 原点に対する距離関数 r = sqrt(x^2 + y^2) の等高線
x_range = np.linspace(800, 1400, 200)
y_range = np.linspace(300, 900, 200)
X, Y = np.meshgrid(x_range, y_range)
R_map = np.sqrt(X**2 + Y**2)
Theta_map = np.arctan2(Y, X)
ax = axes[0]
cs = ax.contour(X, Y, R_map, levels=15, cmap='viridis')
ax.clabel(cs, fontsize=8, fmt='%.0f')
# 線形化点を表示
x0, y0 = 1100, 600
ax.plot(x0, y0, 'r*', markersize=15, label='Linearization point')
# 線形化による等高線（接平面）
r0 = np.sqrt(x0**2 + y0**2)
# 1次近似: r ≈ r0 + (x-x0)*x0/r0 + (y-y0)*y0/r0
R_linear = r0 + (X - x0) * x0/r0 + (Y - y0) * y0/r0
cs2 = ax.contour(X, Y, R_linear, levels=cs.levels, cmap='Reds',
linestyles='--', alpha=0.6)
ax.set_xlabel('x [m]', fontsize=11)
ax.set_ylabel('y [m]', fontsize=11)
ax.set_title('Range: Nonlinear (solid) vs Linearized (dashed)', fontsize=12)
ax.legend(fontsize=10)
ax.grid(True, alpha=0.2)
ax = axes[1]
cs = ax.contour(X, Y, np.degrees(Theta_map), levels=15, cmap='viridis')
ax.clabel(cs, fontsize=8, fmt='%.1f°')
Theta_linear = np.arctan2(y0, x0) + (-y0/r0**2)*(X - x0) + (x0/r0**2)*(Y - y0)
cs2 = ax.contour(X, Y, np.degrees(Theta_linear), levels=cs.levels,
cmap='Reds', linestyles='--', alpha=0.6)
ax.plot(x0, y0, 'r*', markersize=15, label='Linearization point')
ax.set_xlabel('x [m]', fontsize=11)
ax.set_ylabel('y [m]', fontsize=11)
ax.set_title('Bearing: Nonlinear (solid) vs Linearized (dashed)', fontsize=12)
ax.legend(fontsize=10)
ax.grid(True, alpha=0.2)
plt.tight_layout()
plt.savefig('ekf_linearization.png', dpi=150, bbox_inches='tight')
plt.show()
この可視化から、EKFの線形化近似の性質が直感的に理解できます。実線が非線形関数の真の等高線、破線が線形化点（赤い星）まわりのヤコビアンによる1次近似の等高線です。
距離関数（左）では、線形化点の近傍では実線と破線がほぼ一致しており、線形化の精度が良いことが分かります。線形化点から離れるにつれて徐々にずれが生じますが、通常のフィルタリングでは推定誤差は共分散 $\bm{P}$ の範囲に収まるため、この近傍での近似で十分です。
方位角関数（右）でも同様の傾向が見られますが、等高線が放射状になるため、横方向（距離一定で角度が変わる方向）での非線形性がやや強くなっています。原点に近い領域ではこの非線形性が顕著になり、EKFの精度が低下する原因となります。これが「レーダーに近い目標の追跡は遠い目標より難しい」という直感に対応しています。
EKFの実装上の注意点
EKFを実装する際には、いくつかの実務的な注意点があります。
ヤコビアンの計算方法
ヤコビアンの計算には3つの方法があります。
- 解析的微分: 手計算で偏微分を求める。最も正確で高速だが、複雑な非線形関数では導出が面倒
- 数値微分: 有限差分法で近似する。実装は簡単だが、ステップサイズの選択が精度に影響する
- 自動微分: PyTorchやJAXなどの自動微分フレームワークを利用する。解析微分と同等の精度で、実装の手間も少ない
実用上は、単純な系では解析微分、複雑な系では自動微分が推奨されます。数値微分はプロトタイピングには便利ですが、計算コストが高く精度も限られるため、本番環境では避けるべきです。
角度の正規化
方位角などの角度を扱う場合、$-\pi$ と $\pi$ の境界をまたぐイノベーションの計算に注意が必要です。例えば、予測された方位角が $179°$ で観測が $-179°$ の場合、単純な差は $-358°$ になりますが、実際の角度差は $2°$ です。上のコードでは (e[1] + np.pi) % (2*np.pi) - np.pi という正規化を行っています。
共分散行列の正定値性
理論的には $\bm{P}$ は常に正定値対称行列ですが、数値計算では丸め誤差の蓄積により対称性や正定値性が失われることがあります。対策として、更新ステップでJoseph form（ヨゼフ形式）を使う方法があります。
$$ \bm{P}_{k|k} = (\bm{I} – \bm{K}_k\bm{H}_k)\bm{P}_{k|k-1}(\bm{I} – \bm{K}_k\bm{H}_k)^T + \bm{K}_k\bm{R}\bm{K}_k^T $$
この形式は $(\bm{I} – \bm{K}\bm{H})\bm{P}$ よりも数値的に安定で、$\bm{P}$ の正定値性が保たれやすくなります。
まとめ
本記事では、非線形システムに対する状態推定手法として拡張カルマンフィルタ（EKF）を解説しました。
- 非線形状態空間モデル: $\bm{x}_{k+1} = f(\bm{x}_k, \bm{u}_k) + \bm{w}_k$、$\bm{y}_k = h(\bm{x}_k) + \bm{v}_k$ の形で、状態遷移と観測の両方が非線形関数で記述される
- ヤコビアン線形化: テイラー展開の1次項を用いて、各時刻で非線形関数を局所的に線形化する。ヤコビアンは現在の推定値で評価するため、毎時刻再計算が必要
- EKFアルゴリズム: 線形KFと同じ予測→更新の2ステップ構造。状態の伝播には非線形関数をそのまま使い、共分散の伝播にはヤコビアンを使うという非対称な構造が特徴
- 推定精度: EKFは1次近似精度であり、推定誤差が小さく非線形性が穏やかな場合に良好な性能を示す。非線形性が強い場合や初期推定値が悪い場合は精度が低下する
- 実装上の注意: ヤコビアンの計算方法、角度の正規化、共分散行列の正定値性の維持が重要
EKFの最大の限界は、1次テイラー展開による線形化の精度に依存することです。非線形性が強いシステムでは、この線形化が大きな近似誤差を生み、フィルタが発散することもあります。
次のステップとして、以下の記事も参考にしてください。
- 無香カルマンフィルタ（UKF） — ヤコビアン不要の非線形フィルタ
- カルマンスムーザ（RTS） — 過去の状態の最適推定
- カルマンフィルタと最適推定の制御応用 — 線形カルマンフィルタの理論