はじめに

Gaussian Particle Filter(GPF、ガウシアン粒子フィルタ)¹²の勉強をしたので、GPFによる自己位置推定のプログラムを書きました。

Particle Filter(PF)とは

Gaussian Particle Filter(GPF)はParticle Filter(PF)の派生系です。
そもそもPFを知らない方は、PFによる自己位置推定に関する下記の資料などを参照ください。

『確率ロボティクス』の翻訳者でもある千葉工業大学の上田先生の授業スライド。
パーティクルフィルタを用いた自己位置推定

OSS(Open Source Software)のPythonRoboticsのメイン開発者であるSakai Atushiさんのブログ記事。
Particle Filterを使用した自己位置推定MATLAB, Pythonサンプルプログラム

Gaussian Particle Filter(GPF)とは

アルゴリズム

GPFはPFの派生形であり、リサンプリングを必要としないPF¹です。

PFでは、観測更新の結果,観測に近い特定の粒子に重みが集中し,それ以外の粒子の重みが0になるために重みの偏りが発生する縮退と言う現象が起こります。リサンプリングはこの縮退を防ぐために行われます。

GPFでは、リサンプリングをしない代わりに分布にガウシアンを仮定し、観測更新時に粒子から求めた事後推定値と事後推定誤差共分散をもとに粒子を生成し直します。

片山先生の『非線形カルマンフィルタ』¹においてPFとGPFの優劣は明確に述べられておらず、比較においてもPFとGPFの性能は速度・性能ともにほぼ同程度でした。（もっと縮退が大きい事例においてGPFは真価を発揮するんですかね？）

GPFの導出は『非線形カルマンフィルタ』¹等を参照ください。ここでは、結論を書くに留めます。

記号

ベクトルなどは太字表記にしています。
$t$：離散時間
$N_P$：粒子数
$x$：状態量
$u$：指令値
$f$：システムモデル
$w$：システムモデルの雑音
$X=[x^{(1)},x^{(2)},...,x^{(M)}]$：粒子群
$z$：観測量
$\alpha$：尤度
$p(A|B)$：事象Bが起こるという条件のもとで事象Aが起こる場合の条件付き確率
$\mathbf{N}(\mu,\Sigma)$：平均$\mu$、分散$\Sigma$の正規分布
$I_n$：$n\times n$の単位行列

初期化

事前分布に従って初期の粒子を生成します。

予測更新ステップ

システム雑音$w_{t-1}$からサンプリングされた$w_{t-1}^{(i)}(i=1,…,N_P)$をもとに、
各粒子の前の時間の事後推定値$x_{t-1}^{(i)}$からシステムモデル$f_t$をもとに現在の事前推定値$x_{t}^{(i)}$を予測します。

$ x_{t}^{(i)}=f_t(x_{t-1}^{(i)},u_{t},w_{t-1}^{(i)})$

この予測更新ステップはPFと全く同じです。

観測更新ステップ

1)観測値$z_t$と粒子群$X_t$から尤度$ \alpha_t^{(i)}$を求めます。

$ \alpha_t^{(i)} = p(z_t|x_t=x_t^{(i)})$

次に、$\alpha_t^{(i)}$をもとに正規化した尤度$ \tilde{\alpha}_t^{(i)}$を計算します。

2)尤度と粒子をもとに状態推定値（重み付き平均値）と推定誤差共分散行列を求めます。

状態推定値
$ \hat{x}_{t}=\Sigma_{i=1}^{N_P}\tilde{\alpha}_t^{(i)}x_{t}^{(i)}$
推定誤差共分散行列
$ P_{t} = \Sigma_{i=1}^{N_P}\tilde{\alpha}_t^{(i)}[x_{t}^{(i)}-\hat{x}_{t}] [x_{t}^{(i)}-\hat{x}_{t}]^T$

3)推定値$\hat{x}_{t}$と共分散行列$ P_{t}$をもとに下記のように各粒子の事後推定値$x_{t}^{(i)}$を生成します。

$ x_{t}^{(i)} = \hat{x}_{t} + \sqrt P_t \zeta_{i}$

ただし、$\zeta_{i}\in\mathcal{R}^{n}$は$\mathbf{N}(0,I_n)$に従う白色雑音です。
4)tをt+1として予測更新ステップに戻ります。

プログラム

問題設定

プログラムではロボットがホイールオドメトリとジャイロによって入力値を，
ランドマーク位置を既知としてランドマークまでの距離を観測値として取得し，
それらをGPFで統合します．

具体的な設定は以下です。

状態量
$\mathbf{x}=[x \quad y \quad \theta \quad v]^T$
$x$：$x$方向のロボット位置、$y$:$y$方向のロボット位置、
$\theta$：ロボットの方位角,$v$：ロボットの速度
観測量
$\mathbf{z_j}=[d_j \quad z_{xj} \quad z_{yj}]^T(j=1,2,..N_L)$
$d$：ロボット-ランドマーク間距離、$z_x$：$x$方向のランドマーク位置、
$z_y$：$y$方向のランドマーク位置、$N_L$：ランドマークの数
指令値
$\mathbf{u}=[u_v \quad u_\omega]^T$
$u_v$：速度指令値、$u_\omega$：角速度指令値
システムモデル
対向二輪モデル（ルンバみたいなモデル）です。

\mathbf{x_t} =
\begin{pmatrix}
1 & 0 & 0 & 0\\
0 & 1 & 0 & 0\\
0 & 0 & 1 & 0\\
0 & 0 & 0 & 0
\end{pmatrix}
\mathbf{x_{t-1}} +
\begin{pmatrix}
dt \cdot cos(\theta) & 0 \\
dt \cdot sin(\theta) & 0 \\
0 & dt \\
1 & 0  
\end{pmatrix}
\mathbf{u_t} + \mathbf{w}

$dt$：時間刻み

尤度
$ p(\mathbf{z}|\mathbf{x}) = \Sigma_{j=1}^{N_L}\frac{1}{\sqrt{2 \pi \sigma^2}} \cdot exp(-\frac{(d_j-\sqrt{(x-z_{xj})^2-(y-z_{yj})^2})^2}{2 \sigma^2})$
$ \sigma^2$：観測値の誤差分散

ソースコード

コードはPythonRoboticsのParticle Filter³をちょっと弄っただけです。
です。
リサンプリングの代わりに下記のコードを加えるのが主な変更点です。

    xEst = px.dot(pw.T)
    PEst = calc_covariance(xEst, px, pw)
    PSqrt = scipy.linalg.sqrtm(PEst)# 共分散行列PEstの平方根
    for ip in range(NP):# 推定値xEstに共分散行列PEstの平方根 x randnの雑音を与えて粒子を生成
                px[:,ip] = xEst.reshape(-1,) + np.diag(PSqrt) * \
                    np.random.randn(xEst.shape[0])

結果

プログラムを回した結果が以下になります。
星がランドマーク、黒い直線がランドマーク-自己位置間距離、黒いグニャグニャしている線がデッドレコニング(ホイールオドメトリ/ジャイロ）の軌跡、青い線が真の軌跡、赤い線が推定された軌跡，赤い線の先端にあるのが粒子です。

比較用にParticle Filterの場合の結果はPythonRoboticsを参照ください。
PythonRoboticsGifs particle_filter
シミュレーションではノイズが正規分布なためか、GPFの方が少し精度が良さそうですね！

コード置き場

Githubにあります。

片山,"非線形カルマンフィルタ",2011,p152-p155,朝倉書店 ↩
J.H. Kotecha et al,"Gaussian particle filtering",2003 ↩
PythonRobotics Particle Filter ↩

Gaussian Particle Filter(GPF)による自己位置推定[PythonRobotics]