カルマンフィルタ

カルマンフィルター
パラメータ推定（２）カルマンフィルター - Qiita
- 事例を使い、順を追った解説がわかりやすい

3行で言うと、

状態空間モデルの実装の1方法
モデルの線形性と誤差分布の正規性が前提
計算が簡単（理解できるとは言っていない）

状態空間モデル | Logics of Blue

実装

Python3

OpenCV3

インテルが開発した画像処理の有名オープンソース。この中に、KalmanFilterというクラスがある。

OpenCV
サンプルコード opencv/kalman.py at master · opencv/opencv
リファレンス OpenCV: cv::KalmanFilter Class Reference

サンプル

状態変数$X = [x, y, v_x, v_y]^T$ という、2次元xy平面上の位置と速度を、位置を入力値（観測値）として推定するモデルを仮定。

まず状態方程式$X(t+1)=AX(t)+w$を個々について定義すると、運動方程式から、以下のようになるのが一般的だろう。

\begin{eqnarray*} x(t+1)&=&x(t) + dt \cdot v_x(t) + w_x \\ y(t+1)&=&y(t) + dt \cdot v_y(t) + w_y \\ v_x(t+1)&=&v_x(t) + w_{v_x} \\ v_y(t+1)&=&v_y(t) + w_{v_y} \end{eqnarray*}

wはプロセスノイズ。カルマンフィルタでは、平均0のガウス分布に従うと仮定する。

これらの関係性を行列にすると、

$$ \begin{bmatrix} x(t+1) \\ y(t+1) \\ v_x(t+1) \\ v_y(t+1) \end{bmatrix} = \begin{bmatrix} 1 & 0 & dt & 0\\ 0 & 1 & 0 & dt\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x(t) \\ y(t) \\ v_x(t) \\ v_y(t) \end{bmatrix} + \begin{bmatrix} w_x \\ w_y \\ w_{v_x} \\ w_{v_y} \end{bmatrix} $$

よって、遷移行列A(transitionMatrix)は上記右辺左項の4×4行列となる。

また、誤差項 $W=[w_x,w_y,w_{v_x},w_{v_y}]^T$ は状態変数の数(今回は4)のベクトルで、$N_4(0,Q)$ に従うとする。$Q$ は誤差共分散行列と呼び、こちらは4×4の正方行列となる。各誤差は独立であると見なせる場合は対角行列となり、opencvのデフォルトでは単位行列となっている。

次に観測方程式$Y=CX+U$について定義する。$U$ は観測ノイズ。観測できるのは位置のみという設定なので、

$$ \begin{bmatrix} x(t) \\ y(t) \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \end{bmatrix} \begin{bmatrix} x(t) \\ y(t) \\ v_x(t) \\ v_y(t) \end{bmatrix} + \begin{bmatrix} u_x \\ u_y \end{bmatrix} $$

となる。観測行列 $C$ (measurementMatrix)は、2×4の行列となる。誤差項 $U$ は $N_2(0,R)$ に従うとする。誤差共分散行列 $R$ は観測値の数(2)の正方行列となる。

以上より、モデルに与えなければならない初期値は、

状態モデル
- 初期状態X(0)
- 遷移行列A
- 誤差共分散行列Q
観測モデル
- 観測行列C
- 誤差共分散行列R

以上を、numpyの行列形式でKalmanFilterインスタンスに与える。

初期値

A, Cはモデルより決まっている（どのモデルが正しいかを測る同定問題というのもあるが、ここでは決まっているとする）。初期状態X(0)もいいだろう。

誤差共分散行列は、適切な与え方がなかなか無く、いろいろ勘と経験によりモデル毎に試して微調整を行うものらしい。

観測ノイズの方（$R$）は、もし状態を固定しての観測が可能であれば、観測値の誤差共分散がそのまま $R$ になる。状態の固定が無理であっても「平滑化を行った観測値」ー「生の観測値」の誤差共分散などで仮定できる。$Q$ の方は一般的な方法は無い。一応、各誤差が独立とすると対角行列になるのが基本である。

カルマンフィルタでノイズ除去 : 情報系研究者の卵が妄想力を高めるために頑張るブログ

初期状態の与え方

初期状態 $X_0$ は、n行1列の numpy.ndarray（matrixも可?）を与える。（nは状態変数の数）

与える先は2通りあり、statePre, statePost のいずれかとなる。predict() を呼べばstatePostの状態を参考に次のstatePreが作られ、correct()を呼べば逆にstatePreから補正してstatePostが更新される。よって、最初に行う操作がpredict()ならstatePostに、correct()ならstatePreに初期値を与えればよい（はず）。

あまりハマる人はいないかも知れないが、一応ハマった例として記録しておく。

statePre, statePostは、予測のたびにそのオブジェクト自体が更新されていく。

たとえば「予測結果を順次、Listに蓄積していく」など、後から初期状態を参照可能にしたければ、statePre, statePost にはcopy()等を使って別オブジェクトとして与えなければならない。

過去の誤った記述（クリックで表示）

import cv2
import numpy as np

# 状態変数(4)、観測値(2) の各サイズを指定してインスタンス作成
# 制御を伴わない場合は制御変数は省略可
# 自分の環境ではnumpyのデフォルトdtypeがfloat64だったので、typeにはCV_64Fを指定

kalman = cv2.KalmanFilter(4, 2, type=cv2.CV_64F)

# 観測行列、状態遷移行列、状態ノイズ行列を指定
dt = 1  # データの時刻差は1単位時間とする
kalman.measurementMatrix = np.eye(2, 4)
kalman.transitionMatrix = np.array([[1, 0, dt, 0],
                                    [0, 1, 0, dt],
                                    [0, 0, 1, 0],
                                    [0, 0, 0, 1]])
# 初期値を与える
kalman.statePost = np.array([[10, 20, 0, 0]]).T

# 観測値で順次更新(観測値はmeasured_xyに入っているとする)
for x, y in measured_xy:
    tp = kalman.predict()  # 予測
    print(tp.T)
    kalman.correct(np.array([x, y]))  # 更新

# 結果(値は適当)
# [[ 10.  20.  0.  0.]]
# [[ 11.  21.  0.  0.]]
# [[ 11.5  22.5  1.  1.]]
# ...

拡張カルマンフィルタ

カルマンフィルタはモデルが線形であることが前提だが、モデルを線形にできない場合、状態遷移行列と観測行列にヤコビアンを用いることで線形に近似する。この手法を拡張カルマンフィルタという。

その他のプロセスは元のカルマンフィルタと変わらないので、transitionMatrix、measurementMatrixに与える行列の変換処理を追加すれば、拡張カルマンフィルタも同様に利用できる。

PyKalman

名称からして、カルマンフィルタに特化したパッケージ。

まだバージョンが1.になってないので、研究や趣味用途以外で使うのはちょっと不安。

目次