パーティクルフィルタ

パーティクルフィルタの概要

理論的な背景となる逐次ベイズフィルタは、式(1)と式(2)で与えられます。逐次ベイスフィルタは、時刻k-1でのパラメタx の 事後分布 p(x_k-1|z_1:k-1) が得られているとして、１時刻先の事後分布 p(x_k|z_1:k) を推定するものです。式(1)は、１期先予測と呼ばれ、時刻k-1での事後分布p(x_k-1|z_1:k-1) とプロセス（状態遷移）モデルp(x_k|x_k-1)とから、時刻kでのパラメタの分布の予測 p(x_k|z_1:k-1) を得ます。式(2)は、フィルター/更新(update)と呼ばれ、新たに入手したデータ z_k から計算した尤度 p(z_k|x_k) を用いて、予測 p(x_k|z_1:k-1) に修正を加え、時刻kでの推定値 p(x_k|z_1:k) を得ます。

パーティクルフィルタのアルゴリズムを以下にまとめます。

• 1. 初期分布を近似する粒子{x_0|0⁽ⁱ⁾}を得る。x_0|0⁽ⁱ⁾〜p(x₀)

  2. 状態遷移確率分布からサンプリングする。x_k|k-1⁽ⁱ⁾〜N(F_kx_k|k-1⁽ⁱ⁾,Q_k-1)

  3. 各粒子について、尤度から重みを計算する。λ⁽ⁱ⁾=p(z_k|x_k|k-1⁽ⁱ⁾)

  4. 重みを正規化する w⁽ⁱ⁾⁼λ⁽ⁱ⁾/Σλ⁽ⁱ⁾

  5. 重みに従い、リサンプリングする {x_k|k-1⁽ⁱ⁾, w⁽ⁱ⁾} → {x_k|k⁽ⁱ⁾, 1/N}

1. 推定値を計算する。x_k=(1/N) Σ_i x_k|k⁽ⁱ⁾

2. k→k+1として、2-6を繰り返す。

ただし、簡単化のため、以下の仮定をしています。

• • プロセスモデルは線形であり、プロセス雑音は正規分布N(0,Q_k-1)に従う。

*(i)の(i)は、粒子の番号を表します。尤度p(z_|x)は、応用に応じて、観測モデルから定義します。リサンプリングとは、重みに応じて、粒子を増殖/淘汰する過程を表します。例えば、粒子数がN=3であり、それぞれの重みが{0.1, 0.6, 0.3}であったとすると、重みの小さい１番目の粒子は淘汰（削除）され、重みの大きい２番目の粒子は２個に増殖され、３番目の粒子は１個のまま、となります。リサンプリング後の粒子の重みは、均等(1/N)となります。図２は、以上の過程を模式的に描いたものです。

サンプルプログラム

パーティクルフィルタを用いて、サイン波の軌跡（真値）にガウス雑音が乗った観測値から、粒子フィルタによって、軌跡を推定します。

（注意）プログラミングの都合上、観測値z[k]と状態x[k]には、１サンプルのズレがあります。

"""

粒子フィルタを用いたサイン波トラッキング

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()