カルマンフィルタの基礎式を代数とベイズ定理から見る

はじめに

カルマンフィルタの歴史ももう長いことと思いますが，各所で各人なりにまとめられているあたり，なかなか理解の難しいものなんだなあと感じています．

例に漏れず自分もカルマンフィルタの原理がいまいちピンとこなかったので自分なりにまとめてみました．

カルマンフィルタ(Kalman Filter)はフィルタと名付けられてはいますが，使われ方としてはフィルタよりも現代制御のオブザーバ（Observer; 状態推定器）に近いような気がします．

観測した情報から，状態ベクトルが真値に最も近くなるように推定するものです．

概念は以下のところでわかりやすく説明されています．

bufonmake.blogspot.jp

qiita.com

ただ，数式として少し省略されているところもあったので，改めてまとめるに至りました．

基本的に，代数とベイズの定理のみから丁寧に導出していこうという感じです．

カルマンゲインの式の導出，予測値の算出，分散の更新は天下り的な部分も多いので，今回はこれらの導出に焦点を当ててみました．

統計的にみて，予測値と実プラントの誤差が最小となるように演繹的に求めていくとカルマンフィルタの式に行き着くあたり，統計学の応用先の代表例になっているだけのことはあるなあ，という感想です．

ここでは，状態空間と観測空間が同一の場合のみ扱います．

対象とする運動モデル

カルマンフィルタは，基本的に離散時間系で利用されることが多いと思いますので，ここでは連続時間系については言及しません．（単純に理解していないというのもある）

状態空間と観測空間が同一の場合には，状態方程式は以下の式で表されます．

\begin{align}
x(k) &= ax(k - 1)+bu(k) \\
y(k) &= x(k)
\end{align}

現実世界のプラントの挙動は，状態に対するノイズ $\varepsilon(k)$ と観測に対するノイズ $\delta(k)$ を含めると次のように書き換えられます．ここで，ノイズはガウス性（正規分布）を仮定します．

\begin{align}
x(k) &= ax(k - 1)+bu(k)+\varepsilon(k) \\
y(k) &= x(k)+\delta(k)
\end{align}

仮定に置いたとおり，（線形）カルマンフィルタはノイズがガウス性でない場合は考慮されておらず，実際に使用してみても真値と推定値が合わないことが多いです．

このような場合はパーティクルフィルタ（Particle Filter; 粒子フィルタ）や，アンサンブルカルマンフィルタ(Ensemble Kalman Filter)がよく合うように思います．

状態を予測する方法

運動モデルを利用した予測

このような系で，観測値 $y(k)$ から状態 $x(k)$ を得たい場合にはどうするでしょうか．

まずひとつが，オブザーバと似たような考え方で，運動モデルを立てる方法です．

\begin{align}
x_{M}(k) = ax_{M}(k - 1)+bu(k)
\end{align}

運動モデル $x_{M}(k)$ を立てる．
運動モデルに，実プラントに与えた入力 $u(k)$ と同じ入力を与える．
運動モデルの出力 $x_{M}(k)$ を状態の推定値 $\hat{x}_{Model}(k)$ として利用する．

この方法では，きちんとモデル化できていて初期値も合っているようであればよいのですが，現実にはノイズが入ってくるため時間を追うごとに誤差が蓄積していきます．
一方で上手くモデル化できていれば，短い時間の範囲では真値と推定値はよく合います．

観測による推定

もうひとつが，観測値から逆算する方法です．
かなり強引に言うと，状態の推定値 $\hat{x}_{Observe}(k)=y(k)$ とする方法です．
ただし， $y(k)$ は観測ノイズ $\delta(k)$ を含みますので，実際には，

\begin{align}
\hat{x}_{Observe}(k) = x(k)+\delta(k)
\end{align}

となり，観測ノイズ $\delta(k)$ を拾ってしまいます．

しかし動的な特性のない，長い時間一定の値が連続するような条件であれば真値と推定値はそれなりに合います．

極端な話では，一定の値にノイズが乗っているような場合には長い時間の平均を取れば，推定値は真値に近くなります．

テスタで電池の電圧を測るとき，繋いだ瞬間の急激な変化は表示の遅れなどが出て取りづらいですが，数秒経つと表示が安定して正しく測定できるのに少し似ているかもしれません．

予測と推定の組み合わせ

このような，急峻な変化に適した測定と，緩やかな変化に適した測定を上手く組み合わせるのがカルマンフィルタというわけです．

カルマンフィルタによって，動特性の異なる複数のセンサから真値を推定するセンサフュージョンなるものがあります．

構造的にカルマンフィルタはセンサフュージョンに適しているのではないでしょうか．

さて，これをどのように組み合わせればよいのか，という問題です．

カルマンフィルタ

最適カルマンゲインとは

単純に両者の平均を取った場合を考えます．

この場合，モデルによって推定された状態も観測値から推定された状態も等しい重みとなります．

\begin{align}
\hat{x}(k) = 0.5\hat{x}_{Model}(k)+0.5\hat{x}_{Observe}(k)
\end{align}

いま，モデルがかなり正確な一方，観測値では大きくノイズを拾ってしまうようなとき，両者の重み（重要度）を等しくするのはあまり懸命でないような感じがします．

感覚的には，モデルによって推定された状態の重みを大きく，観測値から推定された状態の重みを小さくとるのではないでしょうか．

\begin{align}
\hat{x}(k) =0.8\hat{x}_{Model}(k)+0.2\hat{x}_{Observe}(k)
\end{align}

係数の合計が $1$ であるとすると，重み係数 $K$ を利用して以下のように書き換えられます．

\begin{align}
\hat{x}(k) = (1-K)\hat{x}_{Model}(k)+K\hat{x}_{Observe}(k)
\end{align}

この $K$ を最適となるように決めることができれば，得られる情報から尤もらしさの高い状態が推定できそうです．

これが，カルマンフィルタの基本的な考え方です．

最適とは何か

具体的に尤もらしいとはどのようなことか，について考えます．
正規分布なノイズを仮定する場合，尤もらしさの高い状態とは，分散が小さい状態と言い換えることができます．

いま，推定値 $\hat{x}(k)$ と真値 $x(k)$ の差を取ってみます．いわゆる偏差（誤差）です．

\begin{align}
\hat{x}(k)-x(k) = (1-K)\hat{x}_{Model}(k)+K\hat{x}_{Observe}(k)-x(k)
\end{align}

両辺の分散は以下となります．

\begin{align}
V[\hat{x}(k)-x(k)] = V[(1-K)\hat{x}_{Model}(k)+K\hat{x}_{Observe}(k)-x(k)]
\end{align}

ここで $V[X]$ は $X$ の分散を表します．

分散は，変数 $X$ ， $Y$ が無相関の場合には線形性 $V[X+Y]=V[X]+V[Y]$ が成り立つので右辺は，

\begin{align}
V[\hat{x}(k)-x(k)] = V[(1-K)\hat{x}_{Model}(k)]+V[K\hat{x}_{Observe}(k)]+V[x(k)]
\end{align}

となります．（たしか分散は絶対値を取ったはず……）

また，定数 $p$ を含む場合の $V[pX]=p^2V[X]$ より，

\begin{align}
V[\hat{x}(k)-x(k)] = (1-K)^2V[\hat{x}_{Model}(k)]+K^2V[\hat{x}_{Observe}(k)]+V[x(k)]
\end{align}

が成り立ちます．

この分散が最小となるときのゲインが，最適なゲインと呼べそうです．

分散を最小化するゲインの導出

この左辺の分散が最小となるような重み係数 $K$ を求めたいので，両辺を $K$ で微分し左辺を $0$ とします．

\begin{align}
\frac{\partial V[\hat{x}(k)-x(k)]}{\partial K} &= -2(1-K)V[\hat{x}_{Model}(k)]+2KV[\hat{x}_{Observe}(k)] \\
0 &= (K - 1)V[\hat{x}_{Model}(k)]+KV[\hat{x}_{Observe}(k)]
\end{align}

$K$ でまとめると，

\begin{align}
0 = (V[\hat{x}_{Model}(k)]+V[\hat{x}_{Observe}(k)])K-V[\hat{x}_{Model}(k)]
\end{align}

$K$ について解くと，

\begin{align}
K(k) = \frac{V[\hat{x}_{Model}(k)]}{V[\hat{x}_{Model}(k)]+V[\hat{x}_{Observe}(k)]}
\end{align}

となり，最適な重み係数 $K$ が求まります．

これらの分散を記号 $\sigma$ を使って書き直したものがよく見る以下のカルマンゲインの式です．

カルマンゲイン

$\begin{align}K(k) = \frac{\sigma_{\hat{x}}^2(k)}{\sigma_{\hat{x}}^2(k)+\sigma_{z}^2(k)}\end{align}$

これでカルマンゲインが求まったので，それらのパラメータである分散 $\sigma_{\hat{x}}^2(k)$ と $\sigma_{z}^2(k)$ を求めます．

カルマンゲインのパラメータ導出

入力による分散

分散 $\sigma_{\hat{x}}^2(k)$ は $V[\hat{x}_{Model}(k)]$ だったので，

$\begin{align}\sigma_{\hat{x}}^2(k) = V[\hat{x}_{Model}(k)]\end{align}$

$V[\hat{x}_{Model}(k)]=V[x_{M}(k)]$ であるので，

$\begin{align}\sigma_{\hat{x}}^2(k) = V[x_{M}(k)] \end{align}$

$x_{M}(k)$ を代入して，

$\begin{align}\sigma_{\hat{x}}^2(k) = V[ax_{M}(k-1)+bu(k)] \end{align}$

$\begin{align}\sigma_{\hat{x}}^2(k) = a^2V[x_{M}(k-1)]+b^2V[u(k)] \end{align}$

ここで， $\sigma_{\hat{x}}^2(k) = V[x_{M}(k)]$ より， $\sigma_{\hat{x}}^2(k-1) = V[x_{M}(k-1)]$ となり，

$\begin{align}\sigma_{\hat{x}}^2(k) = a^2\sigma_{\hat{x}}^2(k-1)+b^2V[u(k)] \end{align}$

これでパラメータ $\sigma_{\hat{x}}^2(k)$ が求められました．

ここで， $V[u(k)]$ は操作量（入力）の分散で，設計すべきパラメータです．

操作量に関する分散が既知であればそれを使えばいいですし，未知なら適当な値を入れてしまいます．

$V[u(k)]$ を $\sigma_{x}$ と書く場合もあるようで，その場合には次のように書き換えられます．

$\begin{align} \sigma_{\hat{x}}^2(k) = a^2\sigma_{\hat{x}}^2(k-1)+b^2\sigma_{x}^2 \end{align}$

このパラメータ（分散）は，ある初期値 $\sigma_{\hat{x}}^2(0)$ から時々刻々と更新されていきます．

プログラムなんかで実装する場合にはグローバルもしくはstatic変数でこの値を保持する必要があります．

観測による分散

分散 $\sigma_{z}^2(k)$ は観測の分散で，これも設計パラメータです．

ここにはセンサの分散などを入れればよいです．たぶん．

これを時不変として， $\sigma_{z}^2$ と扱うことが多いように思います．

大抵の場合，設計パラメータ $\sigma_{x}^2$ と $\sigma_{z}^2$ は試行錯誤的に決定することが多いのではないかと思います．

これで，カルマンゲインのパラメータが全て求まりました．

カルマンゲインについてまとめると，以下のようになります．

$\begin{align} K(k) = \frac{\sigma_{\hat{x}}^2(k)}{\sigma_{\hat{x}}^2(k)+\sigma_{z}^2} \end{align}$

$\begin{align} \because \sigma_{\hat{x}}^2(k) = a^2\sigma_{\hat{x}}^2(k-1)+b^2\sigma_{x}^2 \end{align}$

$\begin{align} \sigma_{\hat{x}}^2(0) = 0, \sigma_{x}^2 = const, \sigma_{z}^2 = const \end{align}$

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

さて，状態の推定ですが，以下の式を少し変形します．

\begin{align}
\hat{x}(k) &= (1-K)\hat{x}_{Model}(k)+K\hat{x}_{Observe}(k) \\
&= \hat{x}_{Model}(k)+[\hat{x}_{Observe}(k)-\hat{x}_{Model}(k)]K
\end{align}

ここで， $\hat{x}_{Model}(k) = x_{M}(k)$ ，および $\hat{x}_{Observe}(k) = y(k)$ だったので，

\begin{align}
\hat{x}(k) = x_{M}(k)+[y(k)-x_{M}(k)]K
\end{align}

ここに先ほど求めた $K$ を代入することで，現在の時刻における尤もらしい状態の推定値 $\hat{x}(k)$ を得ることができました．

分散の更新

次にカルマンフィルタの肝要な部分のもうひとつである，分散の更新について考えます．

カルマンフィルタでは以下のような流れでサンプリング周期ごとに状態の推定と分散の更新を行います．

\begin{align}
分散の&事前推定 \\
& \Downarrow \\
カルマンフィル&タによる状態予測 \\
& \Downarrow \\
分散の&事後推定 \\
\end{align}

事前推定

カルマンゲイン $K$ を計算するために， $\sigma_{\hat{x}}^2$ と $\sigma_{z}^2$ の2つの分散を使用しました．

$\sigma_{z}^2$ は設計パラメータとして与えるべきパラメータで，もう一方の $\sigma_{\hat{x}}^2$ は以下の式で表現されました．

$\begin{align} \sigma_{\hat{x}}^2(k) = a^2\sigma_{\hat{x}}^2(k-1)+b^2\sigma_{x}^2 \end{align}$

このとき， $\sigma_{x}^2$ も設計パラメータで， $a$ ， $b$ はプラントモデルの定数でした．

上の式をよく見ると，次の時刻での分散 $\sigma_{\hat{x}}^2(k)$ は，初期値 $\sigma_{\hat{x}}^2(0)$ のみによって決まってしまうことがわかるかと思います．

せっかくモデルを利用した予測値と観測値からカルマンゲインを計算して，尤もらしさの高い，言い換えると分散の小さいはずの推定値 $\hat{x}(k)$ を手に入れたにも関わらず，それが反映されていないため次の時刻の分散はどんどん大きくなってしまうのです．

これは単純に，右辺第2項で時刻ごとに $b^2\sigma_{x}^2$ を足し続けることからも想像できるかと思います．

上の式はあくまでカルマンゲインが固定の場合です．

実際にはカルマンゲインは時々刻々と最適な値に更新されるため，その時々における最適な状態推定値 $\hat{x}(k)$ を利用して，次の時刻での分散を更新することで推定精度を向上できそうです．

事後推定

最終的に求められた状態の推定値は，以下の通りでした．

\begin{align}
\hat{x}(k) = (1-K)\hat{x}_{Model}(k)+K\hat{x}_{Observe}(k)
\end{align}

ここで，カルマンゲイン $K$ で括った $\hat{x}(k) = x_{M}(k)+[y(k)-x_{M}(k)]K$ の式を使うと， $x_{M}$ で括られていないため各項の無相関が成り立ちません．
このため，面倒なので $K$ で括られていない方の式を使っています．

この分散を取ると，

$\begin{align}\sigma_\hat{x}(k) = (1-K)^{2}\sigma_{\hat{x}}^2(k)+K^{2}\sigma_{z}^2 \end{align}$

カルマンゲインの2乗項 $K^{2}$ で括ると，

$\begin{align}V[\hat{x}(k)] = (1-2K)\sigma_{\hat{x}}^2(k)+K^{2}(\sigma_{\hat{x}}^2(k)+\sigma_{z}^2) \end{align}$

ここで， $K[\sigma_{\hat{x}}^2(k)+\sigma_{z}^2(k)] = \sigma_{\hat{x}}^2(k)$ と変形できるのでこれを代入すると，

$\begin{align} V[\hat{x}(k)] = (1-2K)\sigma_{\hat{x}}^2(k)+K\sigma_{\hat{x}}^2(k) \end{align}$

$\begin{align} V[\hat{x}(k)] = (1-K)\sigma_{\hat{x}}^2(k) \end{align}$

となり，カルマンゲイン導出後の予測値の分散を求めることができました．

この分散（事後分布）を次のステップで使用することで，現在のステップよりも分散が小さい条件で推定を行うことが出来るようになります．

上の分散の式に $K$ を代入すると，

$\begin{align}V[\hat{x}(k)] = \frac{\sigma_{\hat{x}}^2(k) \sigma_{z}^2}{\sigma_{\hat{x}}^2(k)+\sigma_{z}^2} \leq \sigma_{\hat{x}}^2(k) \end{align}$

となり，数式から見ても次ステップの分散は現在の分散よりも小さくなることがわかります．

このあたりのことは参考にさせていただいたページに詳しく記載されています．

このような感じで，天下りのない仮定の下でカルマンフィルタの式を導出することができました．

対象が高次元の場合

今回は対象が1次元で，状態空間と観測空間が同一の場合のみについて言及しましたが，
対象が多次元（ベクトルと行列）で，状態空間と観測空間が異なる場合にも同じアプローチで導出できます．

変わるところ

対象が多次元の場合にはシステム全体が行列表記になりますが，基本的には以下の部分を抑えておけば1次元の場合と考え方は殆ど変わりません．

太字で書かれているのはベクトルか行列，細字はスカラ
行列の積は左右で交換できない
ベクトル，行列の2乗は $A^{T}A$ のように転置して掛ける
1次元での割り算は多次元では逆行列になる
数値の1は多くの場合，単位行列になる

多次元でのカルマンフィルタ

状態空間と観測空間が異なる場合には観測方程式 $y(k)$ に新しく $c$ が入って，

\begin{align}
x(k) &= ax(k - 1)+bu(k) \\
y(k) &= cx(k)
\end{align}

であるので，予測の式が以下のように書き換わります．

\begin{align}
\hat{x}(k) = (1-Kc)\hat{x}_{Model}(k)+K\hat{x}_{Observe}(k)
\end{align}

これはつまり，

\begin{align}
\hat{x}(k) = (1-Kc)x(k)+Kcx(k)
\end{align}

であり，カルマンゲイン $K$ が新たに $Kc$ に変わっただけと見ることができます．

状態を予測するために，状態同士で演算する必要があるため，右辺第２項の係数に右辺第１項の係数を合わせた形です．

$Kc$ を $L$ に置き換えると（全く必要ないが），

\begin{align}
\hat{x}(k) = (1-L)x(k)+Lx(k)
\end{align}

となり，これはカルマンフィルタの式そのものです．

おわりに

このような感じで，式を演繹的に追っていくことで，なんとなくカルマンフィルタでやろうとしていることが理解できた気がします．

再掲しますが，以下のページが理解の大きな助けとなりました．