パーティクルフィルタ
パーティクルフィルタの概要
理論的な背景となる逐次ベイズフィルタは、式(1)と式(2)で与えられます。逐次ベイスフィルタは、時刻k-1でのパラメタx の 事後分布 p(xk-1|z1:k-1) が得られているとして、1時刻先の事後分布 p(xk|z1:k) を推定するものです。式(1)は、1期先予測と呼ばれ、時刻k-1での事後分布p(xk-1|z1:k-1) とプロセス(状態遷移)モデルp(xk|xk-1)とから、時刻kでのパラメタの分布の予測 p(xk|z1:k-1) を得ます。式(2)は、フィルター/更新(update)と呼ばれ、新たに入手したデータ zk から計算した尤度 p(zk|xk) を用いて、予測 p(xk|z1:k-1) に修正を加え、時刻kでの推定値 p(xk|z1:k) を得ます。
図1:逐次ベイズフィルタの概念図
パーティクルフィルタのアルゴリズムを以下にまとめます。
初期分布を近似する粒子{x0|0(i)}を得る。x0|0(i)〜p(x0)
状態遷移確率分布からサンプリングする。xk|k-1(i)〜N(Fkxk|k-1(i),Qk-1)
各粒子について、尤度から重みを計算する。λ(i)=p(zk|xk|k-1(i))
重みを正規化する w(i)=λ(i)/Σλ(i)
重みに従い、リサンプリングする {xk|k-1(i), w(i)} → {xk|k(i), 1/N}
推定値を計算する。xk=(1/N) Σi xk|k(i)
k→k+1として、2-6を繰り返す。
ただし、簡単化のため、以下の仮定をしています。
プロセスモデルは線形であり、プロセス雑音は正規分布N(0,Qk-1)に従う。
*(i)の(i)は、粒子の番号を表します。尤度p(z|x)は、応用に応じて、観測モデルから定義します。リサンプリングとは、重みに応じて、粒子を増殖/淘汰する過程を表します。例えば、粒子数がN=3であり、それぞれの重みが{0.1, 0.6, 0.3}であったとすると、重みの小さい1番目の粒子は淘汰(削除)され、重みの大きい2番目の粒子は2個に増殖され、3番目の粒子は1個のまま、となります。リサンプリング後の粒子の重みは、均等(1/N)となります。図2は、以上の過程を模式的に描いたものです。
図2:パーティクルフィルタアルゴリズムの概念図
サンプルプログラム
パーティクルフィルタを用いて、サイン波の軌跡(真値)にガウス雑音が乗った観測値から、粒子フィルタによって、軌跡を推定します。
(注意)プログラミングの都合上、観測値z[k]と状態x[k]には、1サンプルのズレがあります。
"""
粒子フィルタを用いたサイン波トラッキング
Ver 1.2, 2019/6/8, F. Asano
"""
import numpy as np
import matplotlib.pyplot as plt
import scipy.stats as st
##################################
# 尤度
def likelihood(z, x, H, R):
w = np.exp( - ((z - np.dot(H,x))**2)/(2*R ))
return w
def normalizeWeight(w):
w = w/np.sum(w)
return w
##################################
# サンプリング
def sampling(x, F, Q):
(Np,M) = x.shape
xx = np.zeros_like(x)
for n in range(Np):
rv = st.multivariate_normal(mean=np.dot(F,x[n]), cov=Q)
xx[n] = rv.rvs(1)
return xx
##################################
# リサンプリング
def resampling(x,w):
(Np,M) = x.shape
xx = np.zeros((Np,M))
c = np.zeros(Np)
c[0] = w[0]
for n in np.arange(1,Np):
c[n] = c[n-1] + w[n]
u = np.zeros(Np)
n = 0
rv = st.uniform(loc=0.0, scale=1/Np)
u[0] = rv.rvs(1)
for k in range(Np):
u[k] = u[0] + k/Np
while ( u[k] > c[n] ):
n += 1
xx[k] = x[n]
return(xx)
##################################
# 観測値の生成
def generateObservation(K):
# 軌跡の真値
t = np.linspace(0, 3*np.pi, K)
s = np.sin(t)
# 雑音
rnd = np.random.RandomState(0)
u = 0.5*rnd.randn(K)
# 観測値
z = s + u
return z, s, t
#############################################
M = 2 # 状態ベクトルの次元数
K = 100 # 観測値のサンプル数
Np = 100 # 粒子数
deltaT = 1 # 観測値のサンプル間隔
F = np.array([[1,deltaT], [0, 1]]) # 状態遷移行列
Q = np.array([[1,0], [0,1]])*1e-3 # システムノイズの共分散行列, default = 1e-3
H = np.array([1.0, 0.0]) # 観測行列
R = 1e1 # default = 1e1 # 観測ノイズの共分散
# 観測値の生成
(z, s, tm) = generateObservation(K)
# 配列
x = np.zeros((K+1,Np,M))
w = np.zeros(Np)
xHat= np.zeros((K+1,M))
# 初期値
for n in range(Np):
x[0,n,:] = np.zeros((1,M))
# 粒子フィルタ
for k in range(K):
# サンプリング
xx = sampling(x[k], F, Q)
# 重みの計算
for n in range(Np):
w[n] = likelihood(z[k], xx[n], H, R)
w = normalizeWeight(w)
# リサンプリング
x[k+1] = resampling(xx, w)
# 推定
xHat[k+1,0] = np.mean(x[k+1,:,0])
xHat[k+1,1] = np.mean(x[k+1,:,1])
# 軌跡の推定値のグラフ
plt.figure(figsize=(10, 6))
plt.plot(tm, s, color='m', label='true')
plt.scatter(tm, z, marker='x', color='b', label='observation')
plt.plot(tm, xHat[1:K+1,0], color='g', label='estimated')
plt.legend(loc='lower right')
plt.xlim(xmin=tm.min(), xmax=tm.max())
plt.xlabel('Time')
plt.title('Trajectory')
# 粒子のグラフ
plt.figure(figsize=(10, 6))
for k in range(K):
plt.plot(tm[k]*np.ones(Np),x[k+1,:,0],'c.')
plt.plot(tm, xHat[1:K+1,0], color='g', label='estimated')
plt.xlabel('Time')
plt.title('Particle distribution')
plt.xlim(xmin=tm.min(), xmax=tm.max())
plt.show()
図3:軌跡の真値、観測値と粒子フィルタによる推定値
図4:粒子の分布