パーティクルフィルタとモンテカルロ位置推定

地図はあるのに自分がどこにいるか分からない

ロボットが地図を与えられていても、センサ観測だけから自己位置を一意に決められるとは限りません。廊下の途中では前方のLIDARスキャンが数十メートル先まで似たパターンを繰り返し、部屋の対称な配置ではどちらの部屋にいるか観測だけでは区別できません。つまり自己位置推定の確率分布は、単峰のガウス分布どころか複数の山を持つ多峰性分布になり得ます。さらに車輪の滑りや不整地でのオドメトリ誤差は、直進・回転が入り交じる非線形な運動モデルに従います。

カルマンフィルタ系の手法は状態分布をガウス分布（平均と共分散）だけで表現するため、多峰性の分布や強い非線形性の前では前提が崩れます。パーティクルフィルタ（逐次モンテカルロ法、SMC）はこの前提を捨て、分布そのものを多数の離散サンプル（パーティクル）の集合で近似します。各パーティクルは「もしロボットがここにいるなら」という1つの仮説（姿勢の候補）であり、パーティクル群全体の疎密が確率分布の形を表現します。

状態分布をサンプル集合で表す

パーティクルフィルタは信念（ビリーフ）を、重み付きサンプルの集合として持ちます。

信念の表現:
  bel(x_t) ≈ { (x_t^[i], w_t^[i]) }_{i=1..N}

  x_t^[i] : i番目パーティクルの状態仮説（例: 姿勢 [x座標, y座標, 向き]）
  w_t^[i] : i番目パーティクルの重み（Σw^[i] = 1 に正規化）
  N       : パーティクル数

ガウス分布が2つのパラメータ（平均・共分散）で分布の形を固定するのに対し、パーティクル集合は分布の形に制約を置きません。パーティクルが2箇所に密集すれば自然に双峰分布を表現でき、運動モデルが非線形でも各パーティクルを個別に非線形関数へ通すだけで済み、線形化（ヤコビ行列など)は一切不要です。この汎用性と引き換えに、精度はパーティクル数 N に依存する統計的な近似になります。

サイクル1: 予測（サンプリング）

各時刻でまず、前時刻のパーティクル群を運動モデルに通して次時刻の仮説を作ります。

予測ステップ（各パーティクル i について）:
  x_t^[i] ~ p(x_t | x_{t-1}^[i], u_t)

  u_t : オドメトリや制御入力（例: 車輪回転量、速度指令）
  p(x_t | x_{t-1}, u_t) : 運動モデル（車輪滑りなどの誤差を含む確率分布）

ここが線形ガウスのカルマンフィルタと決定的に異なる点です。カルマンフィルタは平均と共分散という2つの数値を式で伝播させますが、パーティクルフィルタは運動モデルに従って乱数を使い各パーティクルを個別に動かすだけです。運動モデルがどれほど非線形（回転を含む2輪差動駆動モデルなど）でも、乱数生成と関数適用ができれば予測は成立します。この段階ではまだ観測を使っていないため、パーティクル群は単純に散らばり、分布の不確かさは増加します。

サイクル2: 重要度サンプリングによる重み付け

観測 z_t（LIDARスキャンやランドマーク検出など）が得られたら、各パーティクルにその仮説がどれだけ観測と整合するかを表す重みを付けます。

重み付けステップ（重要度サンプリング）:
  w_t^[i] = w_{t-1}^[i] · p(z_t | x_t^[i])

  p(z_t | x_t^[i]) : 観測モデル（尤度）
                     例: そのパーティクルの位置から地図上で
                     期待されるLIDAR距離と、実測距離の近さ

  重み正規化: w_t^[i] ← w_t^[i] / Σ_j w_t^[j]

この操作は「重要度サンプリング」と呼ばれる一般原理の応用です。真の事後分布から直接サンプリングできない代わりに、扱いやすい提案分布（ここでは運動モデル）からサンプリングし、目標分布との比（＝尤度）を重みとして補正します。実測とよく合致するパーティクルほど重みが大きくなり、地図とかけ離れた場所にいる仮説は重みがゼロに近づきます。重要な点として、この段階ではパーティクルの位置そのものは変えず、重みだけを更新します。

サイクル3: リサンプリングとパーティクル退化問題

重み付けだけを繰り返すと、時間とともにごく一部のパーティクルだけが大きな重みを持ち、残りはほぼゼロになっていきます。この現象をパーティクル退化と呼びます。

対策がリサンプリングです。現在の重み付きパーティクル集合から、重みに比例した確率で N 個を復元抽出し、重みの低い仮説を淘汰、高い仮説を複製した新しい等重みの集合を作ります。

リサンプリング（系統リサンプリングの概要）:
  1. 累積重み分布を作る: c^[i] = Σ_{j=1..i} w^[j]
  2. 開始点 r を [0, 1/N) から1つだけ乱数で決める
  3. しきい値 u_i = r + (i-1)/N (i=1..N) ごとに
     c^[i] を超える最初のパーティクルを選び直す
  4. 選ばれたパーティクル群を新集合とし、重みを 1/N に初期化

単純に重みに比例して独立に N 回抽選する方法（多項リサンプリング）は選択にばらつき（分散）が大きく、系統リサンプリングは1つの乱数点から等間隔にずらして選ぶことで分散を抑え、同じ有効サンプル数でも安定した推定を得られます。毎ステップ無条件にリサンプリングすると、逆に重みの高い正しい仮説まで偶然消えてしまうサンプル枯渇を招くため、先述の有効サンプル数がしきい値を下回ったときだけリサンプリングする適応的な運用が一般的です。

パーティクル数の設計とKLD-サンプリング

N を大きくすれば近似精度は上がりますが計算コストは線形に増えます。固定 N の代わりに、信念の不確かさに応じて必要数を動的に決める**KLDサンプリング（Kullback-Leibler Divergence Sampling）**もよく使われます。状態空間を離散ビンに分割し、パーティクルが新しいビンを占有する頻度から分布の複雑さを推定し、真の分布との統計誤差が一定範囲に収まるまで N を増減させる手法で、収束後（分布が単峰に絞られた後）は少数のパーティクルで済ませ、初期の広い多峰分布ではパーティクル数を増やす、という効率化ができます。

パーティクルフィルタとカルマンフィルタの使い分け

実務での選び方は明確です。ロボットが**自分がどこにいるか全く分からない状態（グローバルローカリゼーション）**から始める、あるいは地図の対称性や遮蔽物によって位置の仮説が複数割れる可能性がある場合はパーティクルフィルタが唯一の現実的な選択肢です。一方、初期位置がおおよそ分かっていて姿勢を連続的に追跡するだけの用途（IMUと車輪オドメトリの融合など）では、ガウス分布の前提で困らないうえに計算量も少ないカルマンフィルタや拡張カルマンフィルタの方が効率的です。高次元の状態（多関節ロボットの全関節角など)にパーティクルフィルタを素朴に適用すると、次元の呪いにより分布を覆うのに必要な N が指数的に増えるため、次元が高い問題ではカルマンフィルタ系か、パーティクルフィルタと解析的な推定を組み合わせた手法（FastSLAMの各ランドマークをカルマンフィルタで扱う設計など）が選ばれます。

まとめ

パーティクルフィルタは、状態の確率分布をガウス分布という形の制約から解放し、多数のサンプルの疎密で表現する逐次モンテカルロ法です。要点は、(1) 各パーティクルが1つの姿勢仮説であり集合全体で多峰性の分布まで表現できること、(2) 予測ではパーティクルを運動モデルに従って個別に動かし、重み付けでは観測尤度による重要度サンプリングで仮説の確からしさを更新すること、(3) 重みが少数のパーティクルに偏るパーティクル退化が主要な弱点で、有効サンプル数を監視しながら系統リサンプリングで淘汰・複製を行うことで対処すること、(4) 線形ガウス系の姿勢追跡ならカルマンフィルタの方が計算効率と最適性で優れ、初期位置不明のグローバル位置推定や多峰性が避けられない場面でこそパーティクルフィルタが真価を発揮すること。この予測・重み付け・リサンプリングのサイクルは、MCLだけでなくFastSLAMなどパーティクルベースのSLAM手法にも共通する骨格です。