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

拡張カルマンフィルタ(EKF; Extended Kalman Filter)は非線形カルマンフィルタのひとつ。線形カルマンフィルタは線形システムを対象としていましたが、拡張カルマンフィルタは非線形システムを対象とします。ナビゲーションやらGPSやらに利用されている、らしい。

システムが非線形のとき、すなわち

${\bf x}_{t+1}=f({\bf x}_{t},{\bf u}_{t})+{\bf w}_{t+1}$ （状態方程式）
${\bf y}_{t}=h({\bf x}_{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})$ （状態は正規分布）

とするとき、関数 f は前の状態から推定値を与え、関数 h は観測値を与えますが、どちらの関数も直接共分散を求めることはできません。が、拡張カルマンフィルタでは状態方程式も観測方程式も微分可能であれば線形である必要はありません。

拡張カルマンフィルタでは状態方程式と観測方程式の線形化をするために、線形カルマンフィルタにおける時間遷移モデルと観測モデルに各関数の偏微分行列（ヤコビアン）を用います。

${\bf A}_{t} = \left\ \frac{\partial f({\bf x}_{t},{\bf u}_{t})}{\partial {\bf x}_{t}} \right|_{{\bf \mu}_t, {\bf u}_t}$
${\bf C}_{t} = \left\ \frac{\partial h({\bf x}_{t})}{\partial {\bf x}_{t}} \right|_{{\bf \mu}_t}$

あとは、線形カルマンフィルタと同じ。 μ_t, Σ_t, u_t, y_t+1 を入力として、 μ_t+1, Σ_t+1を出力します。1ステップのプロセスは以下のとおり。

# prediction

${\bf \hat{\mu}}_{t+1} \leftarrow f({\bf \mu}_{t},{\bf u})$ （現在の推定値）
${\bf A}_{t} \leftarrow \left\ \frac{\partial f({\bf x}_{t},{\bf u}_{t})}{\partial {\bf x}_{t}} \right|_{{\bf \mu}_t, {\bf u}_t}$ （状態方程式の現在のヤコビアン）
${\bf \hat{\Sigma}}_{t+1} \leftarrow {\bf Q}+{\bf A}_t{\Sigma}_t {\bf A}_t^T$ （現在の誤差行列）

# update

${\bf C}_{t+1} \leftarrow \left\ \frac{\partial h({\bf x}_{t})}{\partial {\bf x}_{t}} \right|_{{\bf \hat{\mu}}_{t+1}}$ （観測方程式のヤコビアン）
${\bf \tilde{y}}_{t+1} \leftarrow {\bf y}_{t+1}-{\bf C}_{t+1}{\hat{\mu}}_{t+1}$ （観測残差）
${\bf S}_{t+1} \leftarrow {\bf C}_{t+1}{\bf \hat{\Sigma}}_{t+1}{\bf C}_{t+1}^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}_{t+1}{\bf \hat{\Sigma}}_{t+1}$ （更新された現在の誤差行列）

例題。

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

Fig.1 ロボットの位置は(x₀,x₁)で、3点からの観測値(y₀,y₁,y₂)を得る。

状態方程式は線形、観測方程式は非線形となります。

$f({\bf x},{\bf u})=\left[\begin{a}{cc}1&0\\0&1\end{}\right]{\bf x}+\left[\begin{a}{cc}1&0\\0&1\end{}\right]{\bf u},\ \ \ {\bf u}=\left[\begin{a}{c}2\\2\end{}\right]$
$h({\bf x})=\left[\begin{array}{c} \sqrt{x_0^2+x_1^2}\\ \sqrt{(x_0-10)^2+x_1^2}\\ \sqrt{x_0^2+(x_1-10)^2} \end{array}\right]$

観測方程式のヤコビアンは

$J=\left[\begin{array}{cc} \fr{\pa{h}_0}{\pa{x_0}}&\fr{\pa{h}_0}{\pa{x_1}} \\ \fr{\pa{h}_1}{\pa{x_0}}&\fr{\pa{h}_1}{\pa{x_1}} \\ \fr{\pa{h}_2}{\pa{x_0}}&\fr{\pa{h}_2}{\pa{x_1}} \\ \end{array}\right]$

として求められます。実装では上式を解析的に解く場合、数値的に解く場合を試してみます。なおSciPyには数値的にヤコビアンを求める関数が用意されているので、ここではそれを使ってみます。（微小区間ズラして傾き求めている…）

以下、実装。61行目と62行目で解析的か数値的か切り替えます。

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

	import numpy as np
	import scipy as sp
	from scipy.optimize.slsqp import approx_jacobian
	import matplotlib.pyplot as plt

	def main():
	# 初期化
	T = 50 # 観測数
	p0 = (0,0); p1 = (10,0); p2 = (0,10) # 観測値の座標
	x = np.mat([[0],[0]]) # 初期位置
	X = [np.mat([[0],[0]])] # 状態
	Y = [np.mat([[0],[0],[0]])] # 観測

	# state x = A * x_ + B * u + w, w~N(0,Q)
	A = np.mat([[1,0],[0,1]])
	B = np.mat([[1,0],[0,1]])
	u = np.mat([[2],[2]])
	Q = np.mat([[1,0],[0,1]])
	# observation Y = h(x) + v, v~N(0,R)
	R = np.mat([[2,0,0],[0,2,0],[0,0,2]])
	def h_(x,p):
	return ((x[0]-p[0])2 + (x[1]-p[1])2)**.5
	def h(x):
	x = (x[0,0],x[1,0])
	return np.mat([[h_(x,p0)],[h_(x,p1)],[h_(x,p2)]])
	def Jh0(x):
	"""
	解析的に求めるh(x)のヤコビアン
	"""
	x = (x[0,0],x[1,0])
	return np.mat([[(x[0]-p0[0])/h_(x,p0),(x[1]-p0[1])/h_(x,p0)],
	[(x[0]-p1[0])/h_(x,p1),(x[1]-p1[1])/h_(x,p1)],
	[(x[0]-p2[0])/h_(x,p2),(x[1]-p2[1])/h_(x,p2)]])
	def Jh1(x):
	"""
	数値的に求めるh(x)のヤコビアン
	"""
	x = (x[0,0],x[1,0])
	h = lambda x: np.asfarray([h_(x,p0),h_(x,p1),h_(x,p2)])
	return np.mat(approx_jacobian(x,h,np.sqrt(np.finfo(float).eps)))

	# 観測データの生成
	for i in range(T):
	x = A * x + B * u + np.random.multivariate_normal([0,0],Q,1).T
	X.append(x)
	y = h(x) + np.random.multivariate_normal([0,0,0],R,1).T
	Y.append(y)

	# EKF
	mu = np.mat([[0],[0]])
	Sigma = np.mat([[0,0],[0,0]])
	M = [mu] # 推定
	for i in range(T):
	# prediction
	mu_ = A * mu + B * u
	Sigma_ = Q + A * Sigma * A.T
	# update
	#C = Jh0(mu_) # 解析的
	C = Jh1(mu_) # 数値的
	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)

	# 描画
	a,b = np.array(np.concatenate(X,axis=1))
	plt.plot(a,b,'rs-')
	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 ekf.py hosted with ❤ by GitHub

割とうまくいっている実行結果。

Fig. 2 赤が実際の軌跡。青が推定値。
左が解析的なヤコビアン、右が数値的なヤコビアンを使った結果。

なんかうまくいってないような(?)結果も出てしまいます。

Fig. 3 なんか微妙…

UKFも実装してみようかな。

http://docs.scipy.org/doc/scipy/reference/optimize.html
http://old.nabble.com/calculating-numerical-jacobian-td20506078.html
http://projects.scipy.org/scipy/browser/trunk/scipy/optimize/slsqp.py
http://en.wikipedia.org/wiki/Broyden's_method

satomacoto

June 22, 2011

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

Related Posts

2 comments: