pythonでUnscented Kalman Filter実装

2件の返信
[:ja]pykalmanで新しく書き直しました．

最近，全然ブログの更新をしていないのに何故か毎月アクセス数が増えていて，なんだか申し訳ない気持ちになったので久々の更新です．アクセス数の多いページの傾向から察するに，かなりマニアックな人たちが見に来てくれている（このへんの記事[1][2][3]のアクセスが多い）ようなので，今後も一般人には媚びず，マニア向けの(?)内容を書いていこうかと思います．

ということで今回は非線形カルマンフィルタの一種のUnscented Kalman Filterを実装しました．
UKFでは状態の正規分布の平均周りの数点（シグマ点と言います）をサンプリングしてきて，それらの点を実際にシステムを表す非線形関数で変換してやり，変換後の点列から平均・分散を計算することで状態の更新を行います．ヤコビアンの計算が必要ない上に，共分散の変換も二次近似になるため推定精度が良くなるといういいこと尽くめです．詳細は山北先生の論文[1]をどうぞ．丁寧な説明でわかりやすいです．
以前にIMUを使った姿勢推定でも使っていましたが，今回はpython用に書き下してみました．ソースを置いておくので欲しい人は持っていってください．

ukf

今回推定してみたのは距離のみを計測できるセンサを複数使って，対象物体の位置を推定するというようなシステムです．
赤丸が実際の対象位置で，青丸がセンサ位置，灰色の細い円は各センサからの誤差を含んだ観測を表しています．緑太丸が推定の平均位置で，緑細丸はシグマ点です．

wasdキー入力で物体に加速度を与えて，eキーで急停止できます．

実システムでの推定でもちょくちょくUKFを使っていますが，ヤコビアンが要らないというのは非常に素晴らしいですね．ヤコビアンを求めるために一日中微分していると，脳内麻薬でなんだか気持ちがハイになることもありますが，現代人たるものそんな時間の浪費は避けなくてはだめですよね．

ソースukf.zip
(Win7, python 2.7.9, OpenCV 2.4.10, numpy で動作確認しています．)
使用は自由ですが，このソースを利用することによって生じた不利益などには一切責任を持ちません（お決まり文句）．

[ukf.py] 
# coding:utf-8
# ukf.py
# @author tetro
# 16/11/18

import numpy

# Unscented Kalman Filter
# @ref http://ci.nii.ac.jp/naid/110004750819
# 以下のような推定対象のシステムを表すクラスを実装し，このクラスに渡してください
# class System:
#   # システム方程式　ある時刻の状態と入力を受け取り，次の時刻の状態を返す
#   def f( self, state, input ):
#     next_state = ...
#     return next_state
#   # 観測方程式　ある時刻の状態を受け取り，得られるべき観測ベクトルを返す
#   def h( self, state ):
#     measurement = ...
#     return measurement
class UnscentedKalmanFilter:
	# コンストラクタ
	# system : システム方程式fと観測方程式hを持つクラス
	# init_state : 初期状態ベクトル
	# init_cov : 初期共分散行列
	# process_noise : 状態遷移ノイズ共分散行列
	# measurement_noise : 観測ノイズ共分散行列
	def __init__(self, system, init_state, init_cov, process_noise, measurement_noise):
		self.system = system
		self.mean = numpy.array(init_state)
		self.cov = numpy.array(init_cov)
		self.process_noise = numpy.array(process_noise)
		self.measurement_noise = numpy.array(measurement_noise)

		self.k = 1.01

	# 予測ステップ
	# input : システムへの入力
	def predict(self, input):
		# シグマ点の計算後，Unscented Transform
		sigma_points = calc_sigma_points( self.mean, self.cov, self.k )
		for i in range( sigma_points.shape[1] ):
			sigma_points[:,i] = self.system.f( sigma_points[:,i], input )

		mean_pred, cov_pred = calc_mean_cov( sigma_points, self.k )
		cov_pred = cov_pred + self.process_noise

		self.mean = mean_pred
		self.cov = cov_pred

	# 修正ステップ
	# measurement : 観測ベクトル
	def correct(self, measurement):
		measurement = numpy.array(measurement)
		N = self.mean.shape[0]
		K = measurement.shape[0]

		# 誤差も含んだ拡張状態空間
		ext_mean_pred = numpy.zeros(N + K)
		ext_cov_pred = numpy.zeros((N + K, N + K))
		ext_mean_pred[0:N] = self.mean
		ext_cov_pred[0:N, 0:N] = self.cov
		ext_cov_pred[N:N+K, N:N+K] = self.measurement_noise

		# シグマ点の計算後　Unscented Transform
		ext_sigma_points = calc_sigma_points( ext_mean_pred, ext_cov_pred, self.k )
		expected_measurement = numpy.zeros( (K, 2*(N+K) + 1) )
		for i in range( ext_sigma_points.shape[1] ):
			noise = ext_sigma_points[N:N+K, i]
			expected_measurement[:,i] = self.system.h( ext_sigma_points[0:N,i] ) + noise

		ext_w = calc_weights( expected_measurement.shape[1], self.k )
		measurement_mean, measurement_cov = calc_mean_cov( expected_measurement, self.k )

		# カルマンゲインの計算
		sigma = numpy.zeros( (N+K, K) )
		for i in range(ext_sigma_points.shape[1]):
			diff_lhs = (ext_sigma_points[:,i] - ext_mean_pred).reshape( N + K, 1 )
			diff_rhs = (expected_measurement[:,i] - measurement_mean).reshape( 1, K )
			sigma = sigma + ext_w[i] * numpy.dot( diff_lhs, diff_rhs )

		kalman_gain = numpy.dot( sigma, numpy.linalg.inv( measurement_cov ) )

		# 修正後の状態
		ext_mean = ext_mean_pred + numpy.dot( kalman_gain, measurement - measurement_mean )
		ext_cov = ext_cov_pred - numpy.dot( kalman_gain, numpy.dot( measurement_cov, numpy.transpose( kalman_gain ) ) )

		self.mean = ext_mean[0:N]
		self.cov = ext_cov[0:N, 0:N]

# シグマ点列から平均，分散を求める
# sigma_points : シグマ点列
# k : シグマ点の広がりを決めるハイパーパラメータ
def calc_mean_cov(sigma_points, k):
	w = calc_weights(sigma_points.shape[1], k)
	mean_pred = numpy.sum( w * sigma_points, axis = 1 )

	diff = sigma_points - mean_pred.reshape( mean_pred.shape[0], 1 )
	cov_pred = numpy.dot( w * diff, numpy.transpose(diff) )

	return mean_pred, cov_pred

# 平均，分散からシグマ点列を求める
# mean : 平均
# cov : 分散
# k : シグマ点の広がりを決めるハイパーパラメータ
def calc_sigma_points(mean, cov, k):
	cov_ = (mean.shape[0] + k) * cov
	l = numpy.linalg.cholesky(cov_)
	sigma_points = numpy.zeros( (mean.shape[0], 2 * mean.shape[0] + 1) )
	sigma_points[:,0] = mean
	for i in range( mean.shape[0] ):
		sigma_points[:,2*i + 1] = mean + l[:,i]
		sigma_points[:,2*i + 2] = mean - l[:,i]
	return sigma_points

# 各シグマ点の重み
# N : 状態ベクトルの次元数
# k : シグマ点の広がりを決めるハイパーパラメータ
def calc_weights(N, k):
	n = (N - 1) / 2
	weights = numpy.zeros(2*n + 1)
	weights[0] = k / (n + k)
	weights[1:] = 1 / (2 * (n + k))
	return weights
[ukf_demo.py] 
# coding:utf-8
# ukf_demo.py
# @author tetro
# 16/11/18
import cv2
import numpy
from ukf import *

# 推定対象システム
# 距離だけ測ることのできるセンサをN個使って，対象の位置を推定する
# 状態空間 [x, y, vx, vy]
# 入力 [ax, ay]
# 観測 [m1, m2, ..., mN]
class System:
	# コンストラクタ
	def __init__(self):
		# センサ位置
		self.sensors = numpy.array( [(64, 64), (312, 312), (64, 448)] )
#		self.sensors = numpy.array( [(64, 64), (312, 64), (64, 448), (448, 448)] )

	# システム方程式
	def f( self, state, input ):
		next_state = numpy.zeros( state.shape )
		next_state[0:2] = state[0:2] + state[2:4]  	# p[t+1] = p[t] + v[t]
		next_state[2:4] = state[2:4] + input        # v[t+1] = v[t] + a[t]
		return next_state

	# 観測方程式
	def h( self, state ):
		# 各センサから目標点までの距離を返す
		return numpy.linalg.norm( self.sensors - state[0:2], axis = 1 )

# システムと推定結果を描画
def draw( system, ukf, measurement, true_state ):
	canvas = numpy.ones( (512, 512, 3), dtype=numpy.uint8 ) * 255
	# 各センサ位置と観測値
	for sensor, m in zip(system.sensors, measurement):
		cv2.circle( canvas, (sensor[0], sensor[1]), 4, (255, 0, 0), 2 )
		cv2.circle( canvas, (sensor[0], sensor[1]), int(max(1, m)), (200, 200, 200) )

	# 真の位置
	cv2.circle(canvas, (int(true_state[0]), int(true_state[1])), 4, (0,0,255), 2)

	# 推定結果
	cv2.circle(canvas, (int(ukf.mean[0]), int(ukf.mean[1])), 4, (0,255,0), 2)
	sigma_points = numpy.transpose(calc_sigma_points( ukf.mean, ukf.cov, ukf.k ))
	for pt in sigma_points:
		cv2.circle(canvas, (int(pt[0]), int(pt[1])), 3, (32, 255, 32))

	cv2.imshow('canvas', canvas)

# エントリポイント
def main():
	# 推定対象システム，パラメータは適当
	system = System()
	init_state = (448, 448, 0.0, 0.0)
	init_cov = numpy.identity( 4 ) * 0.0001
	process_noise = numpy.identity( 4 ) * 0.1
	measurement_noise = numpy.identity( system.sensors.shape[0] ) * 32 * 32 * 8

	# ukf　生成
	ukf = UnscentedKalmanFilter( system, init_state, init_cov, process_noise, measurement_noise )

	# 真の状態
	true_state = numpy.array((256, 256, 0, 0))
	while True:
		key = cv2.waitKey(20)

		# 入力に応じて加速度を与える
		input = (0,0)
		if key == ord('w'):	   # ↑
			input = (0, -1)
		elif key == ord('s'):  # ↓
			input = (0, 1)
		elif key == ord('a'):  # ←
			input = (-1, 0)
		elif key == ord('d'):  # →
			input = (1, 0)
		elif key == ord('e'):  # stop
			true_state[2:4] = (0, 0)
		elif key == 0x1b:
			break              # 終了

		# 真の状態の更新
		true_state = system.f( true_state, input )
		# 観測値を設定　シミュレーションのためノイズ添加
		measurement = system.h( true_state ) + numpy.random.normal(0, 32, system.sensors.shape[0])
		input = input + numpy.random.normal(0, 0.01, 2)

		# 推定処理
		ukf.predict( input )
		ukf.correct( measurement )

		# 描画
		draw( system, ukf, measurement, true_state )

if __name__ == '__main__':
	main()

余談(極めてマニアック)
Unscented Transformではシグマ点列の広がり具合を決めるためのパラメータλ（あるいはκ）を決める必要があります．

googleで一番上にでてくる論文やwikipedia(日・英)では以下のような式でパラメータを決定しています．

\lambda = \alpha^2(L + \kappa) - L \\\alpha = 10^{-3}, \kappa = 0, \beta = 2

… これって正しいですか？
例えば状態空間が5次元(L=5)として実際に値を代入してみるとλ=-4.999995
各シグマ点の重みは以下の式で与えられるので，

W_s^0 = \frac{\lambda}{L + \lambda} \\W_c^0 = \frac{\lambda}{L + \lambda} + (1 - \alpha^2 + \beta) \\W_s^i = W_c^i = \frac{1}{2(L + \lambda)}

代入してみると…

W_s^0 = -999999.0000378577 \\W_c^1 = -999996.0000388577 \\W_s^i = W_c^i = 100000.00000378577

あきらかに変（と思うんだけど…）．一応，\sum^{2L}_{i=0}W_s^i = 1を満たしているので破綻はしないですけれど，こんなに大きな数字で，しかも負っていうのはあまりに直感に反します．
山北先生の論文ではW_i \ge 0って書いてあるし．なんだか悔しいので論文をいくつも遡っていくと古い文献にこんな表記がありました．

1 \leq \alpha \leq 10^{-4}

0 \leq \alpha \leq 10^{-4}と書きたかったのでしょうか？個人的には1 \leq \alpha \leq 1 + 10^{-4}だったのではないかと考えています．試しに\alpha = 1 + 10^{-2}を重みの式に代入すると…

W_s^0 = 0.019704 \\W_s^i = 0.098030

こっちっぽくないですか？そもそも式的に\alpha \ge 1でないと，どうやったってW_s^0 \le 0になっちゃいますし．確率ロボティクスを見てみても，\kappa=0とか\beta=2とか書いている割に\alphaに関しては\alpha \in \mathbb{R}としか書いてないですし，怪しくないでしょうか．

以前，\alpha = 10^{-3}と設定していて推定が時々おかしくなっていたシステムを，\alpha = 1 + 10^{-3}としてやったら挙動が安定したことがありました．個人的には\alpha = 1 + 10^{-n}を推します．

追記(18/7/5)：
pykalmanの実装を見るとデフォルトでは\alpha = 1にしてるみたい．やっぱり1 \leq \alpha \leq 1 + 10^{-4}？

更に追記：
\alpha << 1は非線形性が強い時にプアだから，\alpha = 1にしとけっていう資料がありました．めちゃめちゃ長かったのでスーパー流し読みしただけなのですが，Conclusionの2パラグラフ目にそんな感じのことが書いてます．

“Given these results I strongly discourage the use of small $\alpha$, as typically suggested in descriptions of the UKF (as, e.g., in Wan and van der Merwe (2001), or on Wikipedia) and suggest to stick with the base or mean sets in which $\alpha$ is implicitly set to 1 (see overview figure over sigma point sets at the end of the theory section).”

他にも非常に高次な空間(D>50)ではUnscented Transformの結果がランダムサンプリングよりいまいちだとか，いろいろ面白いことが書いてあるので後でチェックしておこう．[:en]最近，全然ブログの更新をしていないのに何故か毎月アクセス数が増えていて，なんだか申し訳ない気持ちになったので久々の更新です．アクセス数の多いページの傾向から察するに，かなりマニアックな人たちが見に来てくれている（このへんの記事[1][2][3]のアクセスが多い）ようなので，今後も一般人には媚びず，マニア向けの(?)内容を書いていこうかと思います．

ということで今回は非線形カルマンフィルタの一種のUnscented Kalman Filterを実装しました．
UKFでは状態の正規分布の平均周りの数点（シグマ点と言います）をサンプリングしてきて，それらの点を実際にシステムを表す非線形関数で変換してやり，変換後の点列から平均・分散を計算することで状態の更新を行います．ヤコビアンの計算が必要ない上に，共分散の変換も二次近似になるため推定精度が良くなるといういいこと尽くめです．詳細は山北先生の論文[1]をどうぞ．丁寧な説明でわかりやすいです．
以前にIMUを使った姿勢推定でも使っていましたが，今回はpython用に書き下してみました．ソースを置いておくので欲しい人は持っていってください．

ukf

今回推定してみたのは距離のみを計測できるセンサを複数使って，対象物体の位置を推定するというようなシステムです．
赤丸が実際の対象位置で，青丸がセンサ位置，灰色の細い円は各センサからの誤差を含んだ観測を表しています．緑太丸が推定の平均位置で，緑細丸はシグマ点です．

wasdキー入力で物体に加速度を与えて，eキーで急停止できます．

実システムでの推定でもちょくちょくUKFを使っていますが，ヤコビアンが要らないというのは非常に素晴らしいですね．ヤコビアンを求めるために一日中微分していると，脳内麻薬でなんだか気持ちがハイになることもありますが，現代人たるものそんな時間の浪費は避けなくてはだめですよね．

ソースukf.zip
(Win7, python 2.7.9, OpenCV 2.4.10, numpy で動作確認しています．)
使用は自由ですが，このソースを利用することによって生じた不利益などには一切責任を持ちません（お決まり文句）．

[ukf.py] 
# coding:utf-8
# ukf.py
# @author tetro
# 16/11/18

import numpy

# Unscented Kalman Filter
# @ref http://ci.nii.ac.jp/naid/110004750819
# 以下のような推定対象のシステムを表すクラスを実装し，このクラスに渡してください
# class System:
#   # システム方程式　ある時刻の状態と入力を受け取り，次の時刻の状態を返す
#   def f( self, state, input ):
#     next_state = ...
#     return next_state
#   # 観測方程式　ある時刻の状態を受け取り，得られるべき観測ベクトルを返す
#   def h( self, state ):
#     measurement = ...
#     return measurement
class UnscentedKalmanFilter:
	# コンストラクタ
	# system : システム方程式fと観測方程式hを持つクラス
	# init_state : 初期状態ベクトル
	# init_cov : 初期共分散行列
	# process_noise : 状態遷移ノイズ共分散行列
	# measurement_noise : 観測ノイズ共分散行列
	def __init__(self, system, init_state, init_cov, process_noise, measurement_noise):
		self.system = system
		self.mean = numpy.array(init_state)
		self.cov = numpy.array(init_cov)
		self.process_noise = numpy.array(process_noise)
		self.measurement_noise = numpy.array(measurement_noise)

		self.k = 1.01

	# 予測ステップ
	# input : システムへの入力
	def predict(self, input):
		# シグマ点の計算後，Unscented Transform
		sigma_points = calc_sigma_points( self.mean, self.cov, self.k )
		for i in range( sigma_points.shape[1] ):
			sigma_points[:,i] = self.system.f( sigma_points[:,i], input )

		mean_pred, cov_pred = calc_mean_cov( sigma_points, self.k )
		cov_pred = cov_pred + self.process_noise

		self.mean = mean_pred
		self.cov = cov_pred

	# 修正ステップ
	# measurement : 観測ベクトル
	def correct(self, measurement):
		measurement = numpy.array(measurement)
		N = self.mean.shape[0]
		K = measurement.shape[0]

		# 誤差も含んだ拡張状態空間
		ext_mean_pred = numpy.zeros(N + K)
		ext_cov_pred = numpy.zeros((N + K, N + K))
		ext_mean_pred[0:N] = self.mean
		ext_cov_pred[0:N, 0:N] = self.cov
		ext_cov_pred[N:N+K, N:N+K] = self.measurement_noise

		# シグマ点の計算後　Unscented Transform
		ext_sigma_points = calc_sigma_points( ext_mean_pred, ext_cov_pred, self.k )
		expected_measurement = numpy.zeros( (K, 2*(N+K) + 1) )
		for i in range( ext_sigma_points.shape[1] ):
			noise = ext_sigma_points[N:N+K, i]
			expected_measurement[:,i] = self.system.h( ext_sigma_points[0:N,i] ) + noise

		ext_w = calc_weights( expected_measurement.shape[1], self.k )
		measurement_mean, measurement_cov = calc_mean_cov( expected_measurement, self.k )

		# カルマンゲインの計算
		sigma = numpy.zeros( (N+K, K) )
		for i in range(ext_sigma_points.shape[1]):
			diff_lhs = (ext_sigma_points[:,i] - ext_mean_pred).reshape( N + K, 1 )
			diff_rhs = (expected_measurement[:,i] - measurement_mean).reshape( 1, K )
			sigma = sigma + ext_w[i] * numpy.dot( diff_lhs, diff_rhs )

		kalman_gain = numpy.dot( sigma, numpy.linalg.inv( measurement_cov ) )

		# 修正後の状態
		ext_mean = ext_mean_pred + numpy.dot( kalman_gain, measurement - measurement_mean )
		ext_cov = ext_cov_pred - numpy.dot( kalman_gain, numpy.dot( measurement_cov, numpy.transpose( kalman_gain ) ) )

		self.mean = ext_mean[0:N]
		self.cov = ext_cov[0:N, 0:N]

# シグマ点列から平均，分散を求める
# sigma_points : シグマ点列
# k : シグマ点の広がりを決めるハイパーパラメータ
def calc_mean_cov(sigma_points, k):
	w = calc_weights(sigma_points.shape[1], k)
	mean_pred = numpy.sum( w * sigma_points, axis = 1 )

	diff = sigma_points - mean_pred.reshape( mean_pred.shape[0], 1 )
	cov_pred = numpy.dot( w * diff, numpy.transpose(diff) )

	return mean_pred, cov_pred

# 平均，分散からシグマ点列を求める
# mean : 平均
# cov : 分散
# k : シグマ点の広がりを決めるハイパーパラメータ
def calc_sigma_points(mean, cov, k):
	cov_ = (mean.shape[0] + k) * cov
	l = numpy.linalg.cholesky(cov_)
	sigma_points = numpy.zeros( (mean.shape[0], 2 * mean.shape[0] + 1) )
	sigma_points[:,0] = mean
	for i in range( mean.shape[0] ):
		sigma_points[:,2*i + 1] = mean + l[:,i]
		sigma_points[:,2*i + 2] = mean - l[:,i]
	return sigma_points

# 各シグマ点の重み
# N : 状態ベクトルの次元数
# k : シグマ点の広がりを決めるハイパーパラメータ
def calc_weights(N, k):
	n = (N - 1) / 2
	weights = numpy.zeros(2*n + 1)
	weights[0] = k / (n + k)
	weights[1:] = 1 / (2 * (n + k))
	return weights
[ukf_demo.py] 
# coding:utf-8
# ukf_demo.py
# @author tetro
# 16/11/18
import cv2
import numpy
from ukf import *

# 推定対象システム
# 距離だけ測ることのできるセンサをN個使って，対象の位置を推定する
# 状態空間 [x, y, vx, vy]
# 入力 [ax, ay]
# 観測 [m1, m2, ..., mN]
class System:
	# コンストラクタ
	def __init__(self):
		# センサ位置
		self.sensors = numpy.array( [(64, 64), (312, 312), (64, 448)] )
#		self.sensors = numpy.array( [(64, 64), (312, 64), (64, 448), (448, 448)] )

	# システム方程式
	def f( self, state, input ):
		next_state = numpy.zeros( state.shape )
		next_state[0:2] = state[0:2] + state[2:4]  	# p[t+1] = p[t] + v[t]
		next_state[2:4] = state[2:4] + input        # v[t+1] = v[t] + a[t]
		return next_state

	# 観測方程式
	def h( self, state ):
		# 各センサから目標点までの距離を返す
		return numpy.linalg.norm( self.sensors - state[0:2], axis = 1 )

# システムと推定結果を描画
def draw( system, ukf, measurement, true_state ):
	canvas = numpy.ones( (512, 512, 3), dtype=numpy.uint8 ) * 255
	# 各センサ位置と観測値
	for sensor, m in zip(system.sensors, measurement):
		cv2.circle( canvas, (sensor[0], sensor[1]), 4, (255, 0, 0), 2 )
		cv2.circle( canvas, (sensor[0], sensor[1]), int(max(1, m)), (200, 200, 200) )

	# 真の位置
	cv2.circle(canvas, (int(true_state[0]), int(true_state[1])), 4, (0,0,255), 2)

	# 推定結果
	cv2.circle(canvas, (int(ukf.mean[0]), int(ukf.mean[1])), 4, (0,255,0), 2)
	sigma_points = numpy.transpose(calc_sigma_points( ukf.mean, ukf.cov, ukf.k ))
	for pt in sigma_points:
		cv2.circle(canvas, (int(pt[0]), int(pt[1])), 3, (32, 255, 32))

	cv2.imshow('canvas', canvas)

# エントリポイント
def main():
	# 推定対象システム，パラメータは適当
	system = System()
	init_state = (448, 448, 0.0, 0.0)
	init_cov = numpy.identity( 4 ) * 0.0001
	process_noise = numpy.identity( 4 ) * 0.1
	measurement_noise = numpy.identity( system.sensors.shape[0] ) * 32 * 32 * 8

	# ukf　生成
	ukf = UnscentedKalmanFilter( system, init_state, init_cov, process_noise, measurement_noise )

	# 真の状態
	true_state = numpy.array((256, 256, 0, 0))
	while True:
		key = cv2.waitKey(20)

		# 入力に応じて加速度を与える
		input = (0,0)
		if key == ord('w'):	   # ↑
			input = (0, -1)
		elif key == ord('s'):  # ↓
			input = (0, 1)
		elif key == ord('a'):  # ←
			input = (-1, 0)
		elif key == ord('d'):  # →
			input = (1, 0)
		elif key == ord('e'):  # stop
			true_state[2:4] = (0, 0)
		elif key == 0x1b:
			break              # 終了

		# 真の状態の更新
		true_state = system.f( true_state, input )
		# 観測値を設定　シミュレーションのためノイズ添加
		measurement = system.h( true_state ) + numpy.random.normal(0, 32, system.sensors.shape[0])
		input = input + numpy.random.normal(0, 0.01, 2)

		# 推定処理
		ukf.predict( input )
		ukf.correct( measurement )

		# 描画
		draw( system, ukf, measurement, true_state )

if __name__ == '__main__':
	main()

余談(極めてマニアック)
Unscented Transformではシグマ点列の広がり具合を決めるためのパラメータλ（あるいはκ）を決める必要があります．

googleで一番上にでてくる論文やwikipedia(日・英)では以下のような式でパラメータを決定しています．

\lambda = \alpha^2(L + \kappa) - L \\\alpha = 10^{-3}, \kappa = 0, \beta = 2

… これって正しいですか？
例えば状態空間が5次元(L=5)として実際に値を代入してみるとλ=-4.999995
各シグマ点の重みは以下の式で与えられるので，

W_s^0 = \frac{\lambda}{L + \lambda} \\W_c^0 = \frac{\lambda}{L + \lambda} + (1 - \alpha^2 + \beta) \\W_s^i = W_c^i = \frac{1}{2(L + \lambda)}

代入してみると…

W_s^0 = -999999.0000378577 \\W_c^1 = -999996.0000388577 \\W_s^i = W_c^i = 100000.00000378577

あきらかに変（と思うんだけど…）．一応，\sum^{2L}_{i=0}W_s^i = 1を満たしているので破綻はしないですけれど，こんなに大きな数字で，しかも負っていうのはあまりに直感に反します．
山北先生の論文ではW_i \ge 0って書いてあるし．なんだか悔しいので論文をいくつも遡っていくと古い文献にこんな表記がありました．

1 \leq \alpha \leq 10^{-4}

0 \leq \alpha \leq 10^{-4}と書きたかったのでしょうか？個人的には1 \leq \alpha \leq 1 + 10^{-4}だったのではないかと考えています．試しに\alpha = 1 + 10^{-2}を重みの式に代入すると…

W_s^0 = 0.019704 \\W_s^i = 0.098030

こっちっぽくないですか？そもそも式的に\alpha \ge 1でないと，どうやったってW_s^0 \le 0になっちゃいますし．確率ロボティクスを見てみても，\kappa=0とか\beta=2とか書いている割に\alphaに関しては\alpha \in \mathbb{R}としか書いてないですし，怪しくないでしょうか．

以前，\alpha = 10^{-3}と設定していて推定が時々おかしくなっていたシステムを，\alpha = 1 + 10^{-3}としてやったら挙動が安定したことがありました．個人的には\alpha = 1 + 10^{-n}を推します．

追記:

pykalmanの実装を見るとデフォルトでは[math] [:]

「pythonでUnscented Kalman Filter実装」への2件のフィードバック

コメントを残す

メールアドレスが公開されることはありません。 ※ が付いている欄は必須項目です

このサイトはスパムを低減するために Akismet を使っています。コメントデータの処理方法の詳細はこちらをご覧ください。