Particle Filter を学ぶ：実装編
本記事は 2 部構成になっております．
前回の理論編では，Particle Filter（以下 PF）の基本的な仕組みを整理しました．
今回は実装編として，実際の標高地図を使った自己位置推定に PF を適用します．
題材は地形照合航法です．GPS を使わず，移動量と真下の地面の標高だけから現在位置を推定します．
本記事では特に，
- 位置を特定できない区間で PF がどのように不確かさを保持するか
- 地形に情報が現れたとき，粒子分布がどのように収束するか
- 外れ値を含む観測を尤度にどう組み込むか
- 同じ問題を Kalman Filter（以下 KF）で解いた場合に何が起きるか
を見ていきます．
今回の主な結果は次の通りです．
- 松本盆地を横断する 8.8 km のコースで，PF は平坦部では候補を広く保持し，丘陵に入ると数 step で正解位置へ収束した
- 高度計に 20% の外れ値を混ぜた条件でも，混合尤度を使うことで 5/5 試行で推定に成功した
- 同条件の EKF 1 本では 0/5，最終誤差中央値は 163 km だった
- KF を 64 本並べた場合も，外れ値を考慮した尤度を導入することで PF に近い精度まで改善した
- このコースでは 20,000 粒子から 500 粒子まで減らしても 5/5 で成功した
※ 本記事の標高データには国土地理院の地理院タイルを利用しています．
0. 背景
PF を調べ始めたきっかけは，Kaggle の ROGII - Wellbore Geology Prediction でした．
このコンペでは，水平坑井に沿って取得したセンサログと参照井のログを対応付けながら，地質学的な位置を推定します．公開 notebook の中では多くの参加者が PF を使用しており，逐次的な位置合わせとして自然な構成に見えました．
理論編では，
- 状態を予測する
- 観測との整合性から粒子に重みを付ける
- 重みが偏ったらリサンプリングする
という bootstrap Particle Filter の基本形を扱いました．
ただ，数式だけでは，
「複数の候補を粒子として持つ」とは，実際にはどのような挙動になるのか
が分かりにくいところがあります．
そこで今回は，構造が比較的分かりやすい 地形照合による自己位置推定 を実装します．
1. 問題設定：地形照合航法
今回考えるのは，GPS を使わずに，地形の標高パターンから現在位置を推定する問題です．
飛行体は次の情報を持っているものとします．
- 標高地図
- 慣性航法から得られる各 step の移動量
- 真下の地面の標高
現在の絶対位置そのものは分かりません．
1.1 観測は「真下の地面の標高」
たとえば，
気圧高度計 海面からの高さ 1,358 m
電波高度計 地面までの距離 500 m
─────────────────────────────
差分 真下の地面の標高 858 m
であれば，観測として得られるのは
真下の地面は標高 858 m
という値です．
重要なのは，位置そのものは観測していないことです．
ステップごとに，その位置での標高だけが観測されます．
1.2 1 回の観測では位置を特定できない
例えば，現在の観測が 858 m だった場合に位置を推定しようとしても，標高 858 m の地点は地図上に多数存在します．
したがって，
観測値が 858 m だったから，現在位置はここ
とは決められません．
地図上には，同じ観測を説明できる候補が多数残ります．
前回の記事の用語で言えば，事後分布は単純な 1 個の山ではなく，空間上の複数の領域に分かれた分布になります．
PF では，この候補を粒子として直接保持できます．
1.3 時系列として見る
一方，移動しながら観測を繰り返すと事情が変わります．
上図では，候補が A・B・C の 3 つに絞られている状況を考えています．
このまま 1.8 km 直進すると，3 つの候補が予測する標高系列は下図のように大きく異なります．
- A：盆地の中なのでほぼ平坦なまま
- C：実測と比較して，全体的に 400 m ほど高いところを推移
- B：実測とほぼ一致する標高を推移
単に「標高 858 m の場所」は多数存在しても，同じ移動量で同じ標高を生成する場所はそれほど多くありません．
そのため，観測を逐次取り込むことで候補を絞ることができます．
2. 実験条件
2.1 標高地図
国土地理院の標高タイルから，北アルプス〜松本盆地周辺の約 47.4 × 23.7 km を取得しました．
解像度は約 31 m / pixel です．
左側が北アルプス，右側が松本盆地です．
PF や EKF から任意座標の標高を参照できるよう，格子状の標高値を双一次補間しています．
2.2 飛行コース
メインの実験では，松本盆地を東へ横断し，最後に丘陵へ入る約 8.8 km のコースを使用します．
トータル 350 step，1 step あたり約 25 m です．
このコースでは，前半と後半で地形の性質が大きく異なります．
前半の盆地では，周辺約 3 km の標高の標準偏差が約 9 m しかありません．
そのため，標高観測だけでは位置をほとんど区別できません．
一方，step 320 付近から丘陵へ入り，標高変化が大きくなります．
つまり，
長い間ほとんど位置情報が得られず，終盤になって初めて強い観測が入る
という条件です．
2.3 初期位置と移動誤差
初期位置は既知とはせず，
2.4 km × 2.4 km の範囲内にいる
という情報だけを与えます．
PF ではこの範囲に粒子を一様に配置します．
各 step の移動量は既知入力として与えますが，慣性航法の誤差を模擬するため，粒子には 1 step あたり標準偏差 4 m の移動ノイズを加えています．
2.4 観測ノイズ
通常の観測には標準偏差 5 m のノイズを加えます．
さらに外れ値ありの条件では，20% の確率で標高と無関係な値を返すようにしています．
後半では，この外れ値を PF と KF がどのように扱うかも比較します．
3. Particle Filter による位置推定
まず PF の挙動を確認します．
橙色の点群が粒子です．
橙色の ✕ が粒子の重み付き平均，破線の円が粒子分布から計算した位置のばらつきを示します．
3.1 盆地では分布を保持する
step 300 付近まで，粒子は広い範囲に分布したまま移動しています．
これはフィルタが収束できていないのではなく，そもそも観測から位置を識別できないためです．
平坦部では多数の粒子がほぼ同じ標高を予測します．
したがって尤度にも大きな差が付かず，候補はそのまま残ります．
PF はこのような状況で，無理に 1 個の位置仮説へ縮約せず，複数の候補を保持したまま推移します．
3.2 丘陵に入ると収束する
step 320 付近で丘陵へ入ると，標高系列に特徴が現れます．
それまで似た尤度を持っていた候補のうち，観測系列と一致しないものの重みが急速に低下し，正解付近の粒子が残ります．
乱数を変えた 5 試行では，収束 step は 315〜339 でした．
特徴的な観測データが得られ始めてから数 step で位置が決まっています．
今回確認したかった PF の挙動は，ほぼこの部分です．
情報がない間は候補を残し，情報が入った時点で候補を絞る．
理論上の逐次ベイズ推定が，地図上でもそのまま確認できます．
4. Particle Filter の実装
PF の各 step では，理論編と同じく
- predict
- update
- 必要に応じて resample
を行います．
外側のループは次の形です．
for t in range(T):
pf.predict(u[t])
pf.update(y[t])
処理の本体は predict() と update() の内部にあります．
4.1 初期化
初期位置が分かっているのは 2.4 km 四方の範囲だけなので，その領域に粒子を一様に配置します．
pf.init_box(centre, half=1200.0)
各粒子は，現在位置に関する 1 つの仮説です．
初期状態では全粒子の重みを等しくします．
4.2 predict：移動モデルによる予測
慣性航法から各 step の移動量 u[t] が得られるので，各粒子をその分だけ進めます．
ただし，移動量には誤差があるため，粒子ごとに独立したノイズを加えます．
概念的には，
self.particles += displacement
self.particles += motion_noise
という処理です．
その結果，観測がなければ粒子分布は時間とともに徐々に広がります．
これは，慣性航法の誤差が積算されて位置の不確かさが増加していくためです．
4.3 update：標高観測による重み更新
次に，各粒子位置で予測される標高を地図から取得します．
expected = height_map(self.particles)
resid = y - expected # 残差。近いほど 0 に近い（小さい）
観測された標高と，その粒子位置から予想される標高が近いほど高い尤度を与えます．
通常の観測だけを考える場合は，残差に対して Gaussian likelihood を使います．
like = norm_pdf(resid, sigma) # 尤度．resid が小さいほど大きい
self.weights = normalize(self.weights * like)
ここで行っているのは，
「もし本当にこの粒子の位置にいたなら，今の観測が得られる確率はどの程度か」
を粒子ごとに評価することです．
過去から持っていた重みに今回の尤度を掛けるため，1 step の標高が一致するだけではなく，これまでの標高系列全体と整合する粒子が残っていきます．
4.4 resample
重みの更新を繰り返すと，一部の粒子だけが大きな重みを持ち，多くの粒子の寄与がほぼゼロになることがあります．
そこで Effective Sample Size（ESS）が閾値を下回った場合のみリサンプリングします．
重みに比例して粒子を選び直すことで，尤度の高い領域に粒子を再配置します．
今回の盆地区間では，ここが少し特徴的でした．
平坦な地形では粒子間の尤度差が小さいため，長い区間にわたってリサンプリングが発生しません．
したがって，初期位置の広い候補を比較的長く保持することができます．
5. 外れ値を含む観測
次に，高度計が 20% の確率で無関係な値を返す条件を考えます．
ここでは PF 本体ではなく，尤度のモデル化が問題になります．
5.1 Gaussian likelihood だけでは破綻する
正解位置の地図標高が 858 m なのに，外れ値として 3000 m が観測されたとします．
残差は 2142 m です．
観測ノイズを 5 m と仮定しているため，Gaussian likelihood ではこの粒子の尤度は事実上 0 になります．
すると，それまで正解候補だった粒子の重みも 1 回の外れ値で消えます．
一度消滅した粒子は，通常の bootstrap PF では後から復活できません．
5.2 観測そのものが壊れている可能性を入れる
そこで観測を，
- 80%：通常の Gaussian noise
- 20%：地形とは無関係な外れ値
の混合分布として扱います．
コード上の変更は次の部分です．
gauss = norm_pdf(resid, sigma)
like = 0.8 * gauss + 0.2 * uniform
式で書けば，
$$
p(y_t \mid x_t) = 0.8\mathcal{N}(y_t ; h(x_t),\sigma^2) + 0.2 \dot U(y_t)
$$
です．
第 2 項が尤度の床になります．
外れ値が来た場合，Gaussian 部分はほぼゼロになりますが，一様分布の項が残ります．
そのため，
今の観測は，位置が間違っているのではなく，観測自体が壊れていた可能性がある
という仮説を残せます．
重要なのは，外れ値を事前に検出して除外しているわけではないことです．
正常観測と外れ値の両方を観測モデルの中に含めています．
6. Kalman Filter との比較
同じ条件で Extended Kalman Filter（EKF）とも比較しました．
初期位置の不確かさは，2.4 km 四方の範囲を 1 個の Gaussian として近似して与えています．
観測モデルには地形標高を使い，その局所勾配をヤコビアンとして EKF を更新します．
乱数 5 通り，最終位置誤差 40 m 未満を成功とすると，結果は次の通りです．
| 条件 | EKF 1 本 | PF 20,000 粒子 |
|---|---|---|
| センサは正直 | 1/5（中央値 683 m） | 5/5（13.1 m） |
| 20% 外れ値 | 0/5（中央値 163 km） | 5/5（11.2 m） |
6.1 初期分布を Gaussian 1 個で表す
今回の初期状態は，
2.4 km 四方のどこにいてもよい
という分布です．
PF では範囲全体に粒子を置けばそのまま表現できます．
一方 EKF では，これを平均と共分散を持つ単一 Gaussian で近似します．
実際の候補は空間全体に広がっているにもかかわらず，EKF では常に 1 個の中心と局所共分散に要約されます．
地形照合では多数の局所的な候補が存在するため，この近似がかなり厳しくなります．
センサが正常な条件でも 1/5 に留まったのは，主にこの影響でした．
6.2 外れ値の影響
さらに初期共分散が大きい区間では，EKF は観測を強く利用します．
平坦部では地形勾配が小さいため，高さ方向の残差が水平位置の大きな修正へ変換される場合があります．
そこへ数千 m 単位の外れ値が入ると，推定位置が一度に大きく移動します．
今回の例では最終誤差中央値が 163 km まで増加しました．
ただし，これは「PF は外れ値に強く，KF は弱い」という単純な比較ではありません．
PF にはすでに外れ値を考慮した混合尤度を与えています．
KF 側にも同じ考え方を導入すれば結果は改善します．
そのため，次に KF を複数仮説へ拡張して比較します．
7. Kalman Filter を複数並べる
EKF 1 本の問題が「1 個の Gaussian しか持てない」ことであれば，複数の EKF を並列に持てば改善できるはずです．
そこで初期領域を 8 × 8 に分割し，64 個の EKF を配置しました．
各 EKF を 1 個の位置仮説とみなし，観測との整合性に応じて仮説間の重みを更新します．
結果は次のようになりました．
| 手法 | 成功 | 最終誤差中央値 |
|---|---|---|
| EKF 64 本・Gaussian likelihood | 0/5 | 14 km |
| EKF 64 本・3σ gate | 0/5 | — |
| EKF 64 本・観測分散を拡大 | 0/5 | — |
| EKF 64 本・混合尤度 + gate | 5/5 | 14.6 m |
| PF 20,000 粒子 | 5/5 | 11.2 m |
単に複数並べただけでは，外れ値 1 回で正解に近い仮説の重みがほぼ消えてしまいます．
一方，PF と同じ混合尤度を仮説重みの更新にも使用すると，5/5 で成功しました．
この結果から分かるのは，少なくとも今回の外れ値条件では，PF そのものだけでなく観測尤度の設計が重要だということです．
PF では任意の尤度を粒子ごとに評価できるため，このような観測モデルをそのまま組み込みやすい，という利点があります．
8. 推定した不確かさ
位置誤差だけでなく，フィルタ自身が出している不確かさも確認しました．
PF では粒子分布の広がり，EKF では共分散から 1σ・2σ 範囲を計算します．
真値がその範囲に含まれていた割合は次の通りでした．
| 1σ 以内 | 2σ 以内 | |
|---|---|---|
| PF | 34.5 % | 98.0 % |
| EKF 1 本 | 0.6 % | 0.9 % |
| 2 次元 Gaussian の理論値 | 39 % | 86 % |
PF は少なくとも今回の条件では，実際の誤差と粒子分布の広がりが概ね対応していました．
一方 EKF は誤ったモードへ収束した後も共分散が小さくなり，自信を持って間違っている状態になります．
step 250 の例では，EKF が申告する不確かさが約 ±189 m だったのに対し，実際の位置誤差は約 142 km でした．
この点も，単一 Gaussian 近似が破綻した場合の問題として分かりやすい例でした．
9. 粒子数を減らす
PF では計算量が粒子数にほぼ比例します．
そこで，20,000 粒子から徐々に減らして同じ実験を行いました．
| 粒子数 | 成功 | 最終誤差 | 350 step の実行時間 |
|---|---|---|---|
| 500 | 5/5 | 10.4 m | 33 ms |
| 1,000 | 5/5 | 11.6 m | 45 ms |
| 5,000 | 5/5 | 11.9 m | 126 ms |
| 20,000 | 5/5 | 11.3 m | 432 ms |
このコースでは 500 粒子でも結果はほとんど変わりませんでした．
理由の一つは，前半の盆地区間でリサンプリングがほぼ発生しないことだと考えられます．
地形に情報がないため尤度差も小さく，粒子が特定領域へ偏りません．
その結果，初期候補を長期間維持できます．
ただし，これは地形によって大きく変わります．
北アルプスのように地形変化が強い領域では観測尤度が鋭くなり，毎 step に近い頻度でリサンプリングが発生します．
リサンプリングを繰り返すと，同じ粒子が何度も複製されて粒子の種類が減っていきます．
粒子数は変わらないのに分布を表現する能力だけが落ちる状態で，粒子貧化と呼ばれます．
その条件では 20,000 粒子でも失敗するケースがありました．
したがって，
500 粒子あれば十分
という結果ではなく，
今回の盆地→丘陵コースでは 500 粒子でも十分だった
と解釈するのが適切です．
10. 考察
今回の実験で最も分かりやすかったのは，PF が位置を特定できない状態をそのまま保持できることでした．
盆地では観測に位置情報がほとんどありません．
この区間で無理に推定値を 1 点へ絞る必要はありません．
PF では粒子が広い範囲に残り，
現時点では複数の候補が区別できない
という状態自体を分布として保持できます．
その後，丘陵へ入って観測に識別力が生まれると，過去の移動系列と標高系列に整合する粒子だけが残り，一気に収束しました．
もう一つ重要だったのは，外れ値への対応です．
PF だから自動的に外れ値へ強いわけではありません．
Gaussian likelihood だけを使えば，PF でも外れ値 1 回で正解粒子を失います．
今回うまく動いたのは，
観測には一定確率で外れ値が含まれる
という仮定を尤度に明示的に入れたためです．
さらに，同じ混合尤度を KF bank 側にも導入すると，KF 側も大幅に改善しました．
したがって今回の比較からは，
- 多数の位置仮説を直接保持できること
- 非 Gaussian な尤度をそのまま粒子重みへ使えること
- 必要に応じて粒子を有力領域へ再配置できること
が PF の使いやすさとして確認できました．
「PF が KF より常に優れている」という結果ではありません．
今回のように，初期状態が広く，観測が曖昧で，途中まで複数の候補を保持する必要がある問題では PF の表現が自然だった，という結果です．
11. 別の地形でも確認する
同じ比較を別の地形でも行いました．
センサは正直な場合
| 地形 | KF 1 本 | KF bank 64 | PF 2 万粒子 |
|---|---|---|---|
| 北アルプス | 1/5 | 5/5 | 5/5 |
| 前山 | 0/5 | 4/5 | 5/5 |
| 東の丘陵 | 0/5 | 5/5 | 5/5 |
| 盆地→丘陵 | 1/5 | 5/5 | 5/5 |
観測の 20% が外れ値の場合
| 地形 | KF 1 本 | KF bank 64 * | PF 2 万粒子 |
|---|---|---|---|
| 北アルプス | 0/5 | 3/5 | 4/5 |
| 前山 | 0/5 | 3/5 | 5/5 |
| 東の丘陵 | 0/5 | 5/5 | 5/5 |
| 盆地→丘陵 | 0/5 | 5/5 | 5/5 |
* 混合尤度 + gate を移植した強化版
大まかな傾向は同じでしたが，北アルプスでは PF も 4/5 まで低下しています．
地形変化が大きいと観測尤度が鋭くなるため，9 章で述べた粒子貧化の影響が強く出ます．
また，前山で初期範囲を 6 km 四方に広げた条件では，KF bank が 0/5，PF が 4/5 となるケースもありました．
KF bank は，
- 何個の EKF を置くか
- どこに置くか
- 各 EKF の初期共分散をどの程度にするか
といった設計に依存します．
PF にも粒子数や分布などの設計要素はありますが，少なくとも今回の問題では，初期領域へ一様に粒子を配置するだけで比較的安定して動作しました．
12. まとめ
今回は，地形照合航法を題材に Particle Filter を実装しました．
結果として，
- 平坦部では複数の位置候補を保持する
- 地形に特徴が現れると候補が急速に絞られる
- 外れ値を観測モデルへ組み込めば，正解粒子を保持したまま推定を継続できる
- 今回のコースでは 500 粒子でも十分な精度が得られる
という挙動を確認できました．
理論編では predict → update → resample という処理だけを見ると抽象的でしたが，実際に地図上で粒子を動かすと，それぞれの処理の役割がかなり明確になります．
特に，
情報がない間は候補を残し，情報が入ってから絞る
という PF の性質は，今回の盆地→丘陵の例で分かりやすく確認できました．
ROGII の公開解法で PF を見たことが出発点でしたが，少なくとも今回の実験では，なぜこの種の位置合わせ問題で PF が選択肢になるのかを実感できる結果になりました．
Appendix
TBD