Goで粒子フィルタを実装してみる

この記事はエウレカ Advent Calendar 2015 (Qiita) の22日目の記事になります。

particle_img

皆さん、こんにちは!Androidエンジニアの後藤です。
突然、粒子を弄りたくなってしまったのでこの記事を書き始めました。粒子初心者です。よろしくお願いします。

(この記事はAndroidに全く関係ないです…。ご容赦ください。)

さて、本題に入りたいと思います。

粒子フィルタ

皆さんは「粒子フィルタ」という言葉を聞いたことがありますか?

名前がかっこよすぎてワクワクしますね。粒子フィルタとは何ぞや、ということで Wikipedia 先生に助けてもらいましょう。

粒子フィルタ(英: Particle filter)、または逐次モンテカルロ法 (Sequential Monte Carlo: SMC)とは、シミュレーションに基づく複雑なモデルの推定法である。
この手法はふつうベイズモデルを推定するのに用いられ、バッチ処理であるマルコフ連鎖モンテカルロ法 (MCMC) の逐次 (オンライン) 版である。またこの手法は重点サンプリング法にも似たところがある。 うまく設計すると、粒子フィルタはMCMCよりも高速である。拡張カルマンフィルタや無香カルマンフィルタ (Unscented カルマンフィルタ) に対して、十分なサンプル点があればベイズ最適推定に近付くためにより精度が高くなることから、これらの代わりに用いられることがある。手法を組み合わせ、カルマンフィルタを粒子フィルタの提案分布として使うこともできる。

https://ja.wikipedia.org/wiki/粒子フィルタ から抜粋しました。

難しい、、と思った方のために、更に簡単に図解付きで説明します。

maybe

本当に簡単にですが、こんな感じで粒子が動き回るんだなーと雰囲気を掴んでいただければ、実装するとき楽になります。

個人的には粒子フィルタと遺伝的アルゴリズムが物凄く似ていると思うので、そちらの方が理解が早いかもしれません。
https://ja.wikipedia.org/wiki/遺伝的アルゴリズム

粒子フィルタのアルゴリズム と Go

粒子フィルタを実装するにあたって重要なアルゴリズムを図に書き出してみました。

sample

リサンプリング

重みが付いた粒子を元に散布しなおします。

重みが大きい粒子の周囲には多くの点が散布され、重みが小さい粒子はここで消滅します。

// Resample
// 前状態における重みに基き、粒子を選び直す。(ルーレット選択)
func (f *ParticleFilter) Resample() {
// 累積重み
var ws = make([]float64, f.Number)
ws[0] = f.Particles[0].Weight
for i := 1; i < f.Number; i++ {
ws[i] = ws[i-1] + f.Particles[i].Weight
}

// 一時的な変数に前状態の粒子を入れる
var temp = make([]Particle, f.Number)
for i := 0; i < f.Number; i++ {
temp[i] = f.Particles[i]
}

// 粒子を選び直す
for i := 0; i < f.Number; i++ {
var (
r = float64((rand.Int() % 10000)) / 10000.0
m = 0
)
for j := 0; j < f.Number; j++ {
if ws[j] >= r {
m = j
break
}
}
for k := 0; k < f.Dimension; k++ {
f.Particles[i].X[k] = temp[m].X[k]
}
f.Particles[i].Weight = 0.0
}
}

予測

次の座標を、適当に仮定した値の予測モデルを用いて計算します(ここでは calculate という計算関数)。2次元座標の予測であれば、等速直線運動などを用いて予測値を計算します。

// Predict
// 予測モデル(calculate)に従って粒子の位置を予測する
func (f *ParticleFilter) Predict(calculate func(int, []int) Particle) {
for i := 0; i < f.Number; i++ {
// ノイズ
var noises = make([]int, f.Dimension)
for j := 0; j < f.Dimension; j++ {
noises[j] = (rand.Int() % (f.Noise[j] * 2)) - f.Noise[j]
}

// 予測
f.Particles[i] = calculate(i, noises)

// 定められた最大値と最小値を考慮する
for j := 0; j < f.Dimension; j++ {
if f.Particles[i].X[j] < f.Lower[j] {
f.Particles[i].X[j] = f.Lower[j]
}

if f.Particles[i].X[j] >= f.Upper[j] {
f.Particles[i].X[j] = f.Upper[j] - 1
}
}
}
}

重み付け

粒子の尤度を計算し(ここでは calculate という計算関数)、重みを付け直します。
尤度が低い粒子は予測の正しさが低いので重みを減らし、尤度が高い粒子は逆に重みを増やしていきます。

尤度とはその粒子がどれだけもっともらしいかを表す度数です。

// Weight
// 尤度計算式(calculate)に従って重み付けする
func (f *ParticleFilter) Weight(calculate func(int) Particle) {
// 粒子の尤度計算
for i := 0; i < f.Number; i++ {
f.Particles[i] = calculate(i)
}

// 正規化
var sum = 0.0
for i := 0; i < f.Number; i++ {
sum += f.Particles[i].Weight
}
for i := 0; i < f.Number; i++ {
f.Particles[i].Weight /= sum
}
}

観測

重み付き平均を算出します。
ここで観測された粒子が推定位置になります。

// Measure
// 重み付き平均から、現状態の粒子を推定する
func (f *ParticleFilter) Measure() Particle {

var (
res = NewParticle(f.Dimension)

x = make([]float64, f.Dimension)
)

for i := 0; i < f.Number; i++ {
for j := 0; j < f.Dimension; j++ {
x[j] += float64(f.Particles[i].X[j]) * f.Particles[i].Weight
}
}

for i := 0; i < f.Dimension; i++ {
res.X[i] = int(x[i])
}
return *res
}


ここまで書いたような流れで粒子をフィルタリングすることから「粒子フィルタ」と呼ばれているそうです。


説明に使用したコードは下に置いてあります。

https://github.com/gotokatsuya/particle/blob/master/particle.go

粒子フィルタで物体の位置を推定する

以下の条件で、粒子フィルタが物体追跡できているかテストしてみたいと思います。

  • 初期値
    物体が動く範囲の各点には初期値として 0 という値が存在する。

  • 追跡対象
     y = x の方程式に従って等速直線運動する物体である。
     物体のサイズは10*10とし、物体に重なっている各点の値を 1 とする。

  • 予測モデル
     線形予測(等速直線運動)とする。

  • 尤度計算
     0より大きい数値(ここでは 1 と決め打ちしても良いです)をもっともらしい数値とする。

“`go
func main() {

for i := 0; i < xxx; i++ {

f.Resample()
f.Predict(func(j int, noises []int) particle.Particle {
// 予測モデル
f.Particles[j].X[0] += f.Particles[j].X[2] + noises[0]
f.Particles[j].X[1] += f.Particles[j].X[3] + noises[1]
f.Particles[j].X[2] += noises[2]
f.Particles[j].X[3] += noises[3]
return f.Particles[j]
})
f.Weight(func(j int) particle.Particle {
// 尤度計算
x := f.Particles[j].X[0]
y := f.Particles[j].X[1]
if v, ok := src[point{x, y}]; ok {
if v > 0 {
f.Particles[j].Weight = 1.0
} else {
f.Particles[j].Weight = 0.0001
}
}
return f.Particles[j]
})
res := f.Measure()
// 推定位置
fmt.Println(fmt.Sprintf("(%d, %d)", res.X[0], res.X[1]))

}

}
“`

結果

追跡対象の動き

target

本当はもっといい感じに物体を動かしたかったのですが、今回は簡単に (0,0) ~ (400, 0) まで横一直線に動いてから (400, 0) ~ (400, 400) まで縦一直線に動くモデルを追跡対象にしました。


粒子の動き

particles


グラフを見ると、粒子の動きが追跡対象の動きとほぼ一致しているので、粒子フィルタでの物体追跡(位置推定)に成功しています!

まとめ

今回は実装が比較的簡単な粒子フィルタをご紹介しました。もちろん、粒子フィルタにも利点と欠点が潜んでいるので、カルマンフィルタなどと比較することで、ケースに合わせた活用方法を知ることが大事です。社内の誰かが今後、紹介してくれることに期待しています(もしかしたら自分が紹介することになるかもしれませんが、、)。


Goでこの分野の実装サンプル集が無いので作りたいと思っています。最近の野望です。(そもそもこれGoでやる必要あるの?というツッコミは受け付けていません)まだまだ勉強しなければ。。Goにご興味がある方は是非一度弊社へ!お待ちしています!

参考にした資料

http://mist.suenaga.cse.nagoya-u.ac.jp/trac/wiki/Tutorial/Practice0
http://www.ism.ac.jp/editsec/toukei/pdf/44-1-031.pdf

  • このエントリーをはてなブックマークに追加

エウレカでは、一緒に働いていただける方を絶賛募集中です。募集中の職種はこちらからご確認ください!皆様のエントリーをお待ちしております!

Recommend

JSON Schema から API 仕様と Go コードを自動で生成する – BOT エントリーの裏側 Part.1

レイティングアルゴリズム入門