カルマンフィルタの勉強

はじめに

画像認識関連の開発や調査をしているとカルマンフィルタに出くわす事がありますが、よくわかっていない部分があったので改めて勉強することにしました。

前半は勉強したことのまとめ、後半は具体的な数値シミュレーション例を記載します。

カルマンフィルタとは

概要

観測値からシステムの状態を推定するアルゴリズム。逐次ベイズフィルタの一種。
1960年代にカルマン(Rudolf Emil Kalman)によって提案され、アポロ計画において人工衛星の軌道推定に使われたことで有名になった。
現在でも、航空・宇宙工学、ロボット工学、画像処理、計量経済学、生物学といった幅広い分野においてカルマンフィルタおよびそれを拡張したアルゴリズムが活用されている。

処理の流れ

以下にカルマンフィルタを利用する流れを簡単にまとめます。

1. 対象とする時系列システムのモデリング
一般にカルマンフィルタでは「状態空間モデル」を用いて状態を推定します。
※状態空間モデル：測定などで直接得られる「観測値」と、直接得られない潜在的な「状態」を設定するモデル

ここでは、以下のような離散時間の状態空間モデルを考えます。

$\boldsymbol{x}(k+1)=\boldsymbol{Ax}(k) + \boldsymbol{b}v(k)$

$y(k) = \boldsymbol{c}^{T}\boldsymbol{x}(k) + w(k)$

1つ目の式はある時刻の状態から次の状態を求める式であり、「状態方程式」とも呼ばれます。
2つ目の式は状態から観測値を得る式であり、「観測方程式」とも呼ばれます。

ここで $\boldsymbol{x}(k)$ はn次元の状態ベクトル、 $y(k)$ は観測値(スカラー)です。

$v(k)$ はシステムノイズと呼ばれ、平均0, 分散 $\sigma^{2}_{v}$ の正規白色ノイズ、 $w(k)$ は観測ノイズと呼ばれ、平均0, 分散 $\sigma^{2}_{w}$ の正規白色ノイズです。

また、 $\boldsymbol{A}, \boldsymbol{b}, \boldsymbol{c}$ は既知の係数行列・ベクトルであり時間的に一定とします。

2. カルマンフィルタによる状態推定

上で設定した状態空間モデルに対し、カルマンフィルタで状態推定を行うステップを以下に記載します。 ※真の状態 $x$ に対し、状態の推定値は $\hat{x}$ で表します。　

(1) 予測ステップ

$\boldsymbol{\hat{x}}^{-}(k) = \boldsymbol{A\hat{x}}(k-1)$

$\boldsymbol{P}^{-}(k) = \boldsymbol{AP}(k-1)\boldsymbol{A}^{T} + \sigma^{2}_{v}\boldsymbol{bb}^{T}$

予測ステップでは事前状態推定値と事前誤差共分散行列を求めます。

※上付きのマイナス記号は"事前"であることを示しています。

(2) フィルタリングステップ
$\boldsymbol{g}(k) = \dfrac{ \boldsymbol{P}^{-}(k)\boldsymbol{c} }{ \boldsymbol{c}^{T} \boldsymbol{P}^{-}(k)\boldsymbol{c} + \sigma^{2}_{w} }$

$\boldsymbol{\hat{x}}(k) = \boldsymbol{\hat{x}}^{-}(k) + \boldsymbol{g}(k)(y(k) - \boldsymbol{c}^{T}\boldsymbol{\hat{x}}^{-}(k) )$
$\boldsymbol{P}(k) = ( \boldsymbol{I} - \boldsymbol{g}(k)\boldsymbol{c}^{T} )\boldsymbol{P}^{-}(k)$

フィルタリングステップではまず「カルマンゲイン」を計算し、これを用いて状態推定値と誤差共分散行列を更新します。

※カルマンゲインは予測ステップで求めた予測値に対し、"観測"による補正をどれだけ入れるかをコントロールする役割。

以上のように、「予測」と「フィルタリング」から成る一連の更新式を、観測値を受け取る度に逐次的に実行していくのがカルマンフィルタの特徴です。

簡単な具体例

ここでは以下のシステムを考えます。

$x(k+1) = x(k) + v(k)$

$y(k) = x(k) + w(k)$

これは前述の状態空間モデルにおいて状態xをスカラーとし、A=b=c=1とおいたものです。

この場合、カルマンフィルタによる推定のステップは以下となります。

(1) 予測ステップ

$\hat{x}^{-}(k) = \hat{x}(k-1)$

$p^{-}(k) = p(k-1) + \sigma^{2}_{v}$

(2) フィルタリングステップ
$g(k) = \dfrac{ p^{-}(k) }{ p^{-}(k) + \sigma^{2}_{w} }$

$\hat{x}(k) = \hat{x}^{-}(k) + {g}(k)(y(k) - \hat{x}^{-}(k) )$
$p(k) = ( 1 - {g}(k) )p^{-}(k)$

この例で、実際にシミュレーションを行ってみます。　

以下は参考文献のMATLABコードを少し変更してpythonで書いた物です。

import numpy as np
import matplotlib.pyplot as plt
import seaborn as sns
sns.set_style("darkgrid")

# 問題設定
N = 150
var_v = 1 # システムノイズの分散
var_w = 2 # 観測ノイズの分散
# var_w = 20 

np.random.seed(0)
v = np.random.normal(0, np.sqrt(var_v), N) # システムノイズ
w = np.random.normal(0, np.sqrt(var_w), N) # 観測ノイズ

# 真の状態と、観測値を用意する
x = np.zeros(N) # 真の状態
y = np.zeros(N) # 観測値
for k in range(N):
    if k > 0:
        x[k] = x[k - 1] + v[k - 1]
    y[k] = x[k] + w[k]

# ----------------------------------------------

# カルマンフィルタの更新式
def kalman_filter(y, x_est_pre, p_pre, var_v, var_w):
    # 予測ステップ
    x_est = x_est_pre
    p = p_pre + var_v
    
    # フィルタリングステップ
    kalman_gain = p / (p + var_w)
    x_est = x_est + kalman_gain * (y - x_est)
    p = (1 - kalman_gain) * p

    return x_est, p

# カルマンフィルタによる状態推定
x_est = np.zeros(N) # 状態の推定値
p = 0
for k in range(N - 1):
    x_est[k + 1], p = kalman_filter(y[k], x_est[k], p, var_v, var_w)

# 結果の表示
t_ = np.arange(N)
fig, ax = plt.subplots(figsize=(12, 4))
sns.lineplot(t_, x, ax=ax, label="true state")
sns.lineplot(t_, y, color="gray", ax=ax, label="observation")
sns.lineplot(t_, x_est, color="red", ax=ax, label="estimated state")
ax.set_title("results")
ax.legend()
plt.show()