(cache)カルマンフィルタを統一する

この記事は，制御工学 Advent Calendar 2018の20日目の記事です．

はじめに

R.E.Kalmanがカルマンフィルタ(Kalman Filter: KF)を提案して以来，数多くのフィルタが誕生してきました．個々のフィルタは異なるバックグラウンドを持ち，それぞれに特徴があります．
この記事では，それらのフィルタを統一する視点，すなわち

カルマンフィルタの多くはガウシアンフィルタからの派生と見なせる

という主張について解説します．特に，拡張カルマンフィルタ(Extended KF: EKF)，Unscented カルマンフィルタ(UKF)を実際にガウシアンフィルタから導出します．
なお，この記事には実装コードはありません．代わりといっては何ですが，参考文献のページを詳しく書きました．ぜひ，ご活用ください．

カルマンフィルタの問題設定

まずは，カルマンフィルタが対象とする問題を設定しましょう．いま，離散時間非線形システム $\begin{aligned} x_{k + 1} & = f (x_{k}) + w_{k} \\ (1) & y_{k} & = h (x_{k}) + v_{k} \end{aligned}$ を考えます．簡単のため，状態 $x_{k}$ および観測 $y_{k}$ の次元は1次元とします．また， $w_{k}, v_{k}$ は雑音で，それぞれ $w_{k} \sim N (0, Q), v_{k} \sim N (0, R)$ とガウス分布に従うとします（ $Q, R$ は大文字ですが，共分散行列ではなく実数値の分散です）．

このとき，カルマンフィルタは次の問題に対し，一つの答えを与えます．

観測の集合 $Y_{k} = {y_{0}, y_{1}, \dots, y_{k}}$ が与えられているとする．このとき，それぞれ次の評価関数 $J$ を最小化する推定値 ${\hat{x}}_{k | k}, {\hat{x}}_{k + 1 | k}$ を見つけよ． $\begin{aligned} J & = E [‖ x_{k} - {\hat{x}}_{k | k} ‖^{2}] \\ J & = E [‖ x_{k + 1} - {\hat{x}}_{k + 1 | k} ‖^{2}] \end{aligned}$ ただし， ${\hat{x}}_{k | l}$ で時刻 $l$ の時点で予測した時刻 $k$ での状態の推定値を表すとする．

ガウシアンフィルタ(GF)

さて，さきほどの問題に対して，Ito, Xiong[1]はガウシアンフィルタ(Gaussian Fiter: GF)を提案しました．導出はしませんが，どのようなフィルタかについて概略を述べます．

PRML[2]で述べられている通り，この問題は状態 $x_{k}$ が潜在変数であるような隠れマルコフモデルになっています（式( $1$ )は $x_{k + 1}$ と $x_{k}$ の関係を記述しています）．さらに，雑音がガウス分布に従うことから，状態はガウス分布に従います．

このとき，「マルコフ性」と「ガウス分布」，この2つ性質だけをベースに構築したピュアなカルマンフィルタがガウシアンフィルタである，といえます．後述しますが，EKFやUKFでは，この2つの性質の他にも操作を加えながらフィルタを構築します．そのためEKFやUKFがもともと仮定しているこの2つの性質が見えにくくなっています．EKFやUKFから共通の構造を抽出したのがガウシアンフィルタである，ともいえるでしょう．

では，ガウシアンフィルタの式を述べます．この式は文献[3]を参考にしています．

$\begin{aligned} θ_{k | k - 1} := ({\hat{x}}_{k | k - 1}, P_{k | k - 1}) \\ θ_{k | k} := ({\hat{x}}_{k | k}, P_{k | k}) \\ Y_{k} := {y_{0}, y_{1}, \dots, y_{k}} \end{aligned}$ $\begin{aligned} p (x_{k} | Y_{k - 1}) = N (x_{k} | θ_{k | k - 1}) \\ p (x_{k} | Y_{k}) = N (x_{k} | θ_{k | k}) \end{aligned}$ [観測更新] $\begin{aligned} (GF1) & {\hat{y}}_{k | k - 1} = E [h (x_{k}) | θ_{k | k - 1}] \\ (GF2) & U_{k | k - 1} = C o v [x_{k}, h (x_{k}) | θ_{k | k - 1}] \\ (GF3) & V_{k | k - 1} = V [h (x_{k}) | θ_{k | k - 1}] + R \\ (GF4) & K_{k} = U_{k | k - 1} V_{k | k - 1}^{- 1} \\ (GF5) & {\hat{x}}_{k | k} = {\hat{x}}_{k | k - 1} + K_{k} (y_{k} - {\hat{y}}_{k | k - 1}) \\ (GF6) & P_{k | k} = P_{k | k - 1} - K_{k} U_{k | k - 1} \end{aligned}$ [時間更新] $\begin{aligned} (GF7) & {\hat{x}}_{k + 1 | k} = E [f (x_{k}) | θ_{k | k}] \\ (GF8) & P_{k + 1 | k} = V [f (x_{k}) | θ_{k | k}] + Q \end{aligned}$ ただし， $\begin{aligned} (2) & E [F (x) | θ] = \int_{R} F (x) N (x | θ) d x \\ (3) & V [F (x) | θ] = \int_{R} (F (x) - E [F (x) | θ])^{2} N (x | θ) d x \\ (4) & C o v [F_{1} (x), F_{2} (x) | θ] = \int_{R} (F_{1} (x) - E [F_{1} (x) | θ]) (F_{2} (x) - E [F_{2} (x) | θ]) N (x | θ) d x \end{aligned}$

繰り返しになりますが，すべてスカラー値です．
さきほどの問題に対する答えとなる式は，式(

GF5

)，(

GF7

)になります．それ以外は，この2式を計算するために必要な式です．

さて，ざっと式全体を見渡すと，どれも期待値や共分散の積分計算が必要だと気づきます．しかし，一般にこれらの積分は解析的に求められないため，計算するには何らかの操作を施さなければなりません． 実は，この積分計算をするために必要な操作の違いこそがEKFやUKFといった各種の非線形フィルタが現れる源なのです．

式(

2

)～(

4

)の積分計算は，いずれも共分散公式などを用いることで次の形の積分計算に帰着します．

\begin{aligned} (13) & I [F (x) | θ] = \int_{R} F (x) N (x | θ) d x \end{aligned}

そこで，これ以降，積分 $I [F (x) | θ]$ をどう計算するかに集中して考えていきます．

Gaussian Filter + Taylor展開 = Extended Kalman Filter

本来，拡張カルマンフィルタ(EKF)は，状態方程式( $1$ )を状態の推定値 ${\hat{x}}_{k | k}$ や ${\hat{x}}_{k | k - 1}$ のまわりで線形化し，線形カルマンフィルタを適用すると得られます．今回は，線形化した状態方程式からボトムアップにEKFを構築するのではなく，GFからトップダウンに構築してみます． EKFの具体的な式はAppendix.A EKF にあります．GFからトップダウンに計算して，Appendix.Aの式を導出することが目標です．

まず， $f (x_{k}), h (x_{k})$ を次のように推定値のまわりで線形化します． $\begin{aligned} f (x_{k}) \approx f ({\hat{x}}_{k | k}) + f^{'} ({\hat{x}}_{k | k}) (x_{k} - {\hat{x}}_{k | k}) \\ h (x_{k}) \approx h ({\hat{x}}_{k | k - 1}) + h^{'} ({\hat{x}}_{k | k - 1}) (x_{k} - {\hat{x}}_{k | k - 1}) \end{aligned}$ これをガウシアンフィルタの式に放り込んでいきます．この際，共分散の性質 $C o v [a + F_{1} (x), b F_{2} (x) | θ] = b C o v [F_{1} (x), F_{2} (x) | θ]$ を使って変数以外の定数項を処理します．
例えば，式( $GF2$ )に代入すると， $\begin{aligned} U_{k | k - 1} & \approx C o v [x_{k}, h ({\hat{x}}_{k | k - 1}) + h^{'} ({\hat{x}}_{k | k - 1}) (x_{k} - {\hat{x}}_{k | k - 1}) | θ_{k | k - 1}] \\ = C o v [x_{k}, h ({\hat{x}}_{k | k - 1}) - h^{'} ({\hat{x}}_{k | k - 1}) {\hat{x}}_{k | k - 1} + h^{'} ({\hat{x}}_{k | k - 1}) x_{k} | θ_{k | k - 1}] \\ = C o v [x_{k}, h^{'} ({\hat{x}}_{k | k - 1}) x_{k} | θ_{k | k - 1}] \\ = h^{'} ({\hat{x}}_{k | k - 1}) C o v [x_{k}, x_{k} | θ_{k | k - 1}] \\ = h^{'} ({\hat{x}}_{k | k - 1}) V [x_{k} | θ_{k | k - 1}] \\ = h^{'} ({\hat{x}}_{k | k - 1}) P_{k | k - 1} \end{aligned}$ となり，(EKF2)と一致します．同様にして他の共分散，分散も計算できます．
他にも，式( $GF6$ )は， $\begin{aligned} P_{k | k} & = P_{k | k - 1} - K_{k} U_{k | k - 1} \\ \approx P_{k | k - 1} - K_{k} h^{'} ({\hat{x}}_{k | k - 1}) P_{k | k - 1} \end{aligned}$ となり，(EKF6)と一致します．
他も同様にして，GFからEKFが導出できます．

Gaussian Filter + Gauss-Hermite求積 = Unscented Kalman Filter

次は，Unscented カルマンフィルタ(UKF)です． UKFは非線形関数 $f (x_{k})$ の近似よりもガウス分布の近似のほうが簡単という考えのもと，Monte Carlo法をより少ない点数に簡略化したものといえます．まず，その平均と分散が ${\hat{x}}_{k | k}$ と $P_{k | k}$ に一致するような点集合（シグマ点）を生成し，分布を近似します．次にシグマ点を関数 $f, h$ で飛ばすことで，非線形性をシグマ点で捉える，というのが基本的なアイデアです．詳細は文献[5]，[6]を御覧ください．また，UKFの具体的な式はAppendix.B UKF にあります．

閑話休題タイム．さきほどEKFをGFから導出しましたが，思ったより呆気なく感じませんでしたか．結局，線形化した式を代入するタイミングが後になっただけじゃんと感じた方もいると思います．ご安心ください．実はこの記事のハイライトはここからです．

さて，ここでもう一度，いま考えている積分 $I [F (x) | θ]$ を見てみましょう． $\begin{aligned} I [F (x) | θ] = \int_{R} F (x) N (x | θ) d x = \int_{R} F (x) \frac{1}{\sqrt{2 π P}} \exp (- \frac{(x - \hat{x})^{2}}{2 P}) d x \end{aligned}$ $t = (x - \hat{x}) / \sqrt{2 P} \Leftrightarrow x = \hat{x} + \sqrt{2 P} t$ と変数変換すると， $\begin{aligned} I [F (x) | θ] = \frac{1}{\sqrt{π}} \int_{R} F (\hat{x} + \sqrt{2 P} t) \exp (- t^{2}) d t \end{aligned}$ となります．

このように，ある関数 $F$ と $\exp (- t^{2})$ を区間 $[- \infty, \infty]$ で積分する場合にうってつけの数値計算手法があります．それがGauss-Hermite求積です．

Gauss-Hermite求積（より一般にはGauss型積分公式）には素晴らしい性質が知られています([7])．それは，m個の被積分関数の値で積分値を近似するような(m点公式といわれる)求積法のなかで，最高の近似精度を叩き出すのがこのGauss-Hermite求積なのです！ このことを踏まえると， $I [F (x) | θ]$ の計算にはGauss-Hermite求積がまさに適任といえます．

では， $I [F (x) | θ]$ にGauss-Hermite求積を適用してみましょう．すると，Hermite多項式の零点 $t_{i}$ での $F$ の値と，重み $w_{i}$ との重み付き線形和で近似できます([8])． $\begin{aligned} I [F (x) | θ] \approx \sum_{i = 1}^{m} w_{i} F (\hat{x} + \sqrt{2 P} t_{i}) \\ t_{i} : H_{m} (t) の零点 \\ w_{i} = \frac{2^{m - 1} m!}{[m H_{m - 1} (t_{i})]^{2}} \end{aligned}$

$m = 3$ の場合に計算してみましょう．
$H_{3} (t) = 8 t^{3} - 12 t$ より，零点は $(t_{1}, t_{2}, t_{3}) = (0, \sqrt{\frac{3}{2}}, - \sqrt{\frac{3}{2}})$ となります．また， $H_{2} (t) = 4 t^{2} - 2$ より，重みは $(w_{1}, w_{2}, w_{3}) = (\frac{2}{3}, \frac{1}{6}, \frac{1}{6})$ となります．これより，3点公式 $\begin{aligned} I [F (x) | θ] \approx \frac{2}{3} F (\hat{x}) + \frac{1}{6} F (\hat{x} + \sqrt{3 P}) + \frac{1}{6} F (\hat{x} - \sqrt{3 P}) \end{aligned}$ が得られました！

さっそく3点公式をガウシアンフィルタに使ってみます．
式( $GF7$ )に適用すると， $\begin{aligned} {\hat{x}}_{k + 1 | k} \approx \frac{2}{3} f ({\hat{x}}_{k | k}) + \frac{1}{6} f ({\hat{x}}_{k | k} + \sqrt{3 P_{k | k}}) + \frac{1}{6} f ({\hat{x}}_{k | k} - \sqrt{3 P_{k | k}}) \end{aligned}$ となります．これはAppendix.B UKF において， $(m, κ) = (1, 2)$ とおいた式(UKF7)と一致します！なんとHermite多項式からシグマ点が現れました．
同様に式( $GF1$ )にも適用してみましょう．この場合，少し修正が必要で， $\begin{aligned} {\hat{y}}_{k | k - 1} & = E [h (x_{k}) | θ_{k | k - 1}] \\ = E [h (f (x_{k - 1})) | θ_{k - 1 | k - 1}] \end{aligned}$ のように， $θ_{k | k - 1} \to θ_{k - 1 | k - 1}$ とずらす必要があります．これはシグマ点の生成が時間更新(UKF7)の直前にのみ行われることに起因します．この式に3点公式を使うと， $\begin{aligned} {\hat{y}}_{k | k - 1} & \approx \frac{2}{3} h (f ({\hat{x}}_{k - 1 | k - 1})) + \frac{1}{6} h (f ({\hat{x}}_{k - 1 | k - 1} + \sqrt{3 P_{k - 1 | k - 1}})) + \frac{1}{6} h (f ({\hat{x}}_{k - 1 | k - 1} - \sqrt{3 P_{k - 1 | k - 1}})) \\ = \frac{2}{3} h (f (X_{k - 1 | k - 1}^{0})) + \frac{1}{6} h (f (X_{k - 1 | k - 1}^{1})) + \frac{1}{6} h (f (X_{k - 1 | k - 1}^{2})) \\ = \frac{2}{3} h (X_{k | k - 1}^{0}) + \frac{1}{6} h (X_{k | k - 1}^{1}) + \frac{1}{6} h (X_{k | k - 1}^{2}) \end{aligned}$ となり，(UKF1)と一致します．ほかも同様にして3点公式を適用することでGFからUKFが導出できます．

[2018/12/22追記]
やや急ぎ足になっていたので，(UKF2)の導出を新たに追加します．
まず，共分散の定義から， $\begin{aligned} C o v [x_{k}, h (x_{k}) | θ_{k | k - 1}] = E [(x_{k} - E [x_{k} | θ_{k | k - 1}]) (h (x_{k}) - E [h (x_{k}) | θ_{k | k - 1}]) | θ_{k | k - 1}] \end{aligned}$ です．次に，内側の期待値の部分を推定値で書き換えます． $\begin{aligned} E [ & (x_{k} - E [x_{k} | θ_{k | k - 1}]) (h (x_{k}) - E [h (x_{k}) | θ_{k | k - 1}]) | θ_{k | k - 1}] \\ = E [ & (x_{k} - {\hat{x}}_{k | k - 1}) (h (x_{k}) - {\hat{y}}_{k | k - 1}) | θ_{k | k - 1}] \end{aligned}$

最後に，さきほどの式(UKF1)の導出と同様， $θ_{k | k - 1} \to θ_{k - 1 | k - 1}$ と修正します．すると，結局 $\begin{aligned} U_{k | k - 1} = E [(f (x_{k - 1}) - {\hat{x}}_{k | k - 1}) (h (f (x_{k - 1})) - {\hat{y}}_{k | k - 1}) | θ_{k - 1 | k - 1}] \end{aligned}$ となります．この式に3点公式を使うと， $f (x_{k - 1})$ の部分がシグマ点 $X_{k | k - 1}^{i}$ に置き換わり， $\begin{aligned} U_{k | k - 1} \approx \sum_{i = 0}^{2} w_{i} (X_{k | k - 1}^{i} - {\hat{x}}_{k | k - 1}) (h (X_{k | k - 1}^{i}) - {\hat{y}}_{k | k - 1}) \end{aligned}$ となります．これは式(UKF2)です．以上より，式( $GF2$ )から式(UKF2)を得ました．
[追記終わり]

結局何が嬉しいの？

以上， $I [F (x) | θ]$ の積分をどう計算するかによってEKF，UKFが現れることを見てきました．理論的にはスッキリしましたが，結局何が嬉しいのでしょうか．実はこのように整理することで次のような嬉しさがあります．

新しいフィルタの構成が容易になる

フィルタと積分計算が対応しているため，新しい積分計算の手法を考えれば，そこに新しいフィルタが生まれます．実際，文献[1]では， $F (x)$ を二次の項までTaylor展開し，ヤコビアンとヘシアンを中央差分近似することで，Central Difference Filter(CDF)なる新しいフィルタを提案しています．

各フィルタの性能を比較できる

積分計算の手法の性能によって各フィルタの性能は決まります．ここでいう性能とは，精度，効率，安定性の3つの指標です．実際，文献[4]では，EKF，UKF，CDFなどのフィルタたちを各指標ごとに順序付けし，フィルタ選択の指針を与えています．

粒子フィルタが無いんだけど？

今回の記事では，確率変数がガウス分布に従うことを仮定しました．一方，粒子フィルタは非ガウス分布の場合に有効な手法です．ですので，粒子フィルタはガウシアンフィルタでは統合できないフィルタの一つといえるでしょう．

まとめ

いかがでしたでしょうか．
本記事では，ガウシアンフィルタというカルマンフィルタの濃縮液のようなフィルタから， EKFやUKFといった異なるバックグラウンドをもつフィルタが導出できるということを確認しました．特にUKFの導出では，Hermite多項式の零点と統計的に定めたシグマ点の2つが驚きの対応をしました．カルマンフィルタの世界の奥の深さを改めて実感した次第です．

それでは，皆さんも良きカルマンライフを！

Table of Contents