Pythonでパーティクルフィルタを実装してみる

パーティクルフィルタ（Particle filter）は，隠れマルコフモデルやカルマンフィルタと同じように，システムの観測$Y$から状態$X$を推定する手法．どれもベイジアンフィルタに基づくもので，確率分布$p(X_t;Y_{0:t})$の表し方が異なる¹のですが，パーティクルフィルタでは有限個のサンプリングによって確率分布を近似します．今回は重点サンプリング²を使ったパーティクルフィルタを実装してみます．ほかのフィルタに比べてループぐるぐる回すだけだからすごく簡単！


1. 隠れマルコフモデルはヒストグラム（離散），カルマンフィルタはガウシアン（パラメトリック），パーティクルフィルタはサンプリング（ノンパラメトリック）で表す
2. SciPyには有名ドコロの確率分布からサンプリングする関数が用意されている．任意の確率分布からサンプリングしたい場合には逆関数法，棄却サンプリング，重点サンプリングといった手法などを使う

パーティクルフィルタ

• たくさんの粒子をばらまいておいて，それっぽく動かして，観測して，各々実際の観測とのズレを測って，正解っぽいっぽい粒子だけ残す，っていうのを繰り返す
• 入力はN個のパーティクルの状態${\bf x}_t^{(i)}$，重み${\bf w}_t^{(i)}$ $(i=1,...,N)$と制御入力${\bf u}_t$と観測${\bf y}_t$
• 出力は更新された状態${\bf x}_{t+1}^{(i)}$，重み${\bf w}_{t+1}^{(i)}$
• 状態方程式$f$と観測方程式$g$が与えられている

  ${\bf x}_{t+1} = f({\bf x}_t, {\bf u}_t) + {\bf w} \leftrightarrow p({\bf x}_{t+1}|{\bf x}_t, {\bf u}_t)\\
  {\bf y}_t = g({\bf x}_t) + {\bf v} \leftrightarrow p({\bf y}_{t}|{\bf x}_t)$

• 確率分布$p({\bf x}_t|{\bf y}_{0:t})$をN個の重み付きサンプル$\{w_t^{(i)}, {\bf x}_t^{(i)}\}$$(i=1,...,N)$で近似．$\delta$はデルタ関数．

  $p({\bf x}_{t}|{\bf y}_{0:t}) \approx \sum_{i=1}^{N} w_t^{(i)} \cdot \delta ({\bf x}_t - {\bf x}_t^{(i)})$

• 新しい観測${\bf y}_{t+1}$があったら状態推定分布$p({\bf x}_{t+1}|{\bf y}_{0:t+1})$を3つのステップで更新
  
  
  1. 推定
  2. 更新
  3. リサンプリング

例題

2次元座標において、あるロボットが$t=0$に原点を出発して、速度$(4,4)$で動くとする。ロボットの進路は風などの影響を受け（$\sigma_x=\sigma_y=2$），毎秒ごと4つの点$(0,0),(10,0),(0,10),(10,10)$からの距離を計測できて、計測には距離によらない誤差がある（$\sigma_x=\sigma_y=4$）とする．このとき観測された軌跡から実際の軌跡を推定する．

Fig. 1 ピヨピヨ

推定

for i in range(N): ${\bf x}_{t+1}^{(i)} \sim p({\bf x}_{t+1}^{(i)}|{\bf x}_{t}^{(i)}, {\bf u}_{t})$

実際にはN個のパーティクルの位置を状態方程式に代入．

${\bf x}_{t+1}^{(i)} = f({\bf x}_{t}^{(i)}, {\bf u}_{t}) = {\bf A}{\bf x}_{t}^{(i)} + {\bf B}{\bf u}_{t} + {\bf w}$

ただし

${\bf A} = \left[
\begin{array}{cc}
1 & 0 \\
0 & 1 \\
\end{array}
\right],
{\bf B} = \left[
\begin{array}{cc}
1 & 0 \\
0 & 1 \\
\end{array}
\right],
{\bf w} \sim N(0, 2{\bf I})$

更新

for i in range(N): $w_{t+1}^{(i)} \leftarrow w_{t}^{(i)} \cdot p({\bf y}_{t+1}^{(i)}|{\bf x}_{t+1}^{(i)})$

尤度関数によって重みを更新．$\sum^i w_{t+1}^{(i)} = 1$で正規化．今回はモデルを正規分布にしたのでRBFカーネルで．尤度関数は推定値と観測値が似てれば似てるほど大きくなるように設定．物体追跡検知とかだと色の情報を使う．

$p({\bf y}_{t+1}^{(i)}|{\bf x}_{t+1}^{(i)}) \propto \exp(-\frac{(y-g(x))^2}{\sigma^2})$

ただし

$g({\bf x}) = \left[ ||{\bf x}-{\bf p}_1||, ||{\bf x}-{\bf p}_2||, ||{\bf x}-{\bf p}_3||, ||{\bf x}-{\bf p}_4|| \right]^{\mathsf{T}}\\
{\bf p}_1=\left[0, 0\right]^{\mathsf{T}}, {\bf p}_2=\left[10, 0\right]^{\mathsf{T}}, {\bf p}_3=\left[0, 10\right]^{\mathsf{T}}, {\bf p}_4=\left[10, 10\right]^{\mathsf{T}} \\
\sigma^2 = 4$

リサンプリング

$\{ {\bf x}_{t+1}^{(i)}, w_{t+1}^{(i)}=1 \} \leftarrow resampling(\{ {\bf x}_{t+1}^{(i)}, w_{t+1}^{(i)} \})$

重みに応じてリサンプリング．重みが偏らないように．毎回やる必要はない．色々手法があるらしいけど今回は単純に多項分布でサンプリング．

結果

Fig. 2 10ステップ分．●が実際の軌跡．●がパーティクル．●がパーティクル平均．線形システムだから全然パーティクルフィルタ使う意味なかったけど…

実装

要NumPy