update.はじめに前回は拡張カルマンフィルタによる、自己位置推定の可視化を実演しました。Filter)を用いた自己位置推定の可視化を実演していこうと思います。Filter)パーティクルフィルタは基本的に以下4つのサイクルを実行していく形になります。尤度の計算パーティクルフィルタアルゴリズムの詳細については色んなサイトで紹介されているので、あえてここでは説明はしません。アルゴリズムを知りたい方は以下のサイトが参考になると思います。Filterを使用した自己位置推定MATLABサンプルプログラムここでは、パーティクルフィルタアルゴリズムの中でも特に重要そうと思われる個所をピックアップして説明していきます。リサンプリングのタイミング以下を参考しました。ESS)を用いるのが一般的らしいです。\tag{1}\\\end{align}ただし、$N$:パーティルクの数$\omega_{t}^{(i)}$:時刻$t$における、$i$番目のパーティクルの重みなお、算出された$ESS$の値には以下のような意味合いがあります。1$:ひとつを除く、全てのパーティクルの重みが$0$(ひとつの粒子のみが利用されている)$ESS$に任意のしきい値を設定することでリサンプリングを行うタイミングを決定します。・確率ロボティクス第五回リサンプリング手法には以下のようなあります。sampling)確率ロボティクス第五回によると、特に事情がなければ系統抽出法を用いるみたいです。尤度関数の定義と推定方法パーティクルフィルタの良し悪しを決定するのが、この尤度関数の定義になります。最適解付近で尤度が最大となるように設計します。推定方法については「重み付き平均」や「最大重み」を適用するなど、方法はいくつかあるみたいです。一般的には重み付き平均がよく使われるらしいですが、その考察をされた先駆者がおりましたので参考にさせていただきました。・なぜパーティクルフィルタの推定値に「重み付き平均」が使われるのかを考察してみた例題2次元座標において、あるロボットが速度$v[m/s]$、角速度$\omega[deg/s]$で周回移動するものとします。このとき、ロボットの移動には誤差$\boldsymbol{v}(k)$を含みます。ロボットには距離センサが搭載されており、障害物(ここではランドマークとします。)の2次元座標位置を計測することができます。計測には誤差$\boldsymbol{w}(k)$を含みます。ここで、観測されたランドマーク位置から実際の軌跡をパーティクルフィルタによって推定します。ここで、ロボットの姿勢を定義します。$x,y,\theta$はそれぞれ、ロボットの$X$座標、$Y$座標、姿勢角を表し、姿勢ベクトル$\boldsymbol{x}(k)$は以下のように定義されます。\tag{2}\\よって状態方程式は以下のようになります。\tag{4}\\次は観測値$\boldsymbol{y_r}(k)$を定義します。観測ベクトル$\boldsymbol{y_r}(k)$を以下のように定義します。\tag{5}\\添え字の$r$はロボット座標系(ローカル座標系)を表します。よって、$x_r,y_r$はそれぞれロボット座標系にて観測されたランドマークの$X$座標、$Y$座標を表します。観測には観測雑音$\boldsymbol{w}(k)$が含まれることから、$j$番目のランドマークの観測値は以下の式で算出されます。\tag{6}\\例題の説明は以上です。上記例題を実装したスクリプトはGitHubで公開しています。particle_filter.pyまた、スクリプトを実行した結果は以下のようになります。(画像をクリックするとYouTubeへジャンプします。)左図がグローバル座標系を基準とした全体の軌跡を表しています。赤点が現時刻での真の位置を、赤線が過去の真値の軌跡をそれぞれ表しています。青点が現時刻での推定位置を、青線が過去の推定値の軌跡をそれぞれ表しています。星★がランドマークを表しています。右図は赤点を中心とした拡大図です。水色点が現時刻での全パーティクル位置を表しています。おわりに拡張カルマンフィルタとパーティクルフィルタの両方を実装し、実際に動かしてみた感想になります。パーティクルフィルタは2,3週間ほどでできてしまった。手軽に推定ロジックを組むならパーティクルフィルタが適しているかもしれない。拡張カルマンフィルタはパーティクルフィルタに比べ、計算コストが低い!これは設定するパーティクル数$N$に依存するのですが、一般的に拡張カルマンフィルタの計算コストは一定$O(1)$なのに対し、パーティクルフィルタは$O(N)$となります。パーティクル数が多くなれなばなるほど精度が上がりますが、それに比例して計算コストも上がってしまします。パーティクルフィルタは予測に乱数をしようしているため、なんとなく不安になる!パーティクルフィルタでは予測の際に乱数を使用してパーティクルをばら撒きます。よって運が悪いと、最適解付近にばら撒けないときもあるのでは?と思ってしまいます。これはあくまでも私の直観であり、実際に動かしているときはそんなことはありませんでした。upLoginユーザーは見つかりませんでした。