拡張カルマンフィルタ(EKF)を速習！数式の導出と可視化で学ぶ

現実世界のロボティクスや制御システムにおいては、さまざまな不確実性が存在し、私たちはその不確実な情報を用いて状態を推定しなければなりません。こうした環境で、拡張カルマンフィルタ（Extended Kalman filter; EKF）は不確実性に立ち向かうための強力なツールです。しかし、EKFをなんとなく使うことは簡単でも、その背後の数学的な原理を理解するのは容易ではありません。

本記事では、具体的な実例を図を用いて紹介し、直感的な理解を重視しながら、確率論などの基礎に基づいてEKFの数式を一つ一つわかりやすく導出していきます。背後にある数学の美しさを感じていただけるでしょう。最後には、実際の問題に対する実装を通じて、実践的な理解を深めます。本記事の目標は、短時間で「EKFを完全に理解する」ことです。

本記事のすべての実装はgithubに公開していますので、ご自由に参考にしてください。
https://github.com/scomup/MathematicalRobotics

左図： EKFとPF予測の比較　　右図： EKFを用いたodometryとgpsフュージョン

ベイズフィルタ

ベイズフィルタは、不確実なシステムの状態（例：ロボットの位置や速度など）を確率分布で表現し、逐次的に推定するアルゴリズムです。まず、過去の状態分布と制御入力を基に次の状態を予測し、新たな観測データを用いてその予測を修正します。この2つのステップをベイズの定理に基づいて繰り返すことで、時々刻々と状態を推定します。

この仕組みを以下の図にまとめました。

ベイズフィルタは、状態推定のための汎用的な枠組みを提供し、さまざまなフィルタリング技術（例：カルマンフィルタ、拡張カルマンフィルタ、パーティクルフィルタなど）の理論的な基盤となっています。

拡張カルマンフィルタ（EKF）の解説に入る前に、まずベイズフィルタの基本的な予測と修正の仕組みを簡単に理解しておきましょう。

予測ステップ

ベイズフィルタの予測ステップでは、「過去の状態」と「その状態で行われた操作や制御入力」を使って、「次の状態」を確率的に推測します。このプロセスは、次の式で表現されます：

$$
\bar{p}(x_t) = \int p(x_t | u, x_{t-1}) p(x_{t-1}) , dx_{t-1}
\tag{1}
$$

この式を分解すると次のようになります：

1. 運動モデル $ p(x_t | u, x_{t-1}) $:
   これは「もし過去の状態が $ x_{t-1} $ だったとしたら、制御入力 $ u $ によって次の状態が $ x_t $ になる確率」を表します。この運動モデルは、システムの物理的な性質や動作原理に基づいて設計されます。

2. 過去の状態分布 $ p(x_{t-1}) $:
   これは「過去の状態が実際に $ x_{t-1} $ である確率分布」を表します。この分布は、前のステップでの更新（フィルタリング）によって得られた結果です。

式 (1) の積分は、全確率の定理 (Law of total probability)の応用と見ることができます。簡単に言うと、次の状態 $ x_t $ の確率分布 $ \bar{p}(x_t) $ を得るためには、すべての可能な過去の状態 $ x_{t-1} $ を考慮しなければなりません。

修正ステップ

ベイズフィルタの修正ステップでは、予測ステップで得られた「予測された状態分布」と「観測されたデータ」を統合し、現在の状態に関するより信頼性の高い確率分布を計算します。この過程は以下の式で表されます：

$$
p(x_t | z_t) = \eta p(z_t | x_t) \bar{p}(x_t)
\tag{2}
$$

この式を分解すると次のようになります：

1. 予測分布 $\bar{p}(x_t)$: 予測ステップで計算された「状態 $ x_t $ の事前分布」です。
2. 観測モデル $ p(z_t | x_t) $: 「状態 $ x_t $ だった場合に観測データ $ z_t $ が得られる確率」を示します。センサの観測情報はこの部分に反映します。
3. 正規化定数 $\eta$: 確率分布全体の合計が1になるように調整するための定数です。

この式の考え方は ベイズの定理(Bayes' theorem)に基づいています。観測データ $z_t$ による新しい情報を用いて、予測分布 $\bar{p}(x_t)$ を更新し、事後分布 $p(x_t | z_t)$ を得ます。このプロセスによって、観測データと事前情報（予測分布）が統合され、現在の状態の不確実性が減少し、より精度の高い推定が可能になります。
修正後の分布 $p(x_t | z_t)$ は、次の時刻 $t+1$ の予測ステップで使用される事前分布 $\bar{p}(x_{t+1})$ としてそのまま引き継がれ、予測ステップと修正ステップを繰り返し実行されます。

各フィルタリング技術の特徴

式 (1) と式 (2) を用いてベイズフィルタを説明することは可能ですが、そのまま実問題に適用することは困難です。なぜなら、状態の分布 $ p(x) $、運動モデル $ p(x_t | x_{t-1}, u) $、観測モデル $ p(z_t | x_t) $ は、いずれも未知の確率分布だからです。これらの確率分布をモデリングしないと、実際の計算を進めることができません。

モデリング方法の違いによって、以下の2つの代表的なフィルタリング手法が存在します。

拡張カルマンフィルタ（EKF）

• 状態の不確実性を正規分布として仮定します。
• 線形または弱い非線形なシステムに適しており、計算が効率的で処理負荷が低いのが特徴です。ただし、正規分布の仮定が成り立たない複雑な非線形システムには対応できません。

パーティクルフィルタ（PF）

• 状態の不確実性を正規分布と仮定せず、多数のパーティクル（サンプル）を用いて表現します。
• 複雑な非線形システムにも対応できる柔軟な手法です。ただし、パーティクルの数に比例して計算量が増えるため、計算負荷が高くなる場合があります。

正規分布：多くの自然現象に適用され、最も一般的な分布とされています。以下は簡単な1Dと2Dの正規分布の可視化です。

拡張カルマンフィルタ（EKF）

ここからは、拡張カルマンフィルタ（EKF）の詳細に踏み込んでいきます。多くのシステムでは、正規分布を用いて不確実性を十分に表現でき、計算負荷も非常に低い、ロボティクスや制御システムにおいて非常に重要な技術です。

このセクションでは、EKFを徹底的に理解するため、可視化を交えながら、数式の導出を通じてしっかりと理解していきましょう。

予測ステップ：

まず、時刻 $t-1$ の状態の確率分布を正規分布として仮定します。

$$
p(x _{t-1}) = \mathcal{N}(\bar{x} _{t-1}, P _{t-1})
\tag{3}
$$

この正規分布の平均と共分散行列は、それぞれ $ \bar{x} _{t-1} $ ​ と $ P _{t-1} $ を表します。ここで、$x$の上に付されたバー ($\bar{\quad}$) は、「平均値」または「予測値」を意味します。

例えば、ロボットの自己位置推定という問題において、2Dロボットの自己位置（状態の一例）の分布を図で表すと以下のようになります。白い枠で強調された青い大きな矢印は推定された状態の平均を示し、楕円は共分散行列を表しています。色が濃い部分ほど、実際の自己位置がその周辺に存在する確率が高いことを意味します。

また、図中の青い小さな矢印は、この分布に基づいてサンプリングされた可能な自己位置を表しています。このように、EKFは「平均」と「共分散行列」という2つの量を用いて確率分布を表現します。そのため、多数のパーティクル（サンプリングされた小さい矢印）を利用するパーティクルフィルタと比べて、データ量を大幅に削減しながら状態の不確実性を記述できます。

共分散行列 $P_{t-1}$ は、時刻 $t-1$ の状態の不確実性を表しており、以下のように定義されます。

$$
P_{t-1} = \mathrm{E}((\mathbf{x}_{t-1} - \bar{x} _{t-1})(\mathbf{x} _{t-1} - \bar{x} _{t-1})^T) = \mathrm{E}(\Delta{\mathbf{x} _{t-1}} \Delta{\mathbf{x} _{t-1}}^T)
\tag{4}
$$

この式の詳細については、共分散行列の数学定義に参考してください。

ここで、$\Delta{x} _{t-1}$ は $x _{t-1} - \bar{x} _{t-1}$ を表し、可能な自己位置とその平均値の差を示します。

ロボットの運動は、運動方程式 $f$ によって記述されます。この方程式は、前の時刻の状態 $x_{t-1}$ と制御量 $u$ に基づいて次の状態を計算します：

$$
x_t = f(x_{t-1}, u)
\tag{5}
$$

ただし、制御量 $u$ にも不確実性が含まれるのが一般的です。例えば、ロボットのオドメトリデータは車輪の回転量から算出されますが、測定誤差を伴うことが多いため、$u$ の確率分布も正規分布として仮定します：

$$
p(u) = \mathcal{N}(\bar{u}, Q)
\tag{6}
$$

ここで、共分散行列 $Q$ は制御量 $u$ の不確実性を定量化するもので、以下のように定義されます：

$$
Q = \mathrm{E}((\mathbf{u} - \bar{u})(\mathbf{u} - \bar{u})^T) = \mathrm{E}(\Delta{\mathbf{u}}\Delta{\mathbf{u}}^T)
\tag{7}
$$

誤差を無視した場合、次の状態の期待値 $\bar{x}_t$ は次のように計算されます：

$$
\bar{x}_t = f(\bar{x} _{t-1}, \bar{u})
\tag{8}
$$

先ほどの2Dロボットの例を引き続き可視化してみます。制御量としてロボットの加速度と角速度を仮定し、この制御量を運動方程式 $f$ に適用して、全てのパーティクルを一定距離だけ移動させます。図では、移動前のパーティクルを青い矢印、移動後のパーティクルを緑の矢印で示しています。

移動前の各パーティクルは異なる向きを持ち、さらに制御の誤差も考慮されているため、移動後のパーティクル分布はばらつきが拡大しています。

次に、このばらつき（確率分布）を定量的に評価したいです。しかし、式 (5) に基づくと、$x_{t-1}$ と $u$ はいずれも不確実な量であり、運動方程式 $f$ を適用した結果としての次の状態 $\bar{x} _t$ の確率分布がどのようになるでしょうか？果たして、正規分布として表現することは可能でしょうか？

ここでは仮に、次の状態も正規分布で表されると仮定し、その共分散行列を $\bar{P}_t$ とします。以下では、この $\bar{P}_t$ を計算する手順を導出します。

まず、式 (4) を参考に、$\bar{P}_t$ を以下のように書けます：

$$
\bar{P}_t = \mathrm{E}((\mathbf{x}_t - \bar{x}_t)(\mathbf{x}_t - \bar{x}_t)^T)
\tag{9}
$$

$x_t$ は $x_{t-1}$ と制御量 $u$ を運動方程式 $f$ に通して得られるものなので、式 (5) を式 (9) に代入すると以下のように変形できます：

$$
\bar{P}_t = \mathrm{E}((f(\mathbf{x} _{t-1}, \mathbf{u}) - f(\bar{x} _{t-1}, \bar{u}))(f(\mathbf{x} _{t-1}, \mathbf{u}) - f(\bar{x} _{t-1}, \bar{u}))^T)
\tag{10}
$$

真値との差分を$\Delta x = x_{t-1} - \bar{x}_{t-1}$ および $\Delta u = u - \bar{u}$ とします。 $\Delta x$ と $\Delta u$ が十分小さい場合、$f$ をテイラー展開の一次形式で線形近似できます：

$$
f(x_{t-1}, u) \approx f(\bar{x} _{t-1} + \Delta x, \bar{u} + \Delta u) = f(\bar{x} _{t-1}, \bar{u}) + F_x \Delta x + F_u \Delta u
\tag{11}
$$

ここで、$F_x$と $F_u$ ​ は、それぞれ $f$ を状態 $x$ と制御量 $x$ で偏微分したヤコビ行列です：

$$
F_x = \frac{\partial f}{\partial x} \bigg|_{x = \bar{x} _{t-1}, u = \bar{u}}, \quad F_u =\frac{\partial f}{\partial u}\bigg| _{x = \bar{x} _{t-1}, u = \bar{u}} \tag{12}
$$

この線形化近似を式 (10) に代入すると：

$$
\bar{P} _t = \mathrm{E}((F_x \Delta \mathbf{x} + F_u \Delta \mathbf{u})(F_x \Delta \mathbf{x} + F_u \Delta \mathbf{u})^T)
\tag{13}
$$

展開すると：

$$
\bar{P} _t = F_x \mathrm{E}(\Delta \mathbf{x} \Delta \mathbf{x}^T) F_x^T + 2 F_x \mathrm{E}(\Delta \mathbf{x} \Delta \mathbf{u}^T) F_u^T + F_u \mathrm{E}(\Delta \mathbf{u} \Delta \mathbf{u}^T) F_u^T
$$

ここで、$x$ と $u$ が独立しているため、$\mathrm{E}(\Delta \mathbf{x} \Delta \mathbf{u}^T) = 0$ となります。さらに、式 (4) と式(7)を代入すると：

$$
\bar{P}_t = F_x P _{t-1} F_x^T + F_u Q F_u^T
\tag{14}
$$

以上により、予測ステップで必要な $\bar{x}_t$ と共分散行列 $\bar{P}_t$ の更新式が導出されました。この結果を用いて、先ほどの可視化で予測された状態の確率分布（緑の楕円）を計算できます。事前の状態分布（青い楕円）と比べて、予測された状態の分布が広がっている（楕円が大きくなっている）ことが分かります。楕円が大きいほど誤差が大きいことを意味し、予測によって状態の信頼度が減少したことを示しています。

注意点として、$\Delta x$ と $\Delta u$ が小さい場合は線形化近似（式 (11)）が有効ですが、これが成立しない場合、予測分布は正規分布から大きく外れる可能性があります。そのため、EKFは正規分布の仮定が成り立たない複雑な分布には対応できません。

修正ステップ：

拡張カルマンフィルタ（EKF）の修正ステップでは、観測データを利用して予測された状態$\bar{x}_t$ を修正し、より正確な状態 $\hat{x}_t$ を推定します。ここで、$x$頭上のハット($\hat{\quad}$)は補正後の値を示します。

補正後の状態$\hat{x}_t$ を求めるために、まず予測ステップで得られた $\bar{x}_t$ から、必要な更新量 $\Delta{x}_t$ を定義します。

$$
\hat{x}_t = \bar{x}_t + \Delta{x}_t
\tag{15}
$$

具体的な修正ステップの計算では、以下の2つが必要です：

1. $\hat{x}_t$を計算するための更新量 $\Delta{x}_t$ の計算式
2. 補正後の共分散行列$\hat{P}_t$（$\hat{x}_t$​ の不確実性を示す）の計算式

この節では、これらの2つの式を導出していきましょう。

まず、センサの観測モデルを定義します。センサの観測は、真の状態 $x_t$ に基づき、観測方程式 $h$ より、観測$z$を得られます。この観測には誤差$v$があり、大きさは共分散 $R$ で示します。

$$
p(z|x_t) = \mathcal{N}(h(x_t), R)
\tag{16}
$$

$$
z = h(x_t) + v
\tag{17}
$$

予測された状態$\bar{x}_t$から、対応する観測値も予測できます。この予測の観測値と実際の観測値 $ z $ の差を、観測ズレ $ y $ と定義します。

$$
y \equiv z - h(\bar{x}_t)
\tag{18}
$$

先の例を引き続き可視化します。ロボットは、時折GPSから観測データを取得することができます。この観測データを用いることで、ロボットの位置を推定可能です。図では、GPSの観測結果を赤い点で示しており（GPSでは向きがわからないため点で表現されています）、予測された状態 $\bar{x}_t$ は緑の矢印で表されています。赤い点（観測結果）と緑の矢印（予測値）の間の距離が「予測ズレ」に該当します。また、GPSには観測誤差もあるため、赤い楕円はその誤差範囲を示しています。

予測ズレ$y$ の共分散行列は、$S$として定義し、以下のように導出できます。

$$
\begin{aligned}
S &= E(yy^T) \\
&= E((H\Delta{x}_t + v)(H\Delta{x}_t + v)^T) \\
&= E(H\Delta{x}_t\Delta{x}_t^TH^T) + E(vv^T) \\
&= H\bar{P}_tH^T + R
\end{aligned}
\tag{19}
$$

上記の計算には、$h$ の線形化（$h(x_t) \approx h(\bar{x}_t) + H \Delta{x}_t$）を用いています。ここで、$H$ は観測モデルのヤコビ行列です。

理想的な $\Delta{x}_t$ は、観測ズレ $y$ と予測状態の誤差$\Delta{x}_t$を最小化することで求められます。そのため、式 (20) のように、$\Delta{x}_t$ に関する全体の誤差を最小化する目的関数を定式化することができます。

$$
\begin{aligned}
\text{argmin} \quad F(\bar{x}+\Delta{x}_t) &= \lVert y \rVert ^2_S + \lVert \Delta{x}_t \rVert^2 _{\bar{P}_t} \\
&= \lVert z - h(\bar{x}_t) \rVert^2_S + \lVert x - \bar{x}_t \rVert^2 _{\bar{P}_t} \\
&= ( z - h(\bar{x}_t))S^{-1}( z - h(\bar{x}_t))^T + ( x - \bar{x}_t )\bar{P}_t( x - \bar{x}_t )^T
\end{aligned}
\tag{20}
$$

目標関数を $\Delta{x}_t$ について微分し、その結果をゼロに設定することで、最適な $\Delta{x}_t$ を求めます。

$$
\begin{aligned}
0 &= 2 H^TS^{-1}(z - h(\bar{x}_t)) + 2 \bar{P}_t \Delta{x}_t \\
\Delta{x}_t &= \bar{P}_t H^TS^{-1}(z - h(\bar{x}_t)) \\
\Delta{x}_t &= \bar{P}_t H^TS^{-1}y
\end{aligned}
\tag{21}
$$

$PH^TS^{-1}$の部分をカルマンゲイン $ K $として定義します。

$$
K \equiv \bar{P}_t H^TS^{-1}
\tag{22}
$$

式(15)に式(21)(22)を代入することで、修正後の状態 $\hat{x}_t$ を次のように計算します。

$$
\hat{x}_t = \bar{x}_t + Ky
\tag{23}
$$

次に、修正後の共分散行列 $\hat{P}_t$ を評価します。修正後の共分散行列は、真値$x_t$と $\hat{x}_t$ 間の差分の2乗の平均より計算できます。

$$
\hat{P}_t = E((x_t-\hat{x}_t)(x_t-\hat{x}_t)^T)
\tag{24}
$$

この式を展開すると、

$$
\begin{aligned}
\hat{P}_t &= E((x_t-\bar{x}_t-Ky)(x_t-\bar{x}_t-Ky)^T) \\
&= E\left[(x_t - \bar{x}_t)(x_t - \bar{x}_t)^T - (x_t - \bar{x}_t)(Ky)^T - (Ky)(x_t - \bar{x}_t)^T + (Ky)(Ky)^T\right]
\end{aligned}
\tag{25}
$$

• 第1項: $ E[(x_t - \bar{x}_t)(x_t - \bar{x}_t)^T] = \bar{P}_t $

• 第2項と第3項:

$$
\begin{aligned}
&E[(x_t−\bar{x}_t)(h(x_t)+v−h(\bar{x}_t))^TK^T] \\
=&E[(x_t−\bar{x}_t)(h(\bar{x}_t) +H(x_t−\bar{x}_t) +v−h(\bar{x}_t))^TK^T] \\
=&E[(x_t−\bar{x}_t)(H(x_t−\bar{x}_t) +v)^TK^T] \\
=&E[(x_t - \bar{x}_t)(x_t - \bar{x}_t)^T H^TK^T] \\
=& \bar{P}_t H^TK^T \\
=& KH\bar{P}_t
\end{aligned}
\tag{26}
$$

• 第4項:

$$
E((Ky)(Ky)^T) = KE(yy^T)K^T = KSK^T
\tag{27}
$$

したがって、修正後の共分散行列は次のように計算されます。

$$
\begin{aligned}
\hat{P}_t &= \bar{P}_t - 2 \bar{P}_t H^TK^T + KSK^T \\
&= \bar{P}_t - 2 \bar{P}_t H^TK^T + PH^TS^{-1} SK^T \\
&= \bar{P}_t - \bar{P}_t H^TK^T \\
&= (I-K H)\bar{P}_t
\end{aligned}
\tag{28}
$$

これにより、修正ステップに必要な $\hat{x}_t$ と共分散行列 $\hat{P}_t$ の更新式がすべて導出されました。

この結論を用いて、先のGPSの観測例を可視化すると、EKFの修正ステップによって、予測された状態とGPS観測のズレを正規分布として融合し、予測の状態を修正することができます。紫色の楕円は修正後の正規分布を示します。この例の場合、修正の結果は、GPSの観測位置と予測位置の間で、GPSの信頼度が高い（赤い楕円が小さい）ため、よりGPSの観測に近い位置に修正されます。

また、予測状態の分布（緑の楕円）と比べて、修正後の状態（紫色の楕円）の分布は小さくなっています。楕円が小さいほど誤差が少ないことを意味するため、修正によって状態の信頼度が回復したことがわかります。

EKFの計算

これより、EKFの計算がすべて導出されました。まとめると、EKFの計算は以下の6つの式を用いて、予測と修正を繰り返すことで、少ない計算量で状態推定を行うことができます。

修正ステップ

• 式(8)：状態の予測: $ \bar{x} _t = f(\bar{x} _{t-1}, \bar{u})$

• 式(14)：共分散の予測：$\bar{P} _{t} = F_x P _{t-1} F_x^T + F_u Q F_u^T$

修正ステップ

• 式(18)：観測ズレ：$y = z - h(\bar{x}_t)$

• 式(22)：カルマンゲイン：$K = PH^T(H\bar{P}_tH^T + R)^{-1}$

• 式(23)：状態の修正：$\hat{x}_t = \bar{x}_t + Ky$

• 式(28)：共分散の修正：$\hat{P}_t = (I-K H)\bar{P}_t$

実装デモ

次は、先ほど導出したEKFをPythonで実装し、実際に試してみましょう。EKFのフレームワークは、先に示した式に基づいてシンプルに実装できます。

class EKF:
    def __init__(self, initial_mean, initial_cov, motion_model, measurement_model):
        self.x = initial_mean
        self.P = initial_cov
        self.f = motion_model
        self.h = measurement_model

    def predict(self, u, dt):
        Q = self.f.Q
        # 式(8)：状態の予測
        self.x, Fx, Fu = self.f.predict(self.x, u, dt)
        # 式(14)：共分散の予測
        self.P = Fx @ self.P @ Fx.T + Fu @ Q @ Fu.T

    def correct(self, z):
        z_pred, H = self.h.measure(self.x)
        R = self.h.R
        # 式(18)：観測ズレ
        y = z - z_pred
        S = H @ self.P @ H.T + R
        # 式(22)：カルマンゲイン
        K = self.P @ H.T @ np.linalg.pinv(S)
        # 式(23)：状態の修正
        self.x = self.x + K @ y
        I = np.eye(len(self.x))
        # 式(28)：共分散の修正
        self.P = (I - K @ H) @ self.P

次に、このEKFを具体的な問題に応用できるように、状態、制御入力、運動モデルおよび観測モデルを実装します。ここでは、例としてGPSを用いて2Dロボットの自己位置を推定するケースを取り上げます。

状態ベクトルと制御入力

• 状態ベクトル $ x $ は位置$ p $ 、 向き$ \theta $、速度$ v $ から構成しています：$x = [ p, \theta , v ]^T$

• 制御入力 $ u $ は制御の速度成分$ u_v$と制御の角速度から構成しています：$u = [ u_v , u_{\omega} ]^T$

運動モデル

位置、向き、速度の更新は以下のように定式化できます。ヤコビ行列を容易に算出するため、ここでは $SO(2)$ の指数写像（$\exp(\omega \Delta t) $）を使っています。

$$
x_{t} = f(x_{t-1}, u) =
\begin{bmatrix}
p_{t-1} + v_{t-1} \Delta t \\
R_{t-1} \exp(u_{\omega} \Delta t) \\
R_{t-1} \exp(u_{\omega} \Delta t) u_v
\end{bmatrix}
$$

リー群と指数写像についてこのブログに参考してください。

この式を以下のように実装できます。

class Odometry2DModel:
    def __init__(self, process_noise):
        # 2Dオドメトリの運動モデル
        self.Q = process_noise

    def predict(self, state, u, dt):
        # 位置の更新: pos = pos + velocity * dt
        pos = state.pos + state.vel * dt
        # 向きの更新: R = R @ expSO2(omega * dt)
        R = state.R @ expSO2(u[2] * dt)
        # 速度の更新: vel = R @ control velocity
        vel = R @ u[0:2]
        predicted_state = State2D(pos, R, vel)
        # 状態に関するヤコビ行列の計算
        Fx = np.zeros([5, 5])
        Fx[0:2, 0:2] = np.eye(2)
        Fx[0:2, 3:5] = np.eye(2) * dt
        Fx[2, 2] = 1
        Fx[3:5, 2] = state.R @ (hat2d(u[0:2]))
        # 制御に関するヤコビ行列の計算
        Fu = np.zeros([5, 3])
        Fu[2, 2] = -dt
        Fu[3:5, 0:2] = -state.R
        return predicted_state, Fx, Fu

観測モデル

次に、GPSの観測モデルは状態から位置だけを抽出するので、以下のように実装できます。

class GPSModel:
    def __init__(self, measurement_noise):
        # 2D GPS測定の観測モデル
        self.R = measurement_noise

    def measure(self, state):
        H = np.array([[1, 0, 0, 0, 0],
                      [0, 1, 0, 0, 0]])
        return state.pos, H

シンプルでわかりやすい実装ができましたね。この実装を使って、EKFを実際にデモで確認しましょう。

修正ステップの動作

まず、修正ステップのみを実施する場合、制御入力に誤差があるため、ロボットの自己位置がどんどん真値から乖離していくことがわかりますね。EKFはこの誤差を抑えることはできませんが、修正ステップで誤差の大きさを評価し、それを共分散に反映しています。以下のデモのように、共分散を示す青い楕円のサイズがどんどん大きくなることが確認できます。

EKFとPFの予測の比較

誤差を共分散として評価しましたが、この値は本当に正しいのでしょうか？そこで、パーティクルフィルタ（PF）を使って検証してみましょう。ご存じのとおり、PFは有限なパーティクルで離散的に自己位置の分布を評価します。そして、一つ一つのパーティクルを独立に運動モデルに基づき更新します。十分なパーティクル数があれば、真値の分布を容易に推定できるでしょう。以下のデモでは、PFとEKFがそれぞれ独立に自己位置の分散を計算しています。EKFの楕円が非常にきれいにパーティクルたちの分布を表現していることがわかります。この結果から、EKFが予測した誤差が正確であると言えます。また、多くのパーティクルのばらつきを一つの共分散で説明できることは美しいと思いませんか？これはEKFの数学の魅力です。

EKFを用いたオドメトリとGPSフュージョン

次に、予測ステップと修正ステップを両方実施します。
あまり信頼できない精度の低いGPSのため、位置がかなり広範囲にばらついています。共分散（赤い楕円）も大きくなっています。
このような不確実な値を用いても、EKFは有効に活用でき、もともと発散していた自己位置を真値の周囲に修正しました。また、その誤差の大きさを評価する共分散も小さくなりました。

おまけ

完全おまけですが、同じ問題をパーティクルフィルタ（PF）をつかっても実装可能です。しかし、パーティクルごとに、予測と修正が必要ですので、EKFと比べたら大分遅いですね。

コード公開

上記のデモをgithubに公開していますので、以下のコメントで実行できます。
https://github.com/scomup/MathematicalRobotics

python3 -m mathR.filter.demo_ekf # EKFのデモ
python3 -m mathR.filter.demo_pf  # PFのデモ

まとめ

拡張カルマンフィルタ（EKF）は、ロボティクスや制御システムにおける不確実性の課題に立ち向かうための強力なツールです。本記事では、しっかり理解したい方に向けて、可視化を交えながら、すべての式の数学的導出を示しました。

EKFは、修正ステップを繰り返し最適化に置き換えたIterated Extended Kalman Filter（IEKF）や、多様体理論（リー群）を応用したKalman Filters on Manifoldsなど、様々な現代的な拡張があります。最先端の研究においても、EKFは依然として重要な役割を果たしており、基礎となるEKFをしっかり理解することが今でも非常に重要です。この記事を通じて、EKFに対する理解を少しでも深めていただければ幸いです。