カルマンフィルタの勉強

カルマンフィルタの気持ちをわかるために

仕事で教える必要が出てきたので、カルマンフィルタの勉強をしています。目標は SLAM (Simultaneous Localization And Mapping) を理解してチューニングしながら使うことです。

実は、カルマンフィルタがちゃんと理解出来れば、SLAM は80%ぐらいは理解したことになりますが、カルマンフィルタをちゃんと理解するのが結構難しい。

「何が分からないのか」と聴くと、「何をやっているのか分からない」という答えが返ってきます。 

カルマンフィルタが分からない理由は、いくつかあるような気がしています。

1. いきなりイノベーションとかカルマンゲインとか言われてもわけがわからない。
2. それぞれの変数が、確率変数なのか、観測なのか、真の値なのかわからない。
3. 「不確実性を伝搬する」という気持ちがわからない。
4. 行列の微分が分からない。
5. なまじ理解しなくてもとりあえず使えるので、理解する必要が無い。

だいたいこのぐらいだと思います。このうち４は univariate （１変数）の場合を考える事でだいぶ解消できます。また２に関してはノーテーションに注意しながら説明し、３については絵を描きながら説明すると良く分かるかなと思います。１に関しても２－４が解消出来れば分かるはずです。

 

問題は 5 です。理解するモチベーションが無いと、分かるものも分かりません。ですが、最初に言ったように、カルマンフィルタがちゃんと理解できると、SLAM が理解出来ます。ロボットに応用する場合、非常に有用です。なぜ有用かはおいおい書いていきます。

 

というわけで、行ってみましょう。今回は、行列が出てきません。

ロボット位置の「ぼんやりとした情報」 $\bar{x}$ と、観測 $y$

今やりたいのは、刻々と変化するロボットの位置を推定する事ですが、まずはカルマンフィルタのコアである「予測情報と観測情報の合成」について説明します。

 

今、有る時刻に、ロボットの位置に関する情報がある程度分かっているとします。しかし、はっきりとは分かっていません。

 

このはっきり分かっていない状態を

$\bar{x}$

という確率変数で表します。

 

この確率変数は、正規分布になっていて、平均が $\mu_x$, 分散が $\sigma_x^2$ になっています。$\bar{x}$ 上に棒を引くことで、「真の位置ではない」ということを強調してあります。ロボットの真の位置は、$x$ で表します。

 

この状態で、ロボットの真の位置$x$ に関して観測 $y$ を得たとします。しかし、$y$ にもノイズ $w_y$ が乗っていて、はっきりとはわかっていないとします。真の位置 $x$ に、ノイズ $w_y$ が乗っていますので、

 

$y = x + w_y$

 

です。$w_y$ も確率変数で、平均 $\mu_y=0$, 分散 $\sigma_y^2$ の正規分布に従っています。

別々の情報源を合成する

今の状態で、真の状態 $x$ に関して、$\bar{x}$ と $y$ の２つの情報を持っていることになります。

 

ここで、重要な事実です。$\bar{x}$ はもともと持っていた知識、$y$ は別に得られた観測ですので、$\bar{x}$ と$y$は別々の情報源という事になります。

 

すごく一般的な話になりますが、ある情報を知りたい（気になるお店の情報等）時に、一人に感想を聞くよりも、二人に聞いたほうが信頼性は高くなります。

 

同じ$x$から、２つの情報が得られているわけですから、２つを合成すれば更に信頼性の高い情報が得られるでしょう。

 

しかし、どちらをどの程度信頼したらよいでしょうか？観測 $y$ を信頼すれば $\bar{x}$ よりも $y$ に近い推定をしたほうがいいでしょう。こういう気持ちを、どのように数式に表しましょう？「間をとる」という気持ちは、内挿と呼ばれる方法で実現できます。つまり、この観測への信頼具合を $K (0 \leq K \leq 1)$ とすると、最終的な推定は

 

$ \bar{x}^\prime = (1-K) \bar{x} + K y$

 

と書けます。$K=1$ ならば $y$ のみを信頼し、$K=0$ ならば $\bar{x}$ のみを信頼します。

 

この信頼度合い、 $K$ が「カルマンゲイン」と呼ばれているものです。カルマンがなぜこの変数を「観測の予測に対する信頼度(The degree of observation belief to prediction)」と名付けず、自分の名前をつけたのか？それは、この $K$ がカルマンフィルタのコアになるアイデアだからです。カルマンのドヤ顔が、この変数に込められています。

 

上に書いたカルマンゲインは、我々が「こうしたい」という気持ちのみで勝手に決めていることに注意してください。何か定義があって、導出したわけではありません。これは推測ですが、カルマンがこの変数を導入したのは、この「どちらを信頼するかをパラメータで表そう（パラメータをどう決めるかは置いておいて）」という割といい加減なきもちからでしょう。

カルマンゲイン $K$ の決定方法

さて、いい加減に定義されたカルマンゲイン$K$ ですが、単純に、$K=0.5$ とすることもできます。また、$K$ を実験的に決めてうまくいく値で運用するというアプローチもあるでしょう。
 

しかし、もっといい決め方は無いでしょうか？一般的に、２つの情報を統合する場合、信頼度の高い情報をより信用するのが普通です。

 

$\bar{x}$ と $y$ のどこに信頼度に当たる情報が表されているでしょうか？

 

答えは、分散です。分散とはその名の通り、「不確実性」を表す最も単純な指標です。今 $\bar{x}$ の分散は $\sigma_x^2$, $y$ の分散は $\sigma_y^2$ でした。

 

どちらの値も、真値 $x$ に関しての不確実性を表しています。直観的に、分散の小さいほうが信頼できるでしょう。$K$ は「予測に対する観測の信頼度」ですから

 

$ \displaystyle K_{optimal} = \frac{\sigma_x^2}{\sigma_x^2 + \sigma_y^2} $

としてみましょう。もし $\sigma_y^2$ に対して $\sigma_x^2$ が大きい（＝観測のほうが信頼できる）ならば、$K$ は大きくなり、推定値は $y$ に近くなります。

誤差分散の最小二乗基準

$K$ の決め方は、まだあります。その一つが「最終的な予測の誤差の分散を最小化する」という基準です。つまり、情報を統合した後の「不確実性＝分散」が最小になるように $K$ を調整するという方法です。

そんなことがどうやったら可能なのでしょうか。ここは、ノートにちゃんと式を書いて考えながらやってください。

$ \bar{x}^\prime = (1-K) \bar{x} + K y$

これが カルマンゲイン $K$ の気持ちでした。つまり、$\bar{x}^\prime$ (情報を統合したあとの予測)は、$\bar{x}$ と $y$ との信頼度を元に決まる、というものです。今、真の値は $x$ ですので、予測値 $\bar{x}^\prime$ との誤差は、

$ \bar{x}^\prime –x = (1-K) \bar{x} + Ky -x$

です。今、$y=x+w_y$ だったことを思い出すと、

$ \bar{x}^\prime –x = (1-K) (\bar{x}-x) + Kw_y$

になります。この分散を計算するには、少し確率変数の知識が必要ですが、$\bar{x}$ と$w_y$ は独立ですので、右辺の分散は単に足し算になって、

$ \mathbb V[ \bar{x}^\prime –x ] = (1-K)^2 \mathbb V[\bar{x}-x] + K^2 \mathbb V[w_y]$

です。$\mathbb V[\cdot]$ は、分散を表します。$\bar{x}$ の分散は $\sigma_x^2$, $w_y$ の分散は $\sigma_y^2$ でした。左辺を ${\sigma_x^2}^\prime$と書くことにすると

${\sigma_x^2}^\prime = (1-K)^2 \sigma_x^2 + K^2 \sigma_y^2$

となります。今やりたいのは左辺の分散を最小にすることですので、右辺を $K$ で微分してゼロと置くと・・・

$ \displaystyle K_{optimal} = \frac{\sigma_x^2}{\sigma_x^2 + \sigma_y^2} $

先ほどと全く同じカルマンゲインが出てきました。これが、「最適カルマンゲイン」と呼ばれるものです。

最適カルマンゲインを使った予測値

この最適カルマンゲインを使った予測値は、今までの式を使って

$ \displaystyle \mathbb E[\bar{x}^\prime] = \frac{ \sigma_y^2 \mathbb E[\bar{x}] + \sigma_x^2 y}{\sigma_x^2 + \sigma_y^2}= \frac{ \sigma_y^2 \mu_x+ \sigma_x^2 y}{\sigma_x^2 + \sigma_y^2}$

となります。式を見ると、見事に $\bar{x}$ と $y$ の信頼度に応じて、値が調整されるようになっています。$\sigma_y^2$ が小さい場合は？またその逆の場合は？等を考えてみてください。

また、予測値の分散は、

$\displaystyle {\sigma_x^2}^\prime = \frac{\sigma_x^2\sigma_y^2}{\sigma_x^2 + \sigma_y^2} \leq \sigma_x^2 $

で、$\bar{x}$, $y$ のそれぞれの分散よりもより小さくなっています。これは、２つの情報を統合したことで、より信頼性の高い情報になった、ということを示しています。

SLAMの理解に向けて

カルマンフィルタのコアであるカルマンゲインの気持ちと、その導出方法（天下り的なものと、誤差の分散の最小化）を説明しました。

カルマンフィルタの基本的なアイデアはこれだけです。これに加え、普通は

1. プロセスノイズが加わる場合
2. 多次元の場合
3. 非線形の場合 (Extended Kalman Filter, Unscented Kalman Filter)

とカルマンフィルタの講座は進みますが、今回は SLAM を理解するのが目的です。SLAM の本質は

• 観測が相対的に与えられること
• 予測するものが、マップとロボットの位置の２つあること

です（僕はそう思ってます）。上で説明したカルマンフィルタに加わる要素はこれだけで、カルマンゲインもマップとロボットで２つ出てきますが、ほとんど同じ条件で最適カルマンゲインを求めることが可能です。