薄暗い部屋の中で、壁に描かれた円に向かって10本のダーツを投げるとしましょう。あなたの腕の精度には限界があるので、ダーツは的の中心を中心とした「散らばり」を持って刺さります。もし壁が平面であれば、ダーツの散らばりのパターン（分布）はそのままの形を保ちます。しかし、もし壁が湾曲していたらどうでしょう。ダーツが刺さった位置を壁の表面に沿って測ると、散らばりのパターンは歪んでしまいます。
拡張カルマンフィルタ（EKF）は、この「壁の曲がり具合」を1次近似（接平面）で評価し、散らばりの変換を近似していました。しかし曲率が大きい場合、接平面による近似は実際の曲面からかけ離れてしまいます。
無香カルマンフィルタ（Unscented Kalman Filter, UKF）は、まったく異なるアプローチを取ります。壁の曲がり具合を近似するのではなく、「代表的な位置にダーツを何本か刺してみて、それぞれが壁のどこに当たるかを見る」ことで、変換後の散らばりを直接推定するのです。この代表的なダーツの位置がシグマポイント（sigma points）です。
UKFの開発者であるJulierとUhlmannは、この発想を次のように表現しています。「任意の非線形関数を近似するよりも、確率分布を近似する方がはるかに容易である」。EKFは非線形関数を近似（線形化）していましたが、UKFは分布自体を代表点で近似するという、本質的に異なる哲学に基づいています。
UKFの利点は明確です。
- ヤコビアンの計算が不要: 非線形関数をブラックボックスとして扱える。解析的微分が困難または不可能なシステムにも適用可能
- 2次精度: テイラー展開の2次の項まで正確に捕捉し、EKFの1次精度を上回る
- 実装の簡潔さ: ヤコビアンの導出・検証という最もバグが入りやすい工程が不要
UKFは以下のような分野で、EKFに代わる手法として広く使われています。
- 航法: GPS/INS統合、ビジョンベースのナビゲーション
- ロボティクス: マニピュレータのキャリブレーション、非線形力学モデルによるSLAM
- 航空宇宙: 大気圏再突入カプセルの追跡、軌道推定
- 電力システム: 非線形な電力潮流モデルの状態推定
本記事の内容
- 無香変換（Unscented Transform）の理論
- シグマポイントの選び方と重み
- UKFアルゴリズムの全ステップ
- EKFとの理論的比較（近似精度の違い）
- Pythonによるレーダー追跡のEKF vs UKF比較シミュレーション
前提知識
この記事を読む前に、以下の記事を読んでおくと理解が深まります。
- 拡張カルマンフィルタ（EKF） — EKFの理論とヤコビアン線形化
- カルマンフィルタと最適推定の制御応用 — 線形カルマンフィルタの理論
また、以下の概念に馴染みがあることを前提とします。
- 多変量ガウス分布の平均と共分散
- コレスキー分解（正定値行列の平方根分解）
- 線形カルマンフィルタの予測・更新ステップ
無香変換（Unscented Transform）
核心的なアイデア
無香変換（UT）は、非線形関数を通した確率分布の変換を近似する手法です。基本的な問題設定は次の通りです。
確率変数 $\bm{x} \sim \mathcal{N}(\bar{\bm{x}}, \bm{P}_x)$ が非線形関数 $\bm{y} = g(\bm{x})$ で変換されたとき、$\bm{y}$ の平均 $\bar{\bm{y}}$ と共分散 $\bm{P}_y$ を求めたい。
一般に、$\bm{y}$ の正確な分布を解析的に求めることはできません。EKFでは $g$ をヤコビアンで線形化しましたが、UTでは $\bm{x}$ の分布を少数のシグマポイント（代表点）で表現し、各シグマポイントを $g$ に通してから統計量を再計算します。
具体的な手順は3ステップです。
ステップ1: シグマポイントの生成
$n$ 次元のガウス分布 $\mathcal{N}(\bar{\bm{x}}, \bm{P}_x)$ から、$2n+1$ 個のシグマポイント $\{\mathcal{X}_i\}$ とその重み $\{W_i\}$ を計算します。
ステップ2: 非線形変換
各シグマポイントを非線形関数に通します。
$$ \mathcal{Y}_i = g(\mathcal{X}_i), \quad i = 0, 1, \dots, 2n $$
ステップ3: 統計量の再構成
変換後のシグマポイントから平均と共分散を重み付き平均で計算します。
$$ \bar{\bm{y}} \approx \sum_{i=0}^{2n} W_i^{(m)} \mathcal{Y}_i $$
$$ \bm{P}_y \approx \sum_{i=0}^{2n} W_i^{(c)} (\mathcal{Y}_i – \bar{\bm{y}})(\mathcal{Y}_i – \bar{\bm{y}})^T $$
この手順の美しさは、非線形関数 $g$ をブラックボックスとして扱えることです。$g$ の内部構造を知る必要はなく、入力を与えて出力を得る（関数を評価する）だけで済みます。ヤコビアンの計算は一切不要です。
シグマポイントの選び方
シグマポイントは、もとの分布の平均と共分散を正確に再現するように選ばれます。最も標準的な選び方は以下の通りです。
$$ \begin{align} \mathcal{X}_0 &= \bar{\bm{x}} \\ \mathcal{X}_i &= \bar{\bm{x}} + \left(\sqrt{(n+\lambda)\bm{P}_x}\right)_i, \quad i = 1, \dots, n \\ \mathcal{X}_{i+n} &= \bar{\bm{x}} – \left(\sqrt{(n+\lambda)\bm{P}_x}\right)_i, \quad i = 1, \dots, n \end{align} $$
ここで $\left(\sqrt{(n+\lambda)\bm{P}_x}\right)_i$ は行列の平方根（コレスキー分解で得られる下三角行列 $\bm{L}$ で $\bm{P}_x = \bm{L}\bm{L}^T$）の $i$ 行目ベクトルです。$\lambda = \alpha^2(n + \kappa) – n$ はスケーリングパラメータです。
パラメータの意味は次の通りです。
- $\alpha$: シグマポイントの広がりを制御。$0 < \alpha \leq 1$。小さいほど平均の近くにシグマポイントが集まる。典型的には $\alpha = 10^{-3}$ 〜 $1$
- $\kappa$: 2次パラメータ。$\kappa = 0$ が一般的な選択。$\kappa = 3 – n$ はガウス分布に対して4次モーメントも正確に捕捉する
- $\beta$: 分布の事前知識を組み込むパラメータ。ガウス分布では $\beta = 2$ が最適
重みの計算
シグマポイントの重みは、平均計算用と共分散計算用で異なります。
平均の重み $W^{(m)}$:
$$ \begin{align} W_0^{(m)} &= \frac{\lambda}{n + \lambda} \\ W_i^{(m)} &= \frac{1}{2(n + \lambda)}, \quad i = 1, \dots, 2n \end{align} $$
共分散の重み $W^{(c)}$:
$$ \begin{align} W_0^{(c)} &= \frac{\lambda}{n + \lambda} + (1 – \alpha^2 + \beta) \\ W_i^{(c)} &= \frac{1}{2(n + \lambda)}, \quad i = 1, \dots, 2n \end{align} $$
$W_0^{(c)}$ にだけ $\beta$ の補正項が入っていることに注目してください。これは中心点（平均値）での共分散推定を分布の尖度（4次モーメント）に合わせるための調整です。
無香変換の近似精度
無香変換の最も重要な理論的性質は、ガウス分布の非線形変換に対して、テイラー展開の2次の項まで正確に平均と共分散を再現することです。
これを確認するために、$g(\bm{x})$ の $\bar{\bm{x}}$ まわりのテイラー展開を考えます。
$$ g(\bm{x}) = g(\bar{\bm{x}}) + \bm{G}(\bm{x} – \bar{\bm{x}}) + \frac{1}{2}(\bm{x} – \bar{\bm{x}})^T \bm{H}_g (\bm{x} – \bar{\bm{x}}) + \cdots $$
ここで $\bm{G}$ はヤコビアン、$\bm{H}_g$ はヘッセ行列です。EKFは1次の項 $\bm{G}(\bm{x} – \bar{\bm{x}})$ のみを使うため1次精度ですが、UTはシグマポイントの対称配置により2次の項 $\frac{1}{2}(\bm{x} – \bar{\bm{x}})^T \bm{H}_g (\bm{x} – \bar{\bm{x}})$ まで捕捉します。
直感的には、EKFが接線（1次）で曲面を近似しているのに対し、UTは接線+曲率（2次）で近似していると言えます。そのため、非線形性が強いシステムではUTの方が正確な近似を提供します。
無香変換の理論を理解したところで、これをカルマンフィルタのフレームワークに組み込むUKFアルゴリズムを見ていきましょう。
UKFアルゴリズム
アルゴリズムの全体像
UKFは、カルマンフィルタの予測ステップと更新ステップの両方で無香変換を使って非線形変換を近似します。
初期化:
$$ \hat{\bm{x}}_{0|0} = \bm{x}_0, \quad \bm{P}_{0|0} = \bm{P}_0 $$
予測ステップ
1. シグマポイントの生成:
現在の推定値と共分散からシグマポイントを生成します。
$$ \begin{align} \mathcal{X}_0^{(k)} &= \hat{\bm{x}}_{k|k} \\ \mathcal{X}_i^{(k)} &= \hat{\bm{x}}_{k|k} + \left(\sqrt{(n+\lambda)\bm{P}_{k|k}}\right)_i, \quad i = 1, \dots, n \\ \mathcal{X}_{i+n}^{(k)} &= \hat{\bm{x}}_{k|k} – \left(\sqrt{(n+\lambda)\bm{P}_{k|k}}\right)_i, \quad i = 1, \dots, n \end{align} $$
2. シグマポイントの伝播:
各シグマポイントを非線形状態遷移関数に通します。
$$ \mathcal{X}_i^{(k+1|k)} = f(\mathcal{X}_i^{(k)}, \bm{u}_k), \quad i = 0, 1, \dots, 2n $$
3. 予測の平均と共分散の計算:
$$ \hat{\bm{x}}_{k+1|k} = \sum_{i=0}^{2n} W_i^{(m)} \mathcal{X}_i^{(k+1|k)} $$
$$ \bm{P}_{k+1|k} = \sum_{i=0}^{2n} W_i^{(c)} (\mathcal{X}_i^{(k+1|k)} – \hat{\bm{x}}_{k+1|k})(\mathcal{X}_i^{(k+1|k)} – \hat{\bm{x}}_{k+1|k})^T + \bm{Q} $$
プロセスノイズ $\bm{Q}$ は、伝播後の共分散に加算されます。これは「非線形変換による不確実性の伝播」と「プロセスノイズによる新たな不確実性の追加」を分離して扱う方法です。
更新ステップ
4. 新しいシグマポイントの生成:
予測値と予測共分散から新しいシグマポイントを生成します。
$$ \begin{align} \mathcal{X}_0^{(k+1|k)} &= \hat{\bm{x}}_{k+1|k} \\ \mathcal{X}_i^{(k+1|k)} &= \hat{\bm{x}}_{k+1|k} \pm \left(\sqrt{(n+\lambda)\bm{P}_{k+1|k}}\right)_i \end{align} $$
5. 観測のシグマポイント:
各シグマポイントを非線形観測関数に通します。
$$ \mathcal{Y}_i = h(\mathcal{X}_i^{(k+1|k)}), \quad i = 0, 1, \dots, 2n $$
6. 観測の予測:
$$ \hat{\bm{y}}_{k+1|k} = \sum_{i=0}^{2n} W_i^{(m)} \mathcal{Y}_i $$
7. イノベーション共分散と相互共分散:
$$ \bm{S}_{k+1} = \sum_{i=0}^{2n} W_i^{(c)} (\mathcal{Y}_i – \hat{\bm{y}}_{k+1|k})(\mathcal{Y}_i – \hat{\bm{y}}_{k+1|k})^T + \bm{R} $$
$$ \bm{P}_{xy} = \sum_{i=0}^{2n} W_i^{(c)} (\mathcal{X}_i^{(k+1|k)} – \hat{\bm{x}}_{k+1|k})(\mathcal{Y}_i – \hat{\bm{y}}_{k+1|k})^T $$
ここで $\bm{P}_{xy}$ は状態と観測の相互共分散です。この相互共分散がカルマンゲインの計算に使われます。EKFでは $\bm{P}_{xy} = \bm{P}_{k+1|k}\bm{H}^T$ とヤコビアンで計算していましたが、UKFではシグマポイントのサンプル共分散から直接計算するため、ヤコビアンが不要になるのです。
8. カルマンゲインと状態更新:
$$ \bm{K}_{k+1} = \bm{P}_{xy} \bm{S}_{k+1}^{-1} $$
$$ \hat{\bm{x}}_{k+1|k+1} = \hat{\bm{x}}_{k+1|k} + \bm{K}_{k+1}(\bm{y}_{k+1} – \hat{\bm{y}}_{k+1|k}) $$
$$ \bm{P}_{k+1|k+1} = \bm{P}_{k+1|k} – \bm{K}_{k+1}\bm{S}_{k+1}\bm{K}_{k+1}^T $$
EKFとの対比
UKFとEKFの構造を対比すると、アルゴリズムの全体的な流れ（予測→更新）は同じですが、非線形性の扱い方が本質的に異なることが分かります。
| 項目 | EKF | UKF |
|---|---|---|
| 非線形関数の扱い | ヤコビアンで線形化 | シグマポイントで通す |
| 必要な計算 | 偏微分の導出 | 関数評価のみ |
| 近似精度 | テイラー1次（$O(\Delta x^2)$ 誤差） | テイラー2次（$O(\Delta x^3)$ 誤差） |
| 計算コスト | 状態伝播 1回 + ヤコビアン計算 | 状態伝播 $2n+1$ 回 |
| 実装の容易さ | ヤコビアン導出が必要 | 関数をブラックボックスで使える |
計算コストについて注意すべき点があります。UKFは $2n+1$ 回の関数評価が必要ですが、状態次元 $n$ が小さい場合（例えば $n = 4$ なら9回）、EKFのヤコビアン計算と同程度です。$n$ が大きい場合はUKFの計算コストが増加しますが、数値微分によるヤコビアン計算（各方向に2回の関数評価で $2n$ 回）と比較すると、それほど大きな差ではありません。
理論的な比較を理解したところで、次にPythonで両手法を同じ問題に適用し、推定精度を定量的に比較してみましょう。
PythonによるEKF vs UKF比較
シグマポイントの可視化
まず、無香変換がどのように非線形変換を近似するかを可視化します。
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import Ellipse
np.random.seed(42)
def sigma_points(x_mean, P, alpha=1e-1, kappa=0, beta=2):
"""シグマポイントと重みの生成"""
n = len(x_mean)
lam = alpha**2 * (n + kappa) - n
# コレスキー分解
L = np.linalg.cholesky((n + lam) * P)
sigmas = np.zeros((2*n + 1, n))
sigmas[0] = x_mean
for i in range(n):
sigmas[i + 1] = x_mean + L[i]
sigmas[n + i + 1] = x_mean - L[i]
# 重み
Wm = np.full(2*n + 1, 1 / (2*(n + lam)))
Wc = np.full(2*n + 1, 1 / (2*(n + lam)))
Wm[0] = lam / (n + lam)
Wc[0] = lam / (n + lam) + (1 - alpha**2 + beta)
return sigmas, Wm, Wc
def plot_ellipse(ax, mean, cov, color='blue', label=None, linestyle='-'):
"""共分散楕円の描画（2sigma）"""
eigvals, eigvecs = np.linalg.eigh(cov)
angle = np.degrees(np.arctan2(eigvecs[1, 0], eigvecs[0, 0]))
width, height = 2 * 2 * np.sqrt(eigvals) # 2sigma
ell = Ellipse(xy=mean, width=width, height=height, angle=angle,
fill=False, color=color, linewidth=2, linestyle=linestyle,
label=label)
ax.add_patch(ell)
# 非線形関数: 極座標 → 直交座標
def polar_to_cart(x):
"""[r, theta] -> [x, y]"""
return np.array([x[0] * np.cos(x[1]), x[0] * np.sin(x[1])])
# 入力分布（極座標）
x_mean = np.array([100, np.pi/4]) # r=100, theta=45度
P_in = np.diag([20**2, 0.15**2]) # 距離の不確実性大、角度も適度に
# モンテカルロによる真の変換後分布
n_mc = 10000
samples_in = np.random.multivariate_normal(x_mean, P_in, n_mc)
samples_out = np.array([polar_to_cart(s) for s in samples_in])
y_true_mean = np.mean(samples_out, axis=0)
y_true_cov = np.cov(samples_out.T)
# 無香変換
sigmas, Wm, Wc = sigma_points(x_mean, P_in, alpha=0.5)
sigmas_out = np.array([polar_to_cart(s) for s in sigmas])
y_ut_mean = np.sum(Wm[:, None] * sigmas_out, axis=0)
y_ut_cov = np.zeros((2, 2))
for i in range(len(sigmas)):
diff = sigmas_out[i] - y_ut_mean
y_ut_cov += Wc[i] * np.outer(diff, diff)
# EKF（ヤコビアン線形化）
# ヤコビアン: d/d[r,theta] [r*cos(theta), r*sin(theta)]
r, th = x_mean
J = np.array([
[np.cos(th), -r*np.sin(th)],
[np.sin(th), r*np.cos(th)]
])
y_ekf_mean = polar_to_cart(x_mean)
y_ekf_cov = J @ P_in @ J.T
# 可視化
fig, axes = plt.subplots(1, 2, figsize=(14, 6))
# 入力空間（極座標）
ax = axes[0]
ax.scatter(samples_in[:, 0], np.degrees(samples_in[:, 1]),
s=1, c='gray', alpha=0.1)
ax.scatter(sigmas[:, 0], np.degrees(sigmas[:, 1]),
s=120, c='red', marker='D', zorder=5, edgecolors='black',
label='Sigma points')
ax.scatter(x_mean[0], np.degrees(x_mean[1]),
s=200, c='blue', marker='*', zorder=5, label='Mean')
ax.set_xlabel('Range r [m]', fontsize=11)
ax.set_ylabel('Bearing $\\theta$ [deg]', fontsize=11)
ax.set_title('Input Space (Polar)', fontsize=13)
ax.legend(fontsize=10)
ax.grid(True, alpha=0.3)
# 出力空間（直交座標）
ax = axes[1]
ax.scatter(samples_out[:, 0], samples_out[:, 1], s=1, c='gray', alpha=0.1,
label='MC samples')
plot_ellipse(ax, y_true_mean, y_true_cov, color='black', label='True (MC)')
plot_ellipse(ax, y_ut_mean, y_ut_cov, color='red', label='UT (UKF)')
plot_ellipse(ax, y_ekf_mean, y_ekf_cov, color='blue', label='Jacobian (EKF)',
linestyle='--')
ax.scatter(sigmas_out[:, 0], sigmas_out[:, 1], s=120, c='red', marker='D',
zorder=5, edgecolors='black')
ax.set_xlabel('x [m]', fontsize=11)
ax.set_ylabel('y [m]', fontsize=11)
ax.set_title('Output Space (Cartesian)', fontsize=13)
ax.legend(fontsize=9)
ax.grid(True, alpha=0.3)
ax.set_aspect('equal')
plt.tight_layout()
plt.savefig('ut_vs_linearization.png', dpi=150, bbox_inches='tight')
plt.show()
print("=== Transform Comparison ===")
print(f"True mean (MC): ({y_true_mean[0]:.2f}, {y_true_mean[1]:.2f})")
print(f"UT mean: ({y_ut_mean[0]:.2f}, {y_ut_mean[1]:.2f})")
print(f"EKF mean: ({y_ekf_mean[0]:.2f}, {y_ekf_mean[1]:.2f})")
print(f"\nTrue cov trace: {np.trace(y_true_cov):.1f}")
print(f"UT cov trace: {np.trace(y_ut_cov):.1f}")
print(f"EKF cov trace: {np.trace(y_ekf_cov):.1f}")
この可視化から、無香変換とヤコビアン線形化の近似精度の違いが明確に見えます。
-
入力空間（左）: 極座標でのガウス分布（灰色の点群）と5つのシグマポイント（赤いダイヤ）。シグマポイントは平均（青い星）の周囲に配置され、元の分布の広がりを代表しています。
-
出力空間（右）: 極座標→直交座標の非線形変換後の分布を3つの方法で比較しています。黒い楕円がモンテカルロ法による「真の」変換後分布、赤い楕円が無香変換（UT）の近似、青い破線がヤコビアン線形化（EKF）の近似です。モンテカルロサンプル（灰色の点群）を見ると、変換後の分布は「バナナ型」に歪んでいます。UTの楕円（赤）はこの歪んだ分布をEKFの楕円（青破線）よりも正確に捕捉しています。特に平均値の位置が、UTの方がモンテカルロの真の平均に近いことに注目してください。これがUTの2次精度の効果です。
レーダー追跡のEKF vs UKF
次に、前の記事で実装したレーダー追跡問題にUKFを適用し、EKFと直接比較します。
import numpy as np
import matplotlib.pyplot as plt
np.random.seed(42)
# シミュレーションパラメータ
dt = 1.0
n_steps = 100
omega = 0.03
# 真の軌跡の生成（等速旋回運動）
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
sigma_theta = 0.02
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 ==========
Q = np.diag([1.0, 1.0, 0.5, 0.5])
def ekf_step(x_est, P, y, Q, R, dt):
# 予測
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 = 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
# 更新
r = np.sqrt(x_pred[0]**2 + x_pred[1]**2)
y_pred = np.array([r, np.arctan2(x_pred[1], x_pred[0])])
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
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
# ========== UKF ==========
def ukf_step(x_est, P, y, Q, R, dt, alpha=1e-1, kappa=0, beta=2):
n = len(x_est)
lam = alpha**2 * (n + kappa) - n
# 重み
Wm = np.full(2*n+1, 1/(2*(n+lam)))
Wc = np.full(2*n+1, 1/(2*(n+lam)))
Wm[0] = lam/(n+lam)
Wc[0] = lam/(n+lam) + (1 - alpha**2 + beta)
# 予測シグマポイント
L = np.linalg.cholesky((n+lam) * P)
sigmas = np.zeros((2*n+1, n))
sigmas[0] = x_est
for i in range(n):
sigmas[i+1] = x_est + L[i]
sigmas[n+i+1] = x_est - L[i]
# シグマポイントの伝播
sigmas_pred = np.zeros_like(sigmas)
for i in range(2*n+1):
s = sigmas[i]
sigmas_pred[i] = np.array([s[0]+s[2]*dt, s[1]+s[3]*dt, s[2], s[3]])
# 予測平均と共分散
x_pred = np.sum(Wm[:, None] * sigmas_pred, axis=0)
P_pred = np.zeros((n, n))
for i in range(2*n+1):
d = sigmas_pred[i] - x_pred
P_pred += Wc[i] * np.outer(d, d)
P_pred += Q
# 更新用シグマポイント
L2 = np.linalg.cholesky((n+lam) * P_pred)
sigmas2 = np.zeros((2*n+1, n))
sigmas2[0] = x_pred
for i in range(n):
sigmas2[i+1] = x_pred + L2[i]
sigmas2[n+i+1] = x_pred - L2[i]
# 観測シグマポイント
y_sigmas = np.zeros((2*n+1, 2))
for i in range(2*n+1):
s = sigmas2[i]
r = np.sqrt(s[0]**2 + s[1]**2)
y_sigmas[i] = np.array([r, np.arctan2(s[1], s[0])])
# 観測予測
y_pred = np.sum(Wm[:, None] * y_sigmas, axis=0)
# イノベーション共分散と相互共分散
Syy = np.zeros((2, 2))
Sxy = np.zeros((n, 2))
for i in range(2*n+1):
dy = y_sigmas[i] - y_pred
dy[1] = (dy[1] + np.pi) % (2*np.pi) - np.pi
dx = sigmas2[i] - x_pred
Syy += Wc[i] * np.outer(dy, dy)
Sxy += Wc[i] * np.outer(dx, dy)
Syy += R
# 更新
K = Sxy @ np.linalg.inv(Syy)
e = y - y_pred
e[1] = (e[1] + np.pi) % (2*np.pi) - np.pi
x_upd = x_pred + K @ e
P_upd = P_pred - K @ Syy @ K.T
return x_upd, P_upd
# 両フィルタの実行
x_ekf = np.zeros((4, n_steps))
x_ukf = np.zeros((4, n_steps))
P_ekf_hist = np.zeros((4, 4, n_steps))
P_ukf_hist = np.zeros((4, 4, n_steps))
x0 = np.array([950, 550, 8, 3])
P0 = np.diag([200, 200, 10, 10])
x_ekf[:, 0] = x0.copy()
x_ukf[:, 0] = x0.copy()
P_e = P0.copy()
P_u = P0.copy()
P_ekf_hist[:, :, 0] = P0
P_ukf_hist[:, :, 0] = P0
for k in range(1, n_steps):
x_ekf[:, k], P_e = ekf_step(x_ekf[:, k-1], P_e, y_obs[:, k], Q, R, dt)
x_ukf[:, k], P_u = ukf_step(x_ukf[:, k-1], P_u, y_obs[:, k], Q, R, dt)
P_ekf_hist[:, :, k] = P_e
P_ukf_hist[:, :, k] = P_u
# 描画
t = np.arange(n_steps) * dt
pos_err_ekf = np.sqrt((x_true[0]-x_ekf[0])**2 + (x_true[1]-x_ekf[1])**2)
pos_err_ukf = np.sqrt((x_true[0]-x_ukf[0])**2 + (x_true[1]-x_ukf[1])**2)
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')
ax.plot(x_ekf[0], x_ekf[1], 'r--', linewidth=1.5, label='EKF', alpha=0.8)
ax.plot(x_ukf[0], x_ukf[1], 'g--', linewidth=1.5, label='UKF', alpha=0.8)
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: EKF vs UKF', fontsize=13)
ax.legend(fontsize=10)
ax.grid(True, alpha=0.3)
ax.set_aspect('equal')
# 位置誤差比較
ax = axes[0, 1]
ax.plot(t, pos_err_ekf, 'r-', linewidth=1.5, alpha=0.7, label='EKF')
ax.plot(t, pos_err_ukf, 'g-', linewidth=1.5, alpha=0.7, label='UKF')
ax.set_xlabel('Time [s]', fontsize=11)
ax.set_ylabel('Position Error [m]', fontsize=11)
ax.set_title('Position Error Comparison', fontsize=13)
ax.legend(fontsize=10)
ax.grid(True, alpha=0.3)
# 速度推定比較
ax = axes[1, 0]
vel_err_ekf = np.sqrt((x_true[2]-x_ekf[2])**2 + (x_true[3]-x_ekf[3])**2)
vel_err_ukf = np.sqrt((x_true[2]-x_ukf[2])**2 + (x_true[3]-x_ukf[3])**2)
ax.plot(t, vel_err_ekf, 'r-', linewidth=1.5, alpha=0.7, label='EKF')
ax.plot(t, vel_err_ukf, 'g-', linewidth=1.5, alpha=0.7, label='UKF')
ax.set_xlabel('Time [s]', fontsize=11)
ax.set_ylabel('Velocity Error [m/s]', fontsize=11)
ax.set_title('Velocity Error Comparison', fontsize=13)
ax.legend(fontsize=10)
ax.grid(True, alpha=0.3)
# RMSE比較
ax = axes[1, 1]
skip = 10
rmse_pos_ekf = np.sqrt(np.mean(pos_err_ekf[skip:]**2))
rmse_pos_ukf = np.sqrt(np.mean(pos_err_ukf[skip:]**2))
rmse_vel_ekf = np.sqrt(np.mean(vel_err_ekf[skip:]**2))
rmse_vel_ukf = np.sqrt(np.mean(vel_err_ukf[skip:]**2))
x_pos = np.arange(2)
width = 0.3
bars1 = ax.bar(x_pos - width/2, [rmse_pos_ekf, rmse_vel_ekf],
width, color='red', alpha=0.7, label='EKF')
bars2 = ax.bar(x_pos + width/2, [rmse_pos_ukf, rmse_vel_ukf],
width, color='green', alpha=0.7, label='UKF')
ax.set_xticks(x_pos)
ax.set_xticklabels(['Position RMSE [m]', 'Velocity RMSE [m/s]'], fontsize=10)
ax.set_ylabel('RMSE', fontsize=11)
ax.set_title('RMSE Comparison', fontsize=13)
ax.legend(fontsize=10)
ax.grid(True, alpha=0.3, axis='y')
for bars in [bars1, bars2]:
for bar in bars:
height = bar.get_height()
ax.text(bar.get_x() + bar.get_width()/2, height + 0.1,
f'{height:.2f}', ha='center', fontsize=10, fontweight='bold')
plt.tight_layout()
plt.savefig('ekf_vs_ukf_tracking.png', dpi=150, bbox_inches='tight')
plt.show()
print("\n=== RMSE Comparison ===")
print(f"Position RMSE - EKF: {rmse_pos_ekf:.2f} m, UKF: {rmse_pos_ukf:.2f} m")
print(f"Velocity RMSE - EKF: {rmse_vel_ekf:.2f} m/s, UKF: {rmse_vel_ukf:.2f} m/s")
improvement_pos = (1 - rmse_pos_ukf/rmse_pos_ekf) * 100
improvement_vel = (1 - rmse_vel_ukf/rmse_vel_ekf) * 100
print(f"UKF improvement - Position: {improvement_pos:.1f}%, Velocity: {improvement_vel:.1f}%")
EKFとUKFの比較結果から、以下のことが分かります。
-
2D軌跡（左上）: 真の軌跡（青）に対して、EKF（赤破線）とUKF（緑破線）の両方が良好に追従しています。このスケールではほとんど差が見えませんが、詳細な誤差解析で差が現れます。
-
位置推定誤差（右上）: UKF（緑）の位置誤差がEKF（赤）よりもやや小さい傾向が見られます。特にフィルタの初期収束段階と、旋回による非線形性が強い局面で差が生じています。
-
速度推定誤差（左下）: 速度推定でもUKFがやや優位です。速度は直接観測されないため、観測関数の非線形性（距離・方位角→位置→速度の間接推定）の影響がより顕著に現れます。
-
RMSE比較（右下）: 定量的なRMSE比較では、UKFがEKFに対して位置・速度ともにわずかに優れています。この問題では非線形性が比較的穏やかであるため、差は小さいです。非線形性が強い問題（例えば短距離のレーダー追跡や大きな角度変化）では、UKFの優位性がより顕著になります。
非線形性が強い場合の比較
EKFとUKFの差がより顕著になる例として、レーダーに近い目標の追跡を見てみましょう。距離が近いほど方位角の観測関数の非線形性が強くなります。
import numpy as np
import matplotlib.pyplot as plt
# 100回のモンテカルロシミュレーションで統計的に比較
n_mc = 100
n_steps = 80
dt = 0.5
distances = [100, 300, 500, 1000, 2000] # レーダーからの初期距離
rmse_ekf_dist = []
rmse_ukf_dist = []
for d0 in distances:
pos_err_ekf_all = []
pos_err_ukf_all = []
for mc in range(n_mc):
np.random.seed(mc * 100 + int(d0))
# 旋回運動
x_true = np.zeros((4, n_steps))
x_true[:, 0] = [d0, d0*0.3, 3, 2]
for k in range(1, n_steps):
omega_k = 0.05
cw, sw = np.cos(omega_k*dt), np.sin(omega_k*dt)
x_true[0,k] = x_true[0,k-1] + x_true[2,k-1]*sw/omega_k \
- x_true[3,k-1]*(1-cw)/omega_k
x_true[1,k] = x_true[1,k-1] + x_true[2,k-1]*(1-cw)/omega_k \
+ x_true[3,k-1]*sw/omega_k
x_true[2,k] = x_true[2,k-1]*cw - x_true[3,k-1]*sw
x_true[3,k] = x_true[2,k-1]*sw + x_true[3,k-1]*cw
x_true[:,k] += np.random.multivariate_normal(
np.zeros(4), np.diag([0.3,0.3,0.05,0.05]))
# 観測
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)
th = np.arctan2(x_true[1,k], x_true[0,k])
y_obs[0,k] = r + np.random.normal(0, 30)
y_obs[1,k] = th + np.random.normal(0, 0.03)
# 両フィルタの実行
Q = np.diag([0.5, 0.5, 0.2, 0.2])
R_obs = np.diag([30**2, 0.03**2])
x0 = x_true[:, 0] + np.random.normal(0, [20, 20, 2, 2])
P0 = np.diag([100, 100, 5, 5])
x_e, P_e = x0.copy(), P0.copy()
x_u, P_u = x0.copy(), P0.copy()
pe_e, pe_u = [], []
for k in range(1, n_steps):
try:
x_e, P_e = ekf_step(x_e, P_e, y_obs[:,k], Q, R_obs, dt)
x_u, P_u = ukf_step(x_u, P_u, y_obs[:,k], Q, R_obs, dt)
except np.linalg.LinAlgError:
break
pe_e.append(np.sqrt((x_true[0,k]-x_e[0])**2+(x_true[1,k]-x_e[1])**2))
pe_u.append(np.sqrt((x_true[0,k]-x_u[0])**2+(x_true[1,k]-x_u[1])**2))
if len(pe_e) > 20:
pos_err_ekf_all.append(np.sqrt(np.mean(np.array(pe_e[10:])**2)))
pos_err_ukf_all.append(np.sqrt(np.mean(np.array(pe_u[10:])**2)))
rmse_ekf_dist.append(np.median(pos_err_ekf_all))
rmse_ukf_dist.append(np.median(pos_err_ukf_all))
# 描画
fig, ax = plt.subplots(figsize=(10, 6))
ax.plot(distances, rmse_ekf_dist, 'ro-', linewidth=2, markersize=8, label='EKF')
ax.plot(distances, rmse_ukf_dist, 'gs-', linewidth=2, markersize=8, label='UKF')
ax.set_xlabel('Initial Distance from Radar [m]', fontsize=12)
ax.set_ylabel('Median Position RMSE [m]', fontsize=12)
ax.set_title('EKF vs UKF: Effect of Nonlinearity Strength\n'
'(Closer = More Nonlinear)', fontsize=13)
ax.legend(fontsize=11)
ax.grid(True, alpha=0.3)
ax.set_xscale('log')
plt.tight_layout()
plt.savefig('ekf_ukf_nonlinearity.png', dpi=150, bbox_inches='tight')
plt.show()
print("\n=== Distance vs RMSE ===")
for d, re, ru in zip(distances, rmse_ekf_dist, rmse_ukf_dist):
improvement = (1 - ru/re) * 100
print(f" d={d:4d}m: EKF={re:.2f}m, UKF={ru:.2f}m "
f"(UKF {improvement:+.1f}%)")
このモンテカルロ解析から、非線形性の強さとフィルタ性能の関係が明確に見えます。レーダーからの距離が近いほど、方位角の観測関数 $\theta = \arctan(y/x)$ の非線形性が強くなります（曲率が大きくなります）。距離が遠い場合（2000 m）ではEKFとUKFの差はごくわずかですが、距離が近づく（100 m）につれてUKFの優位性が増大します。これは、非線形性が強い場合にEKFの1次近似が不十分になるのに対し、UKFの2次精度が効果を発揮するためです。
UKFの実装上の注意点
コレスキー分解の安定性
UKFの実装で最もよく遭遇する問題は、コレスキー分解の失敗です。共分散行列 $\bm{P}$ が数値的に正定値でなくなると、コレスキー分解が計算できません。対策として、以下の方法が有効です。
- 共分散行列の正定値性の強制: 更新後に $\bm{P} = (\bm{P} + \bm{P}^T)/2 + \epsilon\bm{I}$ で微小な正則化を加える
- 平方根UKF（SR-UKF）: $\bm{P}$ の代わりにその平方根（コレスキー因子）を直接更新する。数値的に安定で、半分の精度で同じ結果が得られる
パラメータ $\alpha$, $\kappa$, $\beta$ の選択
- $\alpha = 10^{-3}$: 状態次元が小さい場合のデフォルト。シグマポイントが平均の近くに集中する
- $\alpha = 1$: シグマポイントが広く散らばる。低次元の問題に適している
- $\kappa = 3 – n$: ガウス分布のの4次モーメントを正確に捕捉する
- $\beta = 2$: ガウス分布に最適
実用上は $\alpha = 10^{-1}$ 〜 $10^{-3}$、$\kappa = 0$、$\beta = 2$ の設定で多くの問題に対応できます。
計算コストの比較
状態次元 $n$ に対するEKFとUKFの計算コストをまとめます。
| 処理 | EKF | UKF |
|---|---|---|
| 関数評価 | 1回（状態）+ 1回（観測） | $(2n+1)$回（状態）+ $(2n+1)$回（観測） |
| ヤコビアン | $n^2 + np$ 個の偏微分 | 不要 |
| 行列演算 | $O(n^3)$ | $O(n^3)$（コレスキー分解） |
$n = 4$ の場合、UKFの関数評価は9回で、EKFの2回（+ヤコビアン計算）と比較して大きな差はありません。$n$ が数十以上になると関数評価の回数が支配的になりますが、そのような高次元の問題では他の次元削減手法と組み合わせることが一般的です。
まとめ
本記事では、シグマポイントによる非線形フィルタリング手法として無香カルマンフィルタ（UKF）を解説しました。
- 無香変換（UT）: 確率分布を少数のシグマポイント（$2n+1$ 個）で代表し、非線形関数を通してから統計量を再構成する。非線形関数をブラックボックスとして扱える
- シグマポイントの選び方: 共分散のコレスキー分解を用いて、平均の周囲に対称配置する。スケーリングパラメータ $\alpha$, $\kappa$, $\beta$ で精度を調整
- 2次精度: テイラー展開の2次の項まで正確に捕捉し、EKFの1次精度を上回る。非線形性が強い問題で特に有効
- ヤコビアン不要: 解析微分が不要であるため、複雑な非線形関数や既存のシミュレーションコードへの適用が容易
- EKFとの比較: 非線形性が穏やかな場合は両者の差は小さいが、非線形性が強くなるほどUKFの優位性が顕著になる
UKFとEKFはいずれもガウス近似に基づくフィルタであり、事後分布をガウス分布で近似するという本質的な制約を共有しています。事後分布が多峰性を持つような状況（例えば、データの関連付けが曖昧な場合）では、パーティクルフィルタなどのより一般的な手法が必要になります。
次のステップとして、以下の記事も参考にしてください。
- 拡張カルマンフィルタ（EKF） — ヤコビアン線形化による非線形フィルタリング
- カルマンスムーザ（RTS） — 過去の状態の最適推定
- パーティクルフィルタ — 一般的な非線形・非ガウスフィルタリング