satomacoto: Pythonでカルマンフィルタを実装してみる

カルマンフィルタは、時間変化するシステムの、誤差のある離散的な観測から現在の状態を推定する手法。Wikipediaの記事（カルマンフィルター）がわかりやすい。

状態方程式と観測方程式が次のように与えられているとき

${\bf x}_{t+1}={\bf Ax}_{t}+{\bf Bu}_{t}+{\bf w}_{t+1}$ （状態方程式）
${\bf y}_{t}={\bf Cx}_{t}+{\bf v}_{t}$ （観測方程式）
${\bf w}_{t}\sim N(0,{\bf Q})$ ${\bf v}_{t}\sim N(0,{\bf R})$ （ノイズ）
$p({\bf x}_{t}|{\bf y}_{0:t})=N({\bf x}_{t};{\bf \mu}_{t},{\bf \Sigma}_{t})$ （フィルタ分布）

線形カルマンフィルタ(LKF; Linear Kalman Filter)は μ_t, Σ_t, u_t, y_t+1 を入力として、 μ_t+1, Σ_t+1を出力する。1ステップのプロセスは以下のとおり。

# prediction

${\bf \hat{\mu}}_{t+1} \leftarrow {\bf A\mu}_{t}+{\bf Bu}$ （現在の推定値）
${\bf \hat{\Sigma}}_{t+1} \leftarrow {\bf Q}+{\bf A\Sigma}_t {\bf A}^T$ （現在の誤差行列）

# update

${\bf \tilde{y}}_{t+1} \leftarrow {\bf y}_{t+1}-{\bf C\hat{\mu}}_{t+1}$ （観測残差）
${\bf S}_{t+1} \leftarrow {\bf C}{\bf \hat{\Sigma}}_{t+1}{\bf C}^T+{\bf R}$ （観測残差の共分散）
${\bf K}_{t+1} \leftarrow {\bf \hat{\Sigma}}_{t+1}{\bf C}^T{\bf S}^{-1}$ （最適カルマンゲイン）
${\bf \mu}_{t+1} \leftarrow {\bf \hat{\mu}}_{t+1}+{\bf K}_{t+1}{\bf \tilde{y}}_{t+1}$ （更新された現在の推定値）
${\bf \Sigma}_{t+1} \leftarrow {\bf \hat{\Sigma}}_{t+1}-{\bf K}_{t+1}{\bf C}{\bf \hat{\Sigma}}_{t+1}$ （更新された現在の誤差行列）

観測を得るごとにPredictionとUpdateを繰り返すことで、現在の状態を推定します。

導出は後述（予定）。

例題を。

2次元座標において、あるロボットがt=0に原点を出発して、速度(2,2)で動くとする。ロボットの進路は風などの影響を受け（σ_x=σ_y=1）、毎秒ごとに観測できるGPSによる位置座標には計測誤差（σ_x=σ_y=2）があるとする。このとき、観測された軌跡から実際の軌跡を推定する。

係数は

${\bf A}=\left[ \begin{array}{cc} 1 & 0 \\ 0 & 1 \\ \end{array} \right]$ ${\bf B}=\left[ \begin{array}{cc} 1 & 0 \\ 0 & 1 \\ \end{array} \right]$ ${\bf u}_{const}=\left[ \begin{array}{c} 2 \\ 2 \\ \end{array} \right]$ ${\bf Q}=\left[ \begin{array}{cc} 1 & 0 \\ 0 & 1 \\ \end{array} \right]$
${\bf C}=\left[ \begin{array}{cc} 1 & 0 \\ 0 & 1 \\ \end{array} \right]$ ${\bf R}=\left[ \begin{array}{cc} 2 & 0 \\ 0 & 2 \\ \end{array} \right]$

初期値は

${\bf \mu}_0=\left[ \begin{array}{c} 0 \\ 0 \\ \end{array} \right]$ ${\bf \Sigma}_0=\left[ \begin{array}{cc} 0 & 0 \\ 0 & 0 \\ \end{array} \right]$

コードは以下。要SciPy。（やってる事自体はSciPyは必須ではないのだけど。）

	#!/usr/bin/env python
	# -- coding: utf-8 --

	import numpy as np
	import matplotlib.pyplot as plt

	def lkf(T, Y, U, mu0, Sigma0, A, B, C, Q, R):
	'''Linear Kalman Filter

	- 状態方程式
	x = A * x_ + B * u + w, w ~ N(0,Q)
	- 観測方程式
	y = C * x + v, v ~ N(0,R)

	Parameters
	==========
	- T : ステップ数
	- Y : 観測列
	- U : 入力列
	- mu0 : 初期状態推定値
	- Sigma0 : 初期誤差共分散行列
	- A, B, C, Q, R : カルマンフィルタの係数

	Returns
	=======
	- M : 状態推定値列
	'''

	mu = mu0 # 初期状態推定値
	Sigma = Sigma0 # 初期誤差共分散行列

	M = [mu] # 状態推定値列

	for i in range(T):
	# 推定
	mu_ = A * mu + B * U[i]
	Sigma_ = Q + A * Sigma * A.T

	# 更新
	yi = Y[i+1] - C * mu_
	S = C * Sigma_ * C.T + R
	K = Sigma_ * C.T * S.I
	mu = mu_ + K * yi
	Sigma = Sigma_ - K * C * Sigma_
	M.append(mu)

	return M

	def main():
	# 状態方程式
	# x = A * x_ + B * u + w, w ~ N(0,Q)
	A = np.mat([[1,0], [0,1]])
	B = np.mat([[1,0], [0,1]])
	Q = np.mat([[1,0], [0,1]])
	# 観測方程式
	# y = C * x + v, v ~ N(0,R)
	C = np.mat([[1,0], [0,1]])
	R = np.mat([[2,0], [0,2]])

	# 観測のテストデータの生成
	T = 10 # 観測数
	x = np.mat([[0],[0]]) # 初期位置
	X = [x] # 状態列
	Y = [x] # 観測列
	u = np.mat([[2],[2]]) # 入力（一定）
	U = [u] # 入力列
	for i in range(T):
	x = A * x + B * u + np.random.multivariate_normal([0, 0], Q, 1).T
	X.append(x)
	y = C * x + np.random.multivariate_normal([0, 0], R, 1).T
	Y.append(y)
	U.append(u)

	# LKF
	mu0 = np.mat([[0],[0]]) # 初期状態推定値
	Sigma0 = np.mat([[0,0],[0,0]]) # 初期誤差共分散行列
	M = lkf(T, Y, U, mu0, Sigma0, A, B, C, Q, R)

	# 描画
	a, b = np.array(np.concatenate(X,axis=1))
	plt.plot(a,b,'rs-')
	a, b = np.array(np.concatenate(Y,axis=1))
	plt.plot(a,b,'g^-')
	a, b = np.array(np.concatenate(M,axis=1))
	plt.plot(a,b,'bo-')
	plt.axis('equal')
	plt.show()

	if __name__ == '__main__':
	main()

view raw lkf.py hosted with ❤ by GitHub

実行結果は下図。

Fig.1 ■が実際の軌跡。▲が観測値。●が推定値。

観測に対してカルマンフィルタによる推定値が、実際の軌跡に近いことがわかります。

satomacoto

June 19, 2011

Pythonでカルマンフィルタを実装してみる

Related Posts

No comments:

Post a Comment