システムが非線形のとき、すなわち
とするとき、関数 f は前の状態から推定値を与え、関数 h は観測値を与えますが、どちらの関数も直接共分散を求めることはできません。が、拡張カルマンフィルタでは状態方程式も観測方程式も微分可能であれば線形である必要はありません。(状態方程式)
(観測方程式)
(ノイズは正規分布)
(状態は正規分布)
拡張カルマンフィルタでは状態方程式と観測方程式の線形化をするために、線形カルマンフィルタにおける時間遷移モデルと観測モデルに各関数の偏微分行列(ヤコビアン)を用います。
あとは、線形カルマンフィルタと同じ。 μt, Σt, ut, yt+1 を入力として、 μt+1, Σt+1を出力します。1ステップのプロセスは以下のとおり。
# prediction
# update(現在の推定値)
(状態方程式の現在のヤコビアン)
(現在の誤差行列)
(観測方程式のヤコビアン)
(観測残差)
(観測残差の共分散)
(最適カルマンゲイン)
(更新された現在の推定値)
(更新された現在の誤差行列)
例題。
2次元座標において、あるロボットがt=0に原点を出発して、速度(2,2)で動くとする。ロボットの進路は風などの影響を受け(σx=σy=1)、毎秒ごと3つの点(0,0),(10,0),(0,10)からの距離を計測できて、計測には距離によらない誤差がある(σx=σy=2)とする。このとき、観測された軌跡から実際の軌跡を推定する。
Fig.1 ロボットの位置は(x0,x1)で、3点からの観測値(y0,y1,y2)を得る。
状態方程式は線形、観測方程式は非線形となります。
観測方程式のヤコビアンは
として求められます。実装では上式を解析的に解く場合、数値的に解く場合を試してみます。なおSciPyには数値的にヤコビアンを求める関数が用意されているので、ここではそれを使ってみます。(微小区間ズラして傾き求めている…)
以下、実装。61行目と62行目で解析的か数値的か切り替えます。
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#!/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() |
割とうまくいっている実行結果。
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