Loading [MathJax]/extensions/MathZoom.js

June 22, 2011

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, ut, yt+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)で動くとする。ロボットの進路は風などの影響を受け(σxy=1)、毎秒ごと3つの点(0,0),(10,0),(0,10)からの距離を計測できて、計測には距離によらない誤差がある(σxy=2)とする。このとき、観測された軌跡から実際の軌跡を推定する。
Fig.1 ロボットの位置は(x0,x1)で、3点からの観測値(y0,y1,y2)を得る。

状態方程式は線形、観測方程式は非線形となります。
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

June 19, 2011

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, ut, yt+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)で動くとする。ロボットの進路は風などの影響を受け(σxy=1)、毎秒ごとに観測できるGPSによる位置座標には計測誤差(σxy=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 が実際の軌跡。が観測値。が推定値。

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

Python+SciPyでk-meansを実装してみる

k-meansK平均法)はクラスタリング手法の一つ。

PythonではSciPyに実装されているので簡単に利用することができます。
Macではこちらの記事に書いてある方法で関連するソフトもインストールできます。

たとえば下図のように2種類の2次元正規分布からサンプリングされたデータがあるとき


k-meansを用いると下図のようにクラスタリングされます。


以下、コード。



SciPyを用いるとk-meansは21~22行目の実質2行で利用することができます。

なおサンプリングについては ここ を、kmeansについては ここ を参照。

ちなみに正規化しないと下図のようになってしまいます。


k-meansのアルゴリズムについては、鹿島先生の講義資料の概論②の14~16ページの説明なんかが分かりやすいかもしれません。