都市部のビル群に囲まれた道路を自動運転車が走行しているとします。GPS 信号はビルに反射してマルチパスを起こし、位置の推定誤差はガウス分布にはなりません。「車はこの交差点にいるかもしれないし、1つ先の交差点にいるかもしれない」というマルチモーダル（多峰性）な不確実性が生まれます。あるいは、広大な倉庫の中で自律ロボットが自己位置推定を行う場面を想像してください。ロボットが「自分はこの棚の近くにいそうだ」と思っていたのに、突然まったく違うランドマークを観測したとき、信念分布は複数のピークに分裂します。
こうした非線形・非ガウスな問題に対して、カルマンフィルタや拡張カルマンフィルタ（EKF）、無香カルマンフィルタ（UKF）は本質的な限界を抱えています。これらの手法はすべて、事後分布をガウス分布で近似するため、マルチモーダルな分布を表現できないのです。
では、任意の形状を持つ確率分布をそのまま扱えるフィルタリング手法はないのでしょうか？ その答えがパーティクルフィルタ（Particle Filter） — 逐次モンテカルロ法（Sequential Monte Carlo, SMC）です。
パーティクルフィルタは、確率分布を「粒子（パーティクル）」と呼ばれるサンプル点の集合で近似します。ガウス分布という仮定を一切置かないため、どんな形の分布でも表現できるという最大の利点があります。この手法を理解すると、以下のような応用が可能になります。
- ロボットの自己位置推定（SLAM）: 複雑な環境地図の中で、ロボットがどこにいるかをリアルタイムに推定する
- 軍事・民間のターゲット追跡: 急激な機動を行う航空機やミサイルの軌道を追跡する
- 金融時系列のボラティリティ推定: 急激な市場変動を含む非ガウスな金融モデルに対応する
- コンピュータビジョン: 映像中の人物追跡や物体認識において、オクルージョン（遮蔽）が発生する状況に対応する
本記事の内容
- ベイズフィルタリングの一般的枠組みとカルマンフィルタの限界
- モンテカルロ近似と重点サンプリングの理論
- 逐次重点サンプリング（SIS）の導出と重みの縮退問題
- リサンプリング戦略（多項、系統的、層化）の比較
- SIR アルゴリズム（Bootstrap Particle Filter）
- Python でのスクラッチ実装と EKF/UKF との比較実験
前提知識
この記事を読む前に、以下の記事を読んでおくと理解が深まります。
ベイズフィルタリングの一般的枠組み
パーティクルフィルタの理論に入る前に、すべての逐次状態推定手法の共通基盤であるベイズフィルタリングの枠組みを整理しておきましょう。ベイズフィルタリングとは、時刻ごとに到着する観測データを使って、隠れた状態変数の確率分布を逐次的に更新していく枠組みです。
状態空間モデル
時刻 $k$ における状態ベクトルを $\bm{x}_k$、観測ベクトルを $\bm{z}_k$ とします。状態空間モデルは、以下の2つの式で定義されます。
状態遷移モデル（システムの時間発展を記述）:
$$ \bm{x}_k = f(\bm{x}_{k-1}, \bm{v}_{k-1}) $$
観測モデル（状態から観測がどう生成されるかを記述）:
$$ \bm{z}_k = h(\bm{x}_k, \bm{w}_k) $$
ここで $f(\cdot)$ は状態遷移関数、$h(\cdot)$ は観測関数、$\bm{v}_{k-1}$ はシステムノイズ、$\bm{w}_k$ は観測ノイズです。これらの関数は一般に非線形でよく、ノイズの分布もガウスに限定しません。
再帰的ベイズ推定
ベイズフィルタリングの目標は、時刻 $k$ までの全観測 $\bm{z}_{1:k} = \{\bm{z}_1, \bm{z}_2, \dots, \bm{z}_k\}$ が与えられたときの事後分布 $p(\bm{x}_k | \bm{z}_{1:k})$ を求めることです。この事後分布は、予測と更新の2ステップで再帰的に計算できます。
予測ステップ: 前の時刻の事後分布からチャップマン-コルモゴロフ方程式を使って、現在の時刻の事前分布を計算します。
$$ p(\bm{x}_k | \bm{z}_{1:k-1}) = \int p(\bm{x}_k | \bm{x}_{k-1}) \, p(\bm{x}_{k-1} | \bm{z}_{1:k-1}) \, d\bm{x}_{k-1} $$
この式は「1つ前の時刻のすべての可能な状態 $\bm{x}_{k-1}$ について、遷移確率 $p(\bm{x}_k | \bm{x}_{k-1})$ で重み付けして足し合わせる」ことを意味しています。
更新ステップ: 新しい観測 $\bm{z}_k$ が到着したら、ベイズの定理を使って事後分布を更新します。
$$ p(\bm{x}_k | \bm{z}_{1:k}) = \frac{p(\bm{z}_k | \bm{x}_k) \, p(\bm{x}_k | \bm{z}_{1:k-1})}{p(\bm{z}_k | \bm{z}_{1:k-1})} $$
分母の $p(\bm{z}_k | \bm{z}_{1:k-1})$ は正規化定数で、次のように計算されます。
$$ p(\bm{z}_k | \bm{z}_{1:k-1}) = \int p(\bm{z}_k | \bm{x}_k) \, p(\bm{x}_k | \bm{z}_{1:k-1}) \, d\bm{x}_k $$
この枠組み自体は完全に一般的であり、非線形・非ガウスの場合も成り立ちます。問題は、上の積分が解析的に解けない場合がほとんどだということです。カルマンフィルタは「線形ガウス」という仮定を置くことで解析解を得ますが、それ以外の場合には近似が必要になります。
ここまでで、ベイズフィルタリングの一般的な枠組みを確認しました。次に、カルマンフィルタやその拡張がどのような仮定のもとで成り立っているのか、そしてなぜその仮定が破綻するケースがあるのかを見ていきましょう。
なぜカルマンフィルタでは不十分か — 非線形・非ガウスの世界
カルマンフィルタが見事に機能するのは、以下の条件がすべて満たされるときです。
- 状態遷移モデル $f(\cdot)$ と観測モデル $h(\cdot)$ が線形
- システムノイズ $\bm{v}_k$ と観測ノイズ $\bm{w}_k$ がガウス分布に従う
- 初期状態 $\bm{x}_0$ もガウス分布
この3つの条件が揃うと、事後分布 $p(\bm{x}_k | \bm{z}_{1:k})$ は常にガウス分布となり、平均と共分散行列の2つのパラメータだけで完全に記述できます。これがカルマンフィルタの優雅さの源泉です。
拡張カルマンフィルタ（EKF）の限界
非線形システムに対処するために、EKF は非線形関数を動作点周りで1次テイラー展開（線形化）して近似します。しかし、この近似には以下の問題があります。
- 強い非線形性: 関数の曲率が大きい場合、1次近似の誤差が大きくなる
- ヤコビ行列の計算: 解析的にヤコビ行列を導出する必要があり、複雑なモデルでは困難
- ガウス近似: 線形化後もガウス分布の仮定は維持されるため、非ガウスな分布は表現不可能
無香カルマンフィルタ（UKF）の限界
UKF は線形化を使わず、シグマポイントと呼ばれる代表点の集合を非線形関数に通すことで統計量（平均と共分散）を計算します。EKF より高精度ですが、以下の限界が残ります。
- ガウス近似: UKF も最終的には事後分布をガウス分布で近似する
- マルチモーダル分布: ガウス分布は単峰性なので、複数のピークを持つ分布を表現できない
- シグマポイント数の制限: $2n + 1$ 個のシグマポイント（$n$ は状態次元）では、高次元の複雑な分布を捉えきれない
具体的に破綻するケース
以下のような場面では、EKF/UKF のいずれも本質的に対処できません。
マルチモーダルな事後分布: 冒頭で述べたGPSのマルチパス問題のように、「車はA地点かB地点のどちらかにいる」という状況では、事後分布は2つのピークを持ちます。ガウス分布ではこの2つのピークの中間に平均が来てしまい、実際にはどちらの地点にもいないという誤った推定になります。
非対称・裾の重い分布: 例えばレーダー観測で外れ値が頻繁に生じる場合、観測ノイズはガウス分布よりも裾が重い分布（例: $t$ 分布やコーシー分布）に従います。ガウスの仮定はこのような外れ値に対して脆弱です。
状態空間の制約: ロボットの姿勢角（$0^\circ$ から $360^\circ$ で周期的）や、確率変数が正値しか取らない場合など、状態空間に幾何学的な制約がある場合も、無制約のガウス分布は不適切です。
このように、現実の多くの問題では「ガウス分布で近似する」という戦略そのものに限界があります。では、ガウス分布の仮定を完全に捨てて、任意の形状の確率分布をそのまま扱える手法はないのでしょうか？ その答えが、モンテカルロ近似に基づくパーティクルフィルタです。
モンテカルロ近似の考え方 — 粒子による分布の表現
ガウス分布の仮定を捨てるとき、確率分布を「平均と共分散」で要約する代わりに、どうやって分布を表現すればよいでしょうか？ ここで登場するのがモンテカルロ近似の考え方です。
直感的に言うと、モンテカルロ近似とは「確率分布の形をたくさんのサンプル点（粒子）の散らばり具合で表現する」方法です。例えば、ある山の等高線を知りたいとき、地図を使う代わりに1万人の登山者を山全体にばらまくことを想像してください。標高が高いところに多くの人が集まるように配置すれば、人の密度を見ることで山の形がわかります。
粒子近似の数学的定式化
確率密度関数 $p(\bm{x})$ を、$N$ 個のサンプル点（粒子） $\{\bm{x}^{(i)}\}_{i=1}^{N}$ とそれに付随する重み $\{w^{(i)}\}_{i=1}^{N}$ で近似します。
$$ p(\bm{x}) \approx \sum_{i=1}^{N} w^{(i)} \, \delta(\bm{x} – \bm{x}^{(i)}) $$
ここで $\delta(\cdot)$ はディラックのデルタ関数です。重みは正規化条件 $\sum_{i=1}^{N} w^{(i)} = 1$ を満たします。
この近似が強力なのは、粒子の数 $N$ を十分に大きくすれば、どんな形の分布でも任意の精度で近似できるからです。マルチモーダルな分布であっても、各モード付近に粒子が多く配置されれば、自然に表現できます。
期待値の近似
確率分布 $p(\bm{x})$ に関する任意の関数 $g(\bm{x})$ の期待値は、粒子近似を使って以下のように計算できます。
$$ \mathbb{E}[g(\bm{x})] = \int g(\bm{x}) \, p(\bm{x}) \, d\bm{x} \approx \sum_{i=1}^{N} w^{(i)} \, g(\bm{x}^{(i)}) $$
つまり、困難な積分計算が「粒子上の重み付き和」という単純な足し算に置き換わるのです。大数の法則により、$N \to \infty$ でこの近似は真の期待値に収束します。
粒子による分布の近似方法がわかりました。しかし、ここで重要な問題が生じます。「粒子を $p(\bm{x})$ から直接サンプリングする」ことが容易でない場合、どうすればよいのでしょうか？ 事後分布 $p(\bm{x}_k | \bm{z}_{1:k})$ は一般に複雑な形状であり、直接サンプリングすることは困難です。この問題を解決するのが重点サンプリングです。
重点サンプリング（Importance Sampling）の理論
重点サンプリングは、サンプリングしたいターゲット分布 $p(\bm{x})$ から直接サンプルを生成できないときに使う強力なテクニックです。
日常的なアナロジーで考えてみましょう。ある都市の平均年収を調べたいとします。理想的にはすべての住民を均等にサンプリングしたいのですが、それが難しい場合を考えます。代わりに、電話帳（提案分布 $q(\bm{x})$）を使って人を選びます。ただし、電話帳に載りやすい人と載りにくい人の偏りがあるため、各サンプルに「本来の出現確率 / 電話帳に載る確率」で重みを付けて修正します。これが重点サンプリングの基本的なアイデアです。
数学的定式化
ターゲット分布 $p(\bm{x})$ のもとでの期待値 $\mathbb{E}_p[g(\bm{x})]$ を計算したいとします。サンプリングが容易な提案分布（proposal distribution） $q(\bm{x})$ を導入します。
$p(\bm{x}) > 0$ であるすべての $\bm{x}$ に対して $q(\bm{x}) > 0$（サポート条件）が成り立つとき、期待値を次のように書き換えられます。
$$ \mathbb{E}_p[g(\bm{x})] = \int g(\bm{x}) \, p(\bm{x}) \, d\bm{x} $$
ここで $p(\bm{x})$ に $q(\bm{x}) / q(\bm{x})$（= 1）を掛けます。
$$ = \int g(\bm{x}) \, \frac{p(\bm{x})}{q(\bm{x})} \, q(\bm{x}) \, d\bm{x} $$
これは提案分布 $q(\bm{x})$ のもとでの期待値として解釈できます。
$$ = \mathbb{E}_q\left[ g(\bm{x}) \, \frac{p(\bm{x})}{q(\bm{x})} \right] $$
重点重み
$q(\bm{x})$ から $N$ 個のサンプル $\{\bm{x}^{(i)}\}_{i=1}^{N}$ を生成し、各サンプルに重点重み（importance weight）を付けます。
$$ \tilde{w}^{(i)} = \frac{p(\bm{x}^{(i)})}{q(\bm{x}^{(i)})} $$
このとき、期待値の近似は次のようになります。
$$ \mathbb{E}_p[g(\bm{x})] \approx \frac{1}{N} \sum_{i=1}^{N} \tilde{w}^{(i)} \, g(\bm{x}^{(i)}) $$
実際には $p(\bm{x})$ が非正規化分布（ベイズの定理の分母が不明）である場合が多いため、重みを正規化して使います。
$$ w^{(i)} = \frac{\tilde{w}^{(i)}}{\sum_{j=1}^{N} \tilde{w}^{(j)}} $$
正規化された重みを使った期待値の近似は次のようになります。
$$ \mathbb{E}_p[g(\bm{x})] \approx \sum_{i=1}^{N} w^{(i)} \, g(\bm{x}^{(i)}) $$
提案分布の選び方
重点サンプリングの性能は、提案分布 $q(\bm{x})$ の選び方に大きく依存します。理想的には $q(\bm{x})$ が $p(\bm{x})$ に近い形状であるほど、重みの分散が小さくなり、少ない粒子数で精度の良い近似が得られます。最悪の場合、$q(\bm{x})$ と $p(\bm{x})$ の形状が大きく異なると、ほとんどの粒子の重みがほぼゼロになり、ごく少数の粒子だけが巨大な重みを持つという重みの縮退（weight degeneracy）が生じます。
ここまでで、重点サンプリングの静的な（1時点の）問題に対する定式化を見てきました。次に、これを時系列データに対する逐次推定に拡張する方法を考えましょう。
逐次重点サンプリング（SIS）の導出
時系列の状態推定問題では、新しい観測 $\bm{z}_k$ が到着するたびに事後分布 $p(\bm{x}_{0:k} | \bm{z}_{1:k})$ を更新する必要があります。毎回ゼロから重点サンプリングをやり直すのは計算的に非効率です。そこで、前の時刻の結果を再利用する逐次重点サンプリング（Sequential Importance Sampling, SIS）を考えます。
提案分布の逐次分解
時刻 $0$ から $k$ までの状態の軌跡 $\bm{x}_{0:k} = \{\bm{x}_0, \bm{x}_1, \dots, \bm{x}_k\}$ に対する提案分布を、次のように逐次的に分解します。
$$ q(\bm{x}_{0:k} | \bm{z}_{1:k}) = q(\bm{x}_0 | \bm{z}_1) \prod_{j=1}^{k} q(\bm{x}_j | \bm{x}_{0:j-1}, \bm{z}_{1:j}) $$
この分解の意味は明快です。まず初期分布からサンプリングし、その後の各時刻で「過去の軌跡と最新の観測」に基づいて新しい状態をサンプリングしていくということです。
重みの逐次更新式の導出
ターゲット分布は事後分布 $p(\bm{x}_{0:k} | \bm{z}_{1:k})$ です。ベイズの定理を使って、これを逐次的に展開します。
まず、事後分布を分解します。
$$ p(\bm{x}_{0:k} | \bm{z}_{1:k}) \propto p(\bm{z}_k | \bm{x}_k) \, p(\bm{x}_k | \bm{x}_{k-1}) \, p(\bm{x}_{0:k-1} | \bm{z}_{1:k-1}) $$
ここで、マルコフ性（$\bm{x}_k$ は $\bm{x}_{k-1}$ のみに依存する）と、観測の条件付き独立性（$\bm{z}_k$ は $\bm{x}_k$ のみに依存する）を使っています。
重点重みの定義から、非正規化重みは次のようになります。
$$ \tilde{w}_k^{(i)} = \frac{p(\bm{x}_{0:k}^{(i)} | \bm{z}_{1:k})}{q(\bm{x}_{0:k}^{(i)} | \bm{z}_{1:k})} $$
分子と分母にそれぞれ逐次分解を代入すると、前の時刻の重みとの関係が得られます。分子の事後分布の逐次展開を代入します。
$$ \tilde{w}_k^{(i)} \propto \frac{p(\bm{z}_k | \bm{x}_k^{(i)}) \, p(\bm{x}_k^{(i)} | \bm{x}_{k-1}^{(i)}) \, p(\bm{x}_{0:k-1}^{(i)} | \bm{z}_{1:k-1})}{q(\bm{x}_k^{(i)} | \bm{x}_{0:k-1}^{(i)}, \bm{z}_{1:k}) \, q(\bm{x}_{0:k-1}^{(i)} | \bm{z}_{1:k-1})} $$
ここで、$p(\bm{x}_{0:k-1}^{(i)} | \bm{z}_{1:k-1}) / q(\bm{x}_{0:k-1}^{(i)} | \bm{z}_{1:k-1})$ は前の時刻の非正規化重み $\tilde{w}_{k-1}^{(i)}$ に比例するので、次の逐次更新式が得られます。
$$ \tilde{w}_k^{(i)} \propto \tilde{w}_{k-1}^{(i)} \, \frac{p(\bm{z}_k | \bm{x}_k^{(i)}) \, p(\bm{x}_k^{(i)} | \bm{x}_{k-1}^{(i)})}{q(\bm{x}_k^{(i)} | \bm{x}_{k-1}^{(i)}, \bm{z}_k)} $$
この式が SIS の核心です。新しい粒子 $\bm{x}_k^{(i)}$ を提案分布 $q(\bm{x}_k | \bm{x}_{k-1}^{(i)}, \bm{z}_k)$ からサンプリングし、上の式で重みを更新します。前の時刻の重みに「尤度 $\times$ 遷移確率 / 提案確率」の比を掛けるだけなので、計算が効率的です。
ここで、提案分布として事前分布（状態遷移モデル）を使う、つまり $q(\bm{x}_k | \bm{x}_{k-1}^{(i)}, \bm{z}_k) = p(\bm{x}_k | \bm{x}_{k-1}^{(i)})$ を選ぶと、重みの更新式は劇的に簡単になります。
$$ \tilde{w}_k^{(i)} \propto \tilde{w}_{k-1}^{(i)} \, p(\bm{z}_k | \bm{x}_k^{(i)}) $$
遷移確率と提案確率が打ち消し合い、重みの更新は尤度を掛けるだけになります。この選択が Bootstrap Particle Filter（SIR フィルタ）の基礎となります。
SIS アルゴリズムの定式化が得られました。しかし、SIS をそのまま長時間実行すると、深刻な問題が生じます。それが重みの縮退です。
重みの縮退問題と有効サンプルサイズ
SIS アルゴリズムの最大の弱点は、時間が経つにつれて重みの縮退（weight degeneracy）が起こることです。これは、ほんの数個の粒子だけが支配的な重みを持ち、残りの大多数の粒子の重みがほぼゼロになる現象です。
直感的には、こう考えるとわかりやすいでしょう。100人の投票者がいて、最初は全員が1票ずつ持っています。しかし、毎回の選挙で「正しく予測した人」に票が集まるルールだとすると、何回か選挙を繰り返すうちに、1人が99票、残り99人が合計1票、という状況になります。この状態では、100人いても実質的には1人の意見しか反映されていません。
有効サンプルサイズ（ESS）
重みの縮退の程度を定量的に測る指標として、有効サンプルサイズ（Effective Sample Size, ESS）があります。
$$ N_{\text{eff}} = \frac{1}{\sum_{i=1}^{N} (w^{(i)})^2} $$
ここで $w^{(i)}$ は正規化された重みです。ESS の性質は直感的です。
- すべての重みが等しい場合（$w^{(i)} = 1/N$）: $N_{\text{eff}} = N$（全粒子が有効）
- 1つの粒子の重みが1で他がすべて0の場合: $N_{\text{eff}} = 1$（実質1粒子）
ESS は常に $1 \leq N_{\text{eff}} \leq N$ の範囲にあり、値が小さいほど縮退が進んでいることを示します。
なぜ縮退が起こるのか
SIS では、重みが時刻ごとに乗算的に更新されます。各時刻の乗算因子が少しでも異なると、対数スケールではその差がランダムウォーク的に蓄積します。これは、ランダムウォークの分散が時間とともに線形に増大することと本質的に同じ現象です。
数学的には、重みの分散が時間とともに単調に増大することが証明されています（Kong et al., 1994）。つまり、SIS を長時間実行すると、どんな提案分布を使っても重みの縮退は避けられないのです。
この深刻な問題を解決するのが、次に説明するリサンプリングです。リサンプリングは、重みの大きい粒子を複製し、重みの小さい粒子を除去することで、粒子集合をリフレッシュする操作です。
リサンプリング — 多項、系統的、層化
リサンプリングの基本的なアイデアは、「重みが大きい粒子は生き残り、重みが小さい粒子は消える」という自然選択に似ています。生物の進化で環境に適応した個体が生き残るように、観測データとよく合致する粒子が選ばれて次世代に引き継がれます。
リサンプリングでは、$N$ 個の重み付き粒子 $\{(\bm{x}^{(i)}, w^{(i)})\}_{i=1}^{N}$ から、重みに比例する確率で $N$ 個の粒子を復元抽出します。リサンプリング後の粒子は等しい重み $1/N$ を持ちます。
多項リサンプリング（Multinomial Resampling）
最も直感的なリサンプリング方法です。各粒子 $i$ が選ばれる確率が $w^{(i)}$ である多項分布から、$N$ 回独立にサンプリングします。
アルゴリズムは次の通りです。
- 重みの累積分布関数（CDF）を計算する: $C_i = \sum_{j=1}^{i} w^{(j)}$
- $N$ 個の一様乱数 $u^{(i)} \sim U(0, 1)$ を独立に生成する
- 各 $u^{(i)}$ に対して、$C_{j-1} < u^{(i)} \leq C_j$ となる $j$ を見つけ、粒子 $j$ を選択する
多項リサンプリングは実装が簡単ですが、$N$ 個の独立な乱数を使うため分散が大きいという欠点があります。
系統的リサンプリング（Systematic Resampling）
系統的リサンプリングは、1つの乱数だけを使って等間隔にサンプリングすることで分散を低減します。
- 重みの CDF $C_i$ を計算する
- 1つの一様乱数 $u_0 \sim U(0, 1/N)$ を生成する
- サンプリング点を $u^{(i)} = u_0 + (i – 1) / N$（$i = 1, \dots, N$）とする
- 各 $u^{(i)}$ に対して CDF を使って粒子を選択する
系統的リサンプリングは、粒子の複製数の期待値が $N w^{(i)}$ に近くなり、多項リサンプリングよりも分散が小さいことが知られています。計算量も $O(N)$ で効率的です。
層化リサンプリング（Stratified Resampling）
層化リサンプリングは、区間 $[0, 1]$ を $N$ 等分し、各小区間内で独立に一様乱数を生成します。
- 重みの CDF $C_i$ を計算する
- 各 $i = 1, \dots, N$ に対して、$\tilde{u}^{(i)} \sim U(0, 1/N)$ を独立に生成する
- サンプリング点を $u^{(i)} = (i – 1)/N + \tilde{u}^{(i)}$ とする
- 各 $u^{(i)}$ に対して CDF を使って粒子を選択する
層化リサンプリングは、系統的リサンプリングと同等の低分散を実現しながら、各小区間で独立な乱数を使うため完全に非決定論的です。
3手法の比較
| 手法 | 乱数の個数 | 分散 | 計算量 |
|---|---|---|---|
| 多項 | $N$ 個（独立） | 大 | $O(N \log N)$ |
| 系統的 | 1個 | 小 | $O(N)$ |
| 層化 | $N$ 個（層化） | 小 | $O(N)$ |
実用上は系統的リサンプリングが最もよく使われます。分散が小さく、実装も簡単で、計算量も少ないためです。
いつリサンプリングするか
毎ステップでリサンプリングすると、粒子の多様性が失われるサンプル枯渇（sample impoverishment）が起こりえます。そこで、ESS がしきい値 $N_{\text{th}}$（通常 $N/2$）を下回ったときにのみリサンプリングする適応的リサンプリングが広く使われます。
リサンプリングの仕組みが理解できたところで、SIS とリサンプリングを組み合わせた最も基本的なパーティクルフィルタのアルゴリズム — SIR（Sampling Importance Resampling）アルゴリズムを見ていきましょう。
SIR アルゴリズム（Bootstrap Particle Filter）
SIR アルゴリズム（Bootstrap Particle Filter とも呼ばれる）は、Gordon, Salmond, Smith（1993）によって提案された、最も基本的かつ広く使われるパーティクルフィルタです。このアルゴリズムは、提案分布として状態遷移モデル（事前分布）を使い、各ステップでリサンプリングを行います。
アルゴリズムの流れ
SIR アルゴリズムの各ステップを順に説明します。
初期化（$k = 0$）: – 初期分布 $p(\bm{x}_0)$ から $N$ 個の粒子をサンプリング: $\bm{x}_0^{(i)} \sim p(\bm{x}_0)$ – 重みを均一に設定: $w_0^{(i)} = 1 / N$
各時刻 $k = 1, 2, \dots$ で以下を繰り返す:
ステップ1: 予測（サンプリング） 各粒子を状態遷移モデル（提案分布）を通して時間発展させます。
$$ \hat{\bm{x}}_k^{(i)} \sim p(\bm{x}_k | \bm{x}_{k-1}^{(i)}) $$
直感的には、各粒子が「自分なりの予測」を行って少し先の未来に移動する操作です。
ステップ2: 更新（重み計算） 新しい観測 $\bm{z}_k$ に基づいて、各粒子の重みを尤度で更新します。
$$ \tilde{w}_k^{(i)} = p(\bm{z}_k | \hat{\bm{x}}_k^{(i)}) $$
そして正規化します。
$$ w_k^{(i)} = \frac{\tilde{w}_k^{(i)}}{\sum_{j=1}^{N} \tilde{w}_k^{(j)}} $$
観測データとよく合致する粒子ほど大きな重みを得ます。
ステップ3: リサンプリング ESS が閾値を下回ったら（または毎ステップ）、重みに比例した確率で粒子を復元抽出し、重みをリセットします。
$$ N_{\text{eff}} = \frac{1}{\sum_{i=1}^{N} (w_k^{(i)})^2} $$
$N_{\text{eff}} < N_{\text{th}}$ ならリサンプリングを実行します。
ステップ4: 状態推定 重み付き平均で状態の推定値を計算します。
$$ \hat{\bm{x}}_k = \sum_{i=1}^{N} w_k^{(i)} \, \bm{x}_k^{(i)} $$
SIR アルゴリズムの擬似コード
以下に、SIR アルゴリズムの擬似コードをまとめます。
Algorithm: SIR Particle Filter
────────────────────────────────────
Input: N (粒子数), z_{1:K} (観測系列)
Output: x̂_{1:K} (状態推定系列)
1. 初期化:
for i = 1 to N:
x₀⁽ⁱ⁾ ~ p(x₀)
w₀⁽ⁱ⁾ = 1/N
2. for k = 1 to K:
(a) 予測:
for i = 1 to N:
x̃ₖ⁽ⁱ⁾ ~ p(xₖ | xₖ₋₁⁽ⁱ⁾)
(b) 重み更新:
for i = 1 to N:
w̃ₖ⁽ⁱ⁾ = p(zₖ | x̃ₖ⁽ⁱ⁾)
正規化: wₖ⁽ⁱ⁾ = w̃ₖ⁽ⁱ⁾ / Σⱼ w̃ₖ⁽ʲ⁾
(c) 状態推定:
x̂ₖ = Σᵢ wₖ⁽ⁱ⁾ x̃ₖ⁽ⁱ⁾
(d) リサンプリング (if N_eff < N/2):
{xₖ⁽ⁱ⁾} = Resample({x̃ₖ⁽ⁱ⁾}, {wₖ⁽ⁱ⁾})
wₖ⁽ⁱ⁾ = 1/N
SIR アルゴリズムの理論的な枠組みが整いました。いよいよ、Python での実装に移りましょう。
Python スクラッチ実装（1次元非線形モデル）
ここでは、パーティクルフィルタの理解を深めるために、有名な1次元非線形ベンチマーク問題を使って SIR アルゴリズムを Python でスクラッチ実装します。このベンチマーク問題は、以下のような状態空間モデルで定義されます。
$$ x_k = \frac{x_{k-1}}{2} + \frac{25 x_{k-1}}{1 + x_{k-1}^2} + 8 \cos(1.2 k) + v_k $$
$$ z_k = \frac{x_k^2}{20} + w_k $$
ここで $v_k \sim \mathcal{N}(0, \sigma_v^2)$、$w_k \sim \mathcal{N}(0, \sigma_w^2)$ です。このモデルは Gordon et al.（1993）の原論文でも使用されたもので、強い非線形性を持っています。状態遷移関数は $\cos$ 項を含む非線形関数であり、観測関数は $x^2/20$ なので状態の符号が観測からは分かりません。つまり、$x_k = 5$ と $x_k = -5$ は同じ観測を生成するため、事後分布がマルチモーダルになる可能性があります。
まず、基本的な関数群を実装します。
import numpy as np
import matplotlib.pyplot as plt
from scipy.stats import norm
# 乱数のシードを固定（再現性のため）
np.random.seed(42)
# --- モデルパラメータ ---
sigma_v = np.sqrt(10.0) # システムノイズの標準偏差
sigma_w = np.sqrt(1.0) # 観測ノイズの標準偏差
def state_transition(x, k):
"""状態遷移関数 f(x, k)"""
return x / 2.0 + 25.0 * x / (1.0 + x**2) + 8.0 * np.cos(1.2 * k)
def observation_function(x):
"""観測関数 h(x)"""
return x**2 / 20.0
def likelihood(z, x):
"""尤度関数 p(z|x): 観測モデルに基づく"""
z_pred = observation_function(x)
return norm.pdf(z, loc=z_pred, scale=sigma_w)
上のコードでは、状態遷移関数、観測関数、尤度関数をそれぞれ定義しています。尤度関数は、予測された観測と実際の観測との差がガウス分布に従うと仮定しており、scipy.stats.norm.pdf を使って計算しています。
次に、真の状態と観測データを生成します。
# --- 真の状態と観測データの生成 ---
T = 50 # 時刻ステップ数
x_true = np.zeros(T)
z_obs = np.zeros(T)
# 初期状態
x_true[0] = 0.1
# 時系列データの生成
for k in range(1, T):
x_true[k] = state_transition(x_true[k-1], k) + sigma_v * np.random.randn()
for k in range(T):
z_obs[k] = observation_function(x_true[k]) + sigma_w * np.random.randn()
print(f"真の状態の範囲: [{x_true.min():.2f}, {x_true.max():.2f}]")
print(f"観測値の範囲: [{z_obs.min():.2f}, {z_obs.max():.2f}]")
このコードでは50ステップ分のシミュレーションデータを生成しています。真の状態は非線形の状態遷移関数にシステムノイズを加えて生成し、観測は状態の二乗を20で割った値に観測ノイズを加えたものです。出力される状態と観測の範囲を確認することで、モデルの動的な挙動を把握できます。
続いて、3種類のリサンプリング手法を実装します。
# --- リサンプリング手法の実装 ---
def multinomial_resample(weights):
"""多項リサンプリング"""
N = len(weights)
cumsum = np.cumsum(weights)
indices = np.searchsorted(cumsum, np.random.rand(N))
return indices
def systematic_resample(weights):
"""系統的リサンプリング"""
N = len(weights)
positions = (np.arange(N) + np.random.rand()) / N
cumsum = np.cumsum(weights)
indices = np.searchsorted(cumsum, positions)
return indices
def stratified_resample(weights):
"""層化リサンプリング"""
N = len(weights)
positions = (np.arange(N) + np.random.uniform(0, 1, N)) / N
cumsum = np.cumsum(weights)
indices = np.searchsorted(cumsum, positions)
return indices
def effective_sample_size(weights):
"""有効サンプルサイズ（ESS）の計算"""
return 1.0 / np.sum(weights**2)
3つのリサンプリング手法はいずれも CDF（累積分布関数）を利用していますが、サンプリング点の生成方法が異なります。多項リサンプリングは $N$ 個の独立な一様乱数を使い、系統的リサンプリングは1つの乱数から等間隔の点を生成し、層化リサンプリングは $N$ 個の小区間それぞれに独立な乱数を配置します。np.searchsorted を使うことで、効率的に CDF の逆引きを行っています。
いよいよ、SIR パーティクルフィルタの本体を実装します。
# --- SIR パーティクルフィルタの実装 ---
def sir_particle_filter(z_obs, N_particles, resample_method='systematic',
threshold_ratio=0.5):
"""
SIR パーティクルフィルタ（Bootstrap Particle Filter）
Parameters
----------
z_obs : array, shape (T,)
観測系列
N_particles : int
粒子数
resample_method : str
リサンプリング手法 ('multinomial', 'systematic', 'stratified')
threshold_ratio : float
リサンプリングを行う ESS のしきい値（N_particles に対する比率）
Returns
-------
x_est : array, shape (T,)
状態推定値（重み付き平均）
x_particles_history : array, shape (T, N_particles)
各時刻の粒子の位置
w_history : array, shape (T, N_particles)
各時刻の正規化重み
ess_history : array, shape (T,)
各時刻の有効サンプルサイズ
"""
T = len(z_obs)
N = N_particles
threshold = threshold_ratio * N
# リサンプリング手法の選択
resample_fn = {
'multinomial': multinomial_resample,
'systematic': systematic_resample,
'stratified': stratified_resample
}[resample_method]
# 記録用の配列
x_est = np.zeros(T)
x_particles_history = np.zeros((T, N))
w_history = np.zeros((T, N))
ess_history = np.zeros(T)
# 初期化: 事前分布 N(0, 5) からサンプリング
particles = np.random.normal(0, 5, N)
weights = np.ones(N) / N
for k in range(T):
if k > 0:
# ステップ1: 予測（状態遷移モデルからサンプリング）
particles = (state_transition(particles, k)
+ sigma_v * np.random.randn(N))
# ステップ2: 重み更新（尤度の計算）
log_weights = norm.logpdf(z_obs[k],
loc=observation_function(particles),
scale=sigma_w)
# 数値安定性のため対数スケールで計算
log_weights -= np.max(log_weights)
weights = np.exp(log_weights)
weights /= np.sum(weights)
# ESS の計算
ess = effective_sample_size(weights)
ess_history[k] = ess
# ステップ3: 状態推定（重み付き平均）
x_est[k] = np.sum(weights * particles)
# 記録
x_particles_history[k] = particles
w_history[k] = weights
# ステップ4: リサンプリング（ESS がしきい値以下の場合）
if ess < threshold:
indices = resample_fn(weights)
particles = particles[indices]
weights = np.ones(N) / N
return x_est, x_particles_history, w_history, ess_history
この実装のポイントをいくつか補足します。重みの計算では対数スケールを使っています。尤度が非常に小さい値になるとアンダーフローが発生するため、対数尤度を計算してから最大値を引く（log-sum-exp トリック）ことで数値安定性を確保しています。また、リサンプリングは ESS がしきい値（デフォルトでは $N/2$）を下回った場合にのみ実行する適応的リサンプリングを採用しています。
それでは、パーティクルフィルタを実行して結果を可視化しましょう。
# --- パーティクルフィルタの実行 ---
N_particles = 500
x_est, x_particles_hist, w_hist, ess_hist = sir_particle_filter(
z_obs, N_particles, resample_method='systematic'
)
# --- 結果の可視化 ---
fig, axes = plt.subplots(3, 1, figsize=(12, 10), sharex=True)
# (a) 状態推定結果
ax = axes[0]
ax.plot(range(T), x_true, 'k-', linewidth=2, label='True state')
ax.plot(range(T), x_est, 'r--', linewidth=1.5, label='PF estimate')
# 粒子の散布（重みを透明度で表現）
for k in range(T):
top_idx = np.argsort(w_hist[k])[-50:] # 重みの大きい上位50粒子
ax.scatter([k]*len(top_idx), x_particles_hist[k, top_idx],
c='blue', s=1, alpha=0.3)
ax.set_ylabel('State $x_k$')
ax.set_title('Particle Filter: State Estimation')
ax.legend(loc='upper right')
ax.grid(True, alpha=0.3)
# (b) 観測データ
ax = axes[1]
ax.plot(range(T), z_obs, 'g.', markersize=5, label='Observations')
ax.set_ylabel('Observation $z_k$')
ax.set_title('Observations')
ax.legend(loc='upper right')
ax.grid(True, alpha=0.3)
# (c) 有効サンプルサイズ
ax = axes[2]
ax.plot(range(T), ess_hist, 'b-', linewidth=1.5, label='ESS')
ax.axhline(y=N_particles/2, color='r', linestyle='--',
label=f'Threshold (N/2={N_particles//2})')
ax.set_xlabel('Time step $k$')
ax.set_ylabel('ESS')
ax.set_title('Effective Sample Size')
ax.legend(loc='upper right')
ax.grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig('particle_filter_result.png', dpi=150, bbox_inches='tight')
plt.show()
上段のグラフでは、黒い実線が真の状態、赤い破線がパーティクルフィルタの推定値、青い点が粒子の分布を表しています。500個の粒子を使った SIR フィルタが、強い非線形性を持つこのベンチマーク問題に対して真の状態をよく追跡していることが確認できます。特に、観測関数 $z = x^2/20$ は状態の符号を区別できないにもかかわらず、粒子が正しい符号の方に集中していることが注目に値します。これは、時間的な相関を通じて符号の曖昧性が解消されるためです。
中段は観測データで、状態の二乗に比例した値にノイズが加わっています。下段の有効サンプルサイズ（ESS）のグラフでは、ESS が閾値（赤い破線、$N/2 = 250$）を下回るたびにリサンプリングが実行されていることがわかります。ESS が大きく低下するのは、観測が極端な値を取って粒子の重みが偏る時刻に対応しています。
次に、推定誤差を定量的に評価するための指標を計算します。
# --- 推定精度の評価 ---
rmse = np.sqrt(np.mean((x_true - x_est)**2))
mae = np.mean(np.abs(x_true - x_est))
avg_ess = np.mean(ess_hist)
resample_count = np.sum(ess_hist < N_particles / 2)
print(f"推定精度の評価（N={N_particles}粒子）:")
print(f" RMSE: {rmse:.4f}")
print(f" MAE: {mae:.4f}")
print(f" 平均ESS: {avg_ess:.1f} / {N_particles}")
print(f" リサンプリング回数: {resample_count} / {T}")
RMSE（二乗平均平方根誤差）と MAE（平均絶対誤差）は、パーティクルフィルタの追跡精度を定量的に示す指標です。平均 ESS が粒子数の半分に近い値であれば、粒子が効率的に使われていることを意味します。リサンプリング回数が多すぎる場合は粒子数の不足が、少なすぎる場合は提案分布が良好であることが示唆されます。
推定の基本的な動作が確認できたところで、次はパーティクルフィルタの真骨頂であるマルチモーダル分布の追跡実験を行いましょう。
実験: マルチモーダル分布での追跡
パーティクルフィルタの最大の強みは、マルチモーダルな事後分布を自然に表現できることです。この節では、その能力を視覚的に確認するために、事後分布の時間発展を可視化します。
先ほどのベンチマーク問題では、観測関数が $z_k = x_k^2 / 20$ であるため、$x_k$ と $-x_k$ が同じ観測を生成します。つまり、観測だけからは状態の符号が判別できず、事後分布が2つのモードを持つ瞬間が存在するはずです。
# --- マルチモーダル分布の可視化 ---
fig, axes = plt.subplots(2, 3, figsize=(15, 8))
# 特定の時刻における粒子分布をヒストグラムで表示
time_steps = [1, 5, 10, 20, 30, 45]
for idx, k in enumerate(time_steps):
ax = axes[idx // 3, idx % 3]
# 重み付きヒストグラム
ax.hist(x_particles_hist[k], bins=50, weights=w_hist[k],
density=True, alpha=0.7, color='steelblue',
edgecolor='white', linewidth=0.5)
# 真の状態
ax.axvline(x=x_true[k], color='red', linewidth=2,
linestyle='--', label=f'True: {x_true[k]:.1f}')
# 推定値
ax.axvline(x=x_est[k], color='orange', linewidth=2,
linestyle='-', label=f'Est: {x_est[k]:.1f}')
ax.set_title(f'$k = {k}$ (ESS = {ess_hist[k]:.0f})')
ax.set_xlabel('$x_k$')
ax.set_ylabel('Weighted density')
ax.legend(fontsize=8)
ax.grid(True, alpha=0.3)
plt.suptitle('Posterior Distribution at Different Time Steps',
fontsize=14, fontweight='bold')
plt.tight_layout()
plt.savefig('particle_filter_multimodal.png', dpi=150, bbox_inches='tight')
plt.show()
この可視化では、6つの異なる時刻における事後分布（粒子の重み付きヒストグラム）を表示しています。各パネルでは、青いヒストグラムが粒子による事後分布の近似、赤い破線が真の状態、オレンジの実線がパーティクルフィルタの推定値を示しています。
注目すべきは、いくつかの時刻で分布が明らかに2つのピークを持っていることです。これは、観測関数 $z = x^2/20$ が $x$ と $-x$ を区別できないために生じるマルチモーダル性です。カルマンフィルタ系の手法ではこの2つのピークを1つのガウス分布で近似してしまうため、2つのモードの中間（本来の状態とは無関係な位置）に推定値が来てしまいます。一方、パーティクルフィルタでは粒子が自然に2つのモードに分配され、重みの大きい方のモードに推定値が寄ります。
次に、パーティクルフィルタの性能を EKF および UKF と定量的に比較してみましょう。
EKF/UKF との比較実験
パーティクルフィルタの利点をより明確にするため、同じベンチマーク問題に対して EKF と UKF を適用し、推定精度を比較します。
まず、EKF を実装します。EKF ではヤコビ行列を解析的に計算する必要があります。
# --- EKF の実装 ---
def ekf_filter(z_obs, Q, R):
"""
拡張カルマンフィルタ（EKF）
Parameters
----------
z_obs : array, shape (T,)
観測系列
Q : float
システムノイズの分散
R : float
観測ノイズの分散
Returns
-------
x_est : array, shape (T,)
状態推定値
P_est : array, shape (T,)
推定誤差の分散
"""
T = len(z_obs)
x_est = np.zeros(T)
P_est = np.zeros(T)
# 初期化
x_est[0] = 0.0
P_est[0] = 5.0
for k in range(1, T):
# 予測ステップ
x_pred = state_transition(x_est[k-1], k)
# 状態遷移関数のヤコビ行列 df/dx
x_prev = x_est[k-1]
F = 0.5 + 25.0 * (1.0 - x_prev**2) / (1.0 + x_prev**2)**2
P_pred = F * P_est[k-1] * F + Q
# 更新ステップ
# 観測関数のヤコビ行列 dh/dx
H = x_pred / 10.0
z_pred = observation_function(x_pred)
# カルマンゲイン
S = H * P_pred * H + R
K = P_pred * H / S
# 状態と共分散の更新
x_est[k] = x_pred + K * (z_obs[k] - z_pred)
P_est[k] = (1 - K * H) * P_pred
return x_est, P_est
次に、UKF を実装します。UKF はヤコビ行列を必要とせず、シグマポイントを使って統計量を計算します。
# --- UKF の実装 ---
def ukf_filter(z_obs, Q, R, alpha=1e-3, beta=2.0, kappa=0.0):
"""
無香カルマンフィルタ（UKF）— 1次元版
Parameters
----------
z_obs : array, shape (T,)
観測系列
Q, R : float
システムノイズと観測ノイズの分散
alpha, beta, kappa : float
UKFのパラメータ
Returns
-------
x_est : array, shape (T,)
状態推定値
P_est : array, shape (T,)
推定誤差の分散
"""
T = len(z_obs)
n = 1 # 状態次元
lam = alpha**2 * (n + kappa) - n
# 重みの計算
Wm_0 = lam / (n + lam)
Wc_0 = Wm_0 + (1 - alpha**2 + beta)
Wm_i = 1.0 / (2.0 * (n + lam))
Wc_i = Wm_i
x_est = np.zeros(T)
P_est = np.zeros(T)
# 初期化
x_est[0] = 0.0
P_est[0] = 5.0
for k in range(1, T):
# シグマポイントの生成
sqrt_P = np.sqrt((n + lam) * P_est[k-1])
sigma_pts = np.array([
x_est[k-1],
x_est[k-1] + sqrt_P,
x_est[k-1] - sqrt_P
])
# 予測ステップ: シグマポイントを状態遷移関数に通す
sigma_pred = state_transition(sigma_pts, k)
# 予測平均と共分散
x_pred = Wm_0 * sigma_pred[0] + 2 * Wm_i * np.sum(sigma_pred[1:])
P_pred = (Wc_0 * (sigma_pred[0] - x_pred)**2
+ 2 * Wc_i * np.sum((sigma_pred[1:] - x_pred)**2)
+ Q)
# 更新用のシグマポイント再生成
sqrt_P_pred = np.sqrt((n + lam) * P_pred)
sigma_upd = np.array([
x_pred,
x_pred + sqrt_P_pred,
x_pred - sqrt_P_pred
])
# 観測の予測
z_sigma = observation_function(sigma_upd)
z_pred = Wm_0 * z_sigma[0] + 2 * Wm_i * np.sum(z_sigma[1:])
# 観測の共分散と相互共分散
Pzz = (Wc_0 * (z_sigma[0] - z_pred)**2
+ 2 * Wc_i * np.sum((z_sigma[1:] - z_pred)**2) + R)
Pxz = (Wc_0 * (sigma_upd[0] - x_pred) * (z_sigma[0] - z_pred)
+ 2 * Wc_i * np.sum(
(sigma_upd[1:] - x_pred) * (z_sigma[1:] - z_pred)))
# カルマンゲイン
K = Pxz / Pzz
# 更新
x_est[k] = x_pred + K * (z_obs[k] - z_pred)
P_est[k] = P_pred - K * Pzz * K
return x_est, P_est
3つの手法を比較する実験を実行しましょう。
# --- 3手法の比較実験 ---
Q = sigma_v**2 # システムノイズの分散
R = sigma_w**2 # 観測ノイズの分散
# EKF
x_ekf, P_ekf = ekf_filter(z_obs, Q, R)
# UKF
x_ukf, P_ukf = ukf_filter(z_obs, Q, R)
# パーティクルフィルタ（粒子数を変えて実験）
particle_counts = [100, 500, 1000]
pf_results = {}
for N_p in particle_counts:
x_pf, _, _, ess_pf = sir_particle_filter(
z_obs, N_p, resample_method='systematic')
pf_results[N_p] = (x_pf, ess_pf)
# RMSE の計算
rmse_ekf = np.sqrt(np.mean((x_true - x_ekf)**2))
rmse_ukf = np.sqrt(np.mean((x_true - x_ukf)**2))
rmse_pf = {N_p: np.sqrt(np.mean((x_true - res[0])**2))
for N_p, res in pf_results.items()}
print("=== RMSE 比較 ===")
print(f" EKF: {rmse_ekf:.4f}")
print(f" UKF: {rmse_ukf:.4f}")
for N_p, rmse in rmse_pf.items():
print(f" PF (N={N_p:4d}): {rmse:.4f}")
RMSE の数値を見ると、このベンチマーク問題においてはパーティクルフィルタが EKF および UKF より低い RMSE を達成していることがわかります。これは、EKF と UKF がガウス近似に制約されているのに対し、パーティクルフィルタが非線形・マルチモーダルな事後分布をそのまま表現できるためです。特に、観測関数 $z = x^2/20$ のように対称性のある非線形関数では、EKF のヤコビ行列による線形化の誤差が大きくなります。
次に、3手法の追跡結果を視覚的に比較します。
# --- 追跡結果の比較プロット ---
fig, axes = plt.subplots(2, 1, figsize=(14, 9))
# 上段: 状態追跡の比較
ax = axes[0]
ax.plot(range(T), x_true, 'k-', linewidth=2.5, label='True state', zorder=5)
ax.plot(range(T), x_ekf, 'b--', linewidth=1.2, alpha=0.8, label=f'EKF (RMSE={rmse_ekf:.2f})')
ax.plot(range(T), x_ukf, 'g-.', linewidth=1.2, alpha=0.8, label=f'UKF (RMSE={rmse_ukf:.2f})')
ax.plot(range(T), pf_results[500][0], 'r-', linewidth=1.2, alpha=0.8,
label=f'PF N=500 (RMSE={rmse_pf[500]:.2f})')
ax.set_ylabel('State $x_k$', fontsize=12)
ax.set_title('Comparison: EKF vs UKF vs Particle Filter', fontsize=14)
ax.legend(fontsize=10)
ax.grid(True, alpha=0.3)
# 下段: 推定誤差の比較
ax = axes[1]
ax.plot(range(T), np.abs(x_true - x_ekf), 'b-', alpha=0.7, label='EKF error')
ax.plot(range(T), np.abs(x_true - x_ukf), 'g-', alpha=0.7, label='UKF error')
ax.plot(range(T), np.abs(x_true - pf_results[500][0]), 'r-', alpha=0.7,
label='PF (N=500) error')
ax.set_xlabel('Time step $k$', fontsize=12)
ax.set_ylabel('Absolute error $|x_k - \\hat{x}_k|$', fontsize=12)
ax.set_title('Absolute Estimation Error', fontsize=14)
ax.legend(fontsize=10)
ax.grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig('pf_ekf_ukf_comparison.png', dpi=150, bbox_inches='tight')
plt.show()
上段のグラフでは、3つのフィルタによる状態追跡の結果を重ねて表示しています。EKF（青い破線）は状態が急変する時刻や観測関数の非線形性が強い領域で大きく外れることがあります。UKF（緑の一点鎖線）は EKF より改善されていますが、やはりガウス近似の限界が見え隠れします。パーティクルフィルタ（赤い実線）は全体的に真の状態（黒い実線）に最も近く追跡できています。
下段の絶対誤差のグラフでは、この差がより明確に見えます。EKF と UKF はいくつかの時刻で非常に大きな誤差のスパイクを示しますが、パーティクルフィルタの誤差は比較的安定しています。これは、EKF/UKF がガウス近似によるモデリングエラーで一度追跡を外すと回復が困難なのに対し、パーティクルフィルタは粒子の多様性によって追跡を維持できるためです。
最後に、リサンプリング手法の比較も行います。
# --- リサンプリング手法の比較 ---
methods = ['multinomial', 'systematic', 'stratified']
n_trials = 20
rmse_by_method = {m: [] for m in methods}
for trial in range(n_trials):
np.random.seed(trial + 100) # 再現性のためのシード
for method in methods:
x_pf_trial, _, _, _ = sir_particle_filter(
z_obs, 200, resample_method=method)
rmse_trial = np.sqrt(np.mean((x_true - x_pf_trial)**2))
rmse_by_method[method].append(rmse_trial)
# 結果の表示
print("\n=== リサンプリング手法比較 (N=200, 20回試行) ===")
print(f"{'手法':<15} {'平均RMSE':>10} {'標準偏差':>10}")
print("-" * 37)
for method in methods:
mean_rmse = np.mean(rmse_by_method[method])
std_rmse = np.std(rmse_by_method[method])
print(f"{method:<15} {mean_rmse:>10.4f} {std_rmse:>10.4f}")
# 箱ひげ図
fig, ax = plt.subplots(figsize=(8, 5))
bp = ax.boxplot([rmse_by_method[m] for m in methods],
labels=['Multinomial', 'Systematic', 'Stratified'],
patch_artist=True)
colors = ['#4C72B0', '#55A868', '#C44E52']
for patch, color in zip(bp['boxes'], colors):
patch.set_facecolor(color)
patch.set_alpha(0.7)
ax.set_ylabel('RMSE', fontsize=12)
ax.set_title('Resampling Method Comparison (N=200, 20 trials)', fontsize=13)
ax.grid(True, alpha=0.3, axis='y')
plt.tight_layout()
plt.savefig('resampling_comparison.png', dpi=150, bbox_inches='tight')
plt.show()
箱ひげ図の結果から、系統的リサンプリングと層化リサンプリングが多項リサンプリングよりも RMSE の中央値が低く、かつ分散も小さいことが確認できます。これは理論的な予測と一致しています。系統的リサンプリングと層化リサンプリングの性能はほぼ同等ですが、系統的リサンプリングは乱数を1つしか使わないため実装がわずかに簡単であり、実用上は系統的リサンプリングが最もよく推奨されます。
ここまでの実験で、パーティクルフィルタの実装と性能評価を一通り行いました。最後に、パーティクルフィルタの応用分野と発展的な話題について概観しましょう。
パーティクルフィルタの応用と発展
主要な応用分野
パーティクルフィルタは、非線形・非ガウスの状態推定が必要なあらゆる分野で活用されています。
ロボットの自己位置推定（MCL）: パーティクルフィルタに基づく Monte Carlo Localization（MCL）は、移動ロボットの自己位置推定における標準手法です。ロボットが環境地図の中でどこにいるかを推定するとき、壁との距離やランドマークの観測を使って粒子の重みを更新します。初期状態が完全に不明な「グローバルローカリゼーション」でも、粒子を地図全体に一様に散布することで対応できる点がカルマンフィルタ系の手法にはない強みです。
ターゲット追跡: 航空機、船舶、ミサイルなどの追跡問題では、目標が急激な旋回やスピードの変化を行う可能性があります。このような機動目標の追跡では、状態遷移モデルが複数の運動モード（等速直線、旋回、加減速）を切り替えるため、事後分布がマルチモーダルになります。パーティクルフィルタは、各粒子が異なる運動モードに対応することで、この複雑な状況に自然に対応します。
SLAM（Simultaneous Localization and Mapping）: FastSLAM と呼ばれるアルゴリズムは、パーティクルフィルタとカルマンフィルタを組み合わせたハイブリッド手法です。ロボットの軌跡をパーティクルフィルタで推定し、各粒子に対応する地図をカルマンフィルタで更新します。このアプローチにより、高次元の SLAM 問題を効率的に解くことができます。
金融工学: 確率的ボラティリティモデルや、レジームスイッチングモデルなど、金融時系列の非線形・非ガウスモデルにおいてパーティクルフィルタが活用されています。市場のクラッシュ時など、分布が正規分布から大きく逸脱する場面での頑健性が評価されています。
発展的な話題
パーティクルフィルタには多くの発展的な手法があります。
Rao-Blackwellized Particle Filter: 状態空間の一部が線形ガウスである場合、その部分をカルマンフィルタで解析的に処理し、残りの非線形部分だけにパーティクルフィルタを適用する手法です。分散を低減でき、少ない粒子数で高い精度が得られます。FastSLAM はこの手法の応用例です。
Auxiliary Particle Filter（APF）: 提案分布の改良として、次の時刻の観測情報を取り入れてサンプリングする手法です。尤度が高い領域に粒子を集中させることで、サンプリング効率を向上させます。
粒子数の自動調整: 推定問題の難しさに応じて粒子数を動的に変更する手法（KLD-sampling など）も研究されています。ESS が低い時には粒子数を増やし、高い時には減らすことで、計算コストと精度のバランスを最適化します。
高次元問題への対応: パーティクルフィルタの最大の弱点は、状態空間の次元が高くなると必要な粒子数が指数的に増大する「次元の呪い」です。これに対処するため、ブロック粒子フィルタや、変分推論と組み合わせたハイブリッド手法などが研究されています。
まとめ
本記事では、パーティクルフィルタ（逐次モンテカルロ法）の理論と実装について、基礎から応用まで解説しました。
- ベイズフィルタリングの一般的枠組みとして、予測ステップ（チャップマン-コルモゴロフ方程式）と更新ステップ（ベイズの定理）を確認しました。カルマンフィルタや EKF/UKF はこの枠組みの特殊ケースです
- モンテカルロ近似の考え方に基づき、確率分布を粒子（サンプル点）の集合で近似する方法を学びました。ガウス分布の仮定を一切置かないため、マルチモーダルな分布も自然に表現できます
- 重点サンプリングを時系列問題に拡張した逐次重点サンプリング（SIS）の重み更新式を導出し、提案分布として状態遷移モデルを選ぶことで重みの更新が尤度の乗算に簡略化されることを示しました
- 重みの縮退問題と、それを解決するリサンプリングの3つの手法（多項、系統的、層化）を比較し、系統的リサンプリングが実用上最も優れていることを確認しました
- SIR アルゴリズムを Python でスクラッチ実装し、1次元非線形ベンチマーク問題に適用しました。パーティクルフィルタがマルチモーダルな事後分布を正しく捉え、EKF や UKF よりも高い推定精度を達成することを実験的に確認しました
- パーティクルフィルタの応用として、ロボットの自己位置推定、ターゲット追跡、SLAM、金融工学などの分野を紹介し、Rao-Blackwellized Particle Filter や Auxiliary Particle Filter などの発展的手法についても概観しました
パーティクルフィルタは「ガウス分布の仮定から完全に自由」という最も一般的なベイズフィルタリング手法であり、非線形・非ガウスの状態推定問題に対する強力なツールです。一方で、計算コストの高さと高次元問題における次元の呪いが実用上の制約となります。問題の性質に応じて、カルマンフィルタ、EKF、UKF、パーティクルフィルタの中から適切な手法を選択する判断力が重要です。
次のステップとして、以下の記事も参考にしてください。
- カルマンフィルタの理論と導出 — パーティクルフィルタの前提となる線形ガウスの場合の厳密解
- 拡張カルマンフィルタ（EKF）の理論と実装 — 線形化による非線形フィルタリングの古典的手法
- 無香カルマンフィルタ（UKF）の理論と実装 — シグマポイントによるガウス近似の改良
- ベイズの定理の基礎 — パーティクルフィルタの理論的基盤