カルマンフィルター(その3)
このページでは、線形カルマンフィルターでは扱えない問題にも適用可能な粒子フィルターについて考えてみる。
観測情報を用いた推定
トロッコ問題において、観測データ
ある時点でのトロッコの位置が
もう少し丁寧な考察
時刻
時刻
一方で、一般的な関係
すなわち、
多点サンプリングによる状態の推定
多数のサンプル点を状態モデルに従って確率的に「動かし」ながら分布の変化を推定する手法は、サンプル点を「粒子」になぞらえ、粒子フィルターと呼ばれている。 粒子フィルターを用いたモンテカルロ法は、状態モデルや観測モデルが非線形な場合や、揺らぎや誤差が非ガウス的である場合にも適用可能であり、効率を別とすれば、汎用性に優れている。
トロッコ問題のケースでは、以下の手順によってサンプリングすることができる。
状態モデルによる更新ステップ
時刻
条件付き確率に応じた重み付け
次に、状態
以上を踏まえると、事後確率分布
事後確率に応じたサンプル点集合の再調整
最後に、
粒子フィルターによる「トロッコ」問題の計算例
トロッコ問題について、
# coding: utf-8 import numpy as np import math import random import matplotlib.pyplot as plt N=1000 dt=0.1 xsamp = np.random.normal(0,1,N) vsamp = np.random.normal(0,1,N) wt = np.zeros((N,)) XT=[ ] ZT=[ ] XEST=[ ] T=[ ] R=1 Q=1 x=0 v=1 t=0 while t<10: xsamp = xsamp + vsamp*dt vsamp = vsamp + np.random.normal(0,math.sqrt(R), N) * math.sqrt(dt) z = x + random.gauss(0,math.sqrt(Q)) wt = np.exp(-np.square(z-xsamp)/(2*Q)) wsum = np.sum(wt) wt = wt/wsum xest = wt.dot(xsamp) vest = wt.dot(vsamp) XT.append(x) ZT.append(z) XEST.append(xest) T.append(t) xv=[[x,v] for x,v in zip(xsamp,vsamp)] new_sample = np.array(random.choices(xv, k=N, weights = wt) ) xsamp = new_sample[:,0] vsamp = new_sample[:,1] x = x + v * dt v = v + random.gauss(0,math.sqrt(R))*math.sqrt(dt) t = t + dt plt.plot(T,XT, color='blue', linewidth=1.0, label='State Model') plt.plot(T,XEST, 'o', color='red',label='Estimate') plt.plot(T,ZT, '*', color='green', linewidth=1.0, label='Observation') plt.xlabel('T') plt.ylabel('X') plt.grid(True) plt.legend() plt.show()