はじめに
カルマンフィルタを使えるようになりたいと思っているのですが、初っ端から躓いたので、勉強しながら、そのその過程を少し記録として残せればと思います。
確率統計を殆ど勉強してこなかったので、多くの書籍等で自明の事項として扱っている部分についていけず、また、一般化された数式がプログラマにとって逆に分かり難く、シンプルな例で具体的な計算が欲しかったこともあり、私なりの解釈で本当に初歩から考えてみたものです。
いろいろ誤りもあるかもしれませんが、同じレベルの人の勉強の助けになれば幸いです。
なお、内容はこちらを大いに参考にさせていただいております。
カルマンフィルタとは
カルマンフィルタの基本はそれぞれ誤差を持った２つの値として「過去の事象から予測した値」と「新しく観測した値」の２つの値から、最も尤もらしい現在の推定値を更新していくものと理解しています。
本記事では、単純に「２つの確率変数を合成する」という部分に絞って、しかも互いに相関がない上に正規分布というシンプルな条件に絞って考えてみました。
1つの値の場合の最尤推定
まず、２つの値の合成に入る前に、1つの確率事象に着目して、最尤推定の考え方を確認しておきます。ここでも簡単のため、誤差は必ず正規分布に従うと仮定して考えてみます。つまり本来の値を$ \bar{x} $ としたときに、予測値や観測値が $ \hat{x} $ となる確率が次の正規分布の式に従って誤差を伴って現れると仮定します。
f(\hat{x})=\frac{1}{\sqrt{2\pi\sigma^2}}\exp{\biggl(-\frac{(\hat{x}-\bar{x})^2}{2\sigma^2} \biggr) } \tag{1}
この式は、本来の値に対して、予測値や観測値がどこに現れる確率が高いかの確率分布を示したものです。
しかし、実際に行いたいのはその逆で、得られた予測値や観測値から、本当の値がどこにあると考えるのが最も尤もらしいかを推定したいわけです。この観測可能な結果から、元となる本来の値を推定していくのが最尤推定です。
さっそく視点を入れ替えて見てみると、観測誤差が分散 $ {\sigma_1}^2 $ に従うことが既知である観測系において、ある一つの観測結果として $ \hat{x}_1 $ が得られたとき、ある値 $ x $ が本来の値 $ x_1 $ であると期待できる確率、すなわち尤度の分布は、先ほどの式(1)から
F(x)=\frac{1}{\sqrt{2\pi\sigma^2}}\exp{\biggl(-\frac{(\hat{x}_1 - x)^2}{2\sigma^2} \biggr) } \tag{2}
となります。変数の場所が入れ替わりますが、もともと正規分布は左右対称なのでそのまま同じ $ \sigma^2 $ での分布となります。
この場合、もちろん、最も尤もらしい $ x $ は $ x = \hat{x}_1 $ の時となります。
ただし、これは誤差に正規分布を仮定しているからであり、例えば実際の値より少し大きな値側に偏った確率で観測される観測系であれば、観測結果から実際の値を推定する場合には、尤度関数は観測値より小さな値に偏った尤度を持ちます。正規分布だと気づかずに読み進めてしまいかねませんが、視点を入れ替えて確率事象を見ているのが最尤推定の重要な点かと思います。
（確率論で言えば、事前確率か事後確率の違いであり、ベイズ統計などを勉強すればより深く理解できるのかもしれません。）
2つの値の場合の最尤推定
ここで、互いに誤差について相関のない予測値 $ \hat{x_1} $ と観測値 $ \hat{x_2} $ が得られた場合を考えてみます。
２つのそれぞれ分散値 $ {\sigma_1}^2 $ と $ {\sigma_2}^2 $ に従った誤差を含んで得られることが既知だったと仮定します。
この場合、$ x $ が本当の値である尤度を示す関数は、その $ x $ のときに、２つの結果が同時に得られる確率なので、単純に乗算することで下記の式から計算できるはずです。
\begin{align}
F(x)&=\frac{1}{\sqrt{2\pi{\sigma_1}^2}}\exp{\biggl(-\frac{(\hat{x}_1 - x)^2}{2{\sigma_1}^2} \biggr) } \times \frac{1}{\sqrt{2\pi{\sigma_2}^2}}\exp{\biggl(-\frac{(\hat{x}_2 - x)^2}{2{\sigma_2}^2} \biggr) } \\
&= \frac{1}{2 \pi \sigma_1 \sigma_2 }\exp{\biggl(-\frac{(\hat{x}_1 - x)^2}{2{\sigma_1}^2} -\frac{(\hat{x}_2 - x)^2}{2{\sigma_2}^2} \biggr) }
\end{align} \tag{3}
のようになります。
尤度が最大になる $ x $ の値を求めたいので $ \exp $ の中が最大になる場合を求めればよく、
$ \exp $ の中を取り出すと
\begin{align}
G(x) &= -\frac{(\hat{x}_1 - x)^2}{2{\sigma_1}^2} - \frac{(\hat{x}_2 - x)^2}{2{\sigma_2}^2} \\
&= - \frac{{\sigma_2}^2(\hat{x}_1 - x)^2 + {\sigma_1}^2(\hat{x}_2 - x)^2}{2{\sigma_1}^2 {\sigma_2}^2} \\
&= -\frac{({\sigma_1}^2 + {\sigma_2}^2)x^2 - 2({\sigma_2}^2 \hat{x}_1 + {\sigma_1}^2 \hat{x}_2)x + \hat{x}_1 \hat{x}_2}{2 {\sigma_1}^2 {\sigma_2}^2}
\end{align} \tag{4}
となり、この最大値を求めるために微分値が0になるように解くと(平方完成でもよいかもしれません)
\begin{align}
G'(x) &= 0 \\
\frac{2(\sigma_1^2 + \sigma_2^2)x - 2({\sigma_2}^2 \hat{x}_1 + {\sigma_1}^2 \hat{x}_2)}{2 \sigma_1^2 \sigma_2^2} &= 0 \\
2(\sigma_1^2 + \sigma_2^2)x &= 2({\sigma_2}^2 \hat{x}_1 + {\sigma_1}^2 \hat{x}_2) \\
x &= \frac{{\sigma_2}^2}{{\sigma_1}^2+{\sigma_2}^2}\hat{x}_1 + \frac{{\sigma_1}^2}{{\sigma_1}^2+{\sigma_2}^2}\hat{x}_2
\end{align} \tag{5}
という比較的見通しのよい式になります。
つまり2つの値をそれぞれの分散の大きさに応じて線形補間するような計算式が得られます。
カルマンゲイン
ここまでは単なる２つの誤差を含んだ確率的な事象の合成でした。カルマンフィルタではこの考え方を、漸化式に組み込んで時系列の事象に対してフィルタを構成していくものです。
ですが、今回の記事ではそこまで踏み込めておらず、もう少し、この合成部分のみにフォーカスして考えてみます。
ここで、式(5)を整理するために
K_{gain} = {\sigma_1}^2 / ({\sigma_1}^2 + {\sigma_2}^2) \tag{6}
のようにおき、最尤値を新しい合成値として $ \hat{x}_{update} $ と定義すると、式(5)は、
\hat{x}_{update} = \hat{x}_1 + K_{gain}(\hat{x}_2 - \hat{x}_1) \tag{7}
のようにおき直せます。
前回の予測値に対して、新しい観測値を使って、最も尤もらしい次の $ \hat{x_{update}} $ を求める式になります。このときの $ K_{gain} $ をカルマンゲインと呼ぶようです。
ここでさらに得られた $ \hat{x_{update}} $ も当然、誤差を含んでいます。
結論から先に書くと新しい誤差の分散 $ {\sigma_{update}}^2 $ は
{\sigma_{update}}^2 = (1 - K_{gain}) {\sigma_1}^2 \tag{8}
となるようです。
ここで重要なのは、 $ K_{gain} $ は必ず正の値なので、$ {\sigma_{update}}^2 $ は $ {\sigma_1}^2 $ よりも小さくなります。 さらに $ {\sigma_1}^2 $ と $ {\sigma_2}^2 $ は対称なので、 $ {\sigma_{update}}^2 $ は $ {\sigma_2}^2 $ よりも小さくなることが保証されます。
つまり、 $ {\sigma_1}^2 $ と $ {\sigma_2}^2 $ が事前に知られていれば、最適なカルマンゲインを選ぶことで、必ず合成前の値より誤差が小さくなった更新値が得られるということを意味すると考えます。
合成値の分散の求め方
この式(8)がなぜこうなるのかについてみていきます(本記事を書いた目的でもあります)。
式(8) の $ K_{gain} $ を展開すると、
\begin{align}
{\sigma_{update}}^2 &= (1 - ({\sigma_1}^2 / ({\sigma_1}^2 + {\sigma_2}^2))) {\sigma_1}^2 \\
&= \frac{{\sigma_1}^2 {\sigma_2}^2}{{\sigma_1}^2+{\sigma_2}^2}
\end{align} \tag{9}
となります。
もとの形からこの式が求まればよいわけです。
まず一般に、確率変数 $ X $ の分散を $ V[X] $ と表し、確率変数 $ X $ と $ Y $ のに対する共分散を $ Cov(X, Y) $ と表すとすると、
V[X + Y] = V[X] + V[Y] + 2Cov(X, Y) \tag{10}
が成り立ちます。
（ここは、確率変数の加法性などで調べれば出てくると思います）
$ X $ と $ Y $ に相関がなければ $ Cov(X, Y) = 0 $ であるので、さらに $ a $ と $ b $ を定数とすると
V[aX + bY] = a^2V[X] + b^2V[Y] \tag{11}
が成り立ちます。
ここで式(11)の形に今回求める分散 $ {\sigma_{update}}^2 $ をあてはめようとすると
\begin{align}
V[X] &= {\sigma_1}^2 \\
V[Y] &= {\sigma_2}^2
\end{align} \tag{12}
とおいて、さらにカルマンゲインに展開する前の式(5)から
\begin{align}
a &= \frac{{\sigma_1}^2}{{\sigma_1}^2+{\sigma_2}^2} \\
b &= \frac{{\sigma_2}^2}{{\sigma_1}^2+{\sigma_2}^2}
\end{align} \tag{13}
として 式(11) を適用すると
\begin{align}
{\sigma_{update}}^2 &= \biggl(\frac{{\sigma_1}^2}{{\sigma_1}^2+{\sigma_2}^2}\biggr)^2 \sigma_1 + \biggl(\frac{{\sigma_2}^2}{{\sigma_1}^2+{\sigma_2}^2}\biggr)^2 \sigma_2 \\
&= \frac{{\sigma_1}^4{\sigma_2}^2 + {\sigma_1}^2{\sigma_1}^4}{({\sigma_1}^2+{\sigma_2}^2)^2} \\
&= \frac{{\sigma_1}^2{\sigma_2}^2({\sigma_1}^2 + {\sigma_2}^2)}{({\sigma_1}^2+{\sigma_2}^2)^2} \\
&= \frac{{\sigma_1}^2{\sigma_2}^2}{{\sigma_1}^2+{\sigma_2}^2}
\end{align} \tag{14}
となり、無事に 式(9) と同じになりました。
Web上のプログラムなどを読んでいると式(7)や式(8)に相当する箇所だけがプログラムされていますが、それだけ見るとなぜそうなっているのか理解できなかったのですが、ようやく糸口が掴めました。
カルマンフィルタは導出過程で複雑な式になっても、最終結果がシンプルになるケースが多いようで、最終結果のプログラムだけ読んでも理解が難しいように思います。
補足
今回は理解を簡単にするために非常に限定されたシンプルな形で計算してみました。
実際には誤差が無相関ということはあまり無いので、途中出てきた共分散を含んだ式で扱うようです。
また、実際は状態変数は１つということは無く、複数の確率変数を含んだベクトル変数で扱うようです。
この場合、共分散行列として変数間の相関を扱うことで一般化できるようです。
また今回、分散を既知と扱っていますが、これも実際には、実データの分析などから推定したり、個別に計測したりといったことが事前作業として必要になるようです。
#おわりに
まだ勉強をはじめたばかりで、一般化した式を扱えるところに至っていません。
一方で、勉強する過程での式が本やWebを読んだだけでは理解できなかったので、自分の理解できるレベルまで分解してみました。
ブログが適当だったのかも知れませんが、数式の記述が大変なのでQiitaに初挑戦して見ました。
しかしながら一部、文中の数式表記で \hat などの範囲を正しく記述すると、正しく表示されないなどの不具合もあり、一部おかしな表記にあっている箇所もあります。
この手の数学的作業に不慣れなので、間違っていたり、変数の置き方や修飾が不適当だったりと色々あるかと思います。ご指摘いただければ幸いです。
[追記]
今回の考察を実際にPythonプログラムで試したものを次回記事として投稿いたしました。