離散ベイズフィルタ(カルマンフィルタ)

12月から GMapping の勉強をするベくコードを読んでます.で,SLAMのバイブルこと確率ロボティックスを読みながら理解していってるんですが,やっぱりちょっとサンプルコードを書いたほうが理解が深まるかと思い,確率ロボティックスを部分的に Python で実装して勉強してます.一応 OGM ベースの Fast Slam まで動くようになったので,今日から順次ブログを書いていきたいと思います.コードは下記の Git においてます.

github.com
このエントリで触れるのは一番最初の離散ベイズフィルタの内容で,上記リポジトリの "prob_robotics/tests/hist_filter_example.py" のコードになります.

1.離散ベイズフィルタ

で,「離散ベイズフィルタっていったい何よ?」って話なんですが,ロボットとか車載の検知アルゴリズム作っている人にとっては「離散ベイズフィルタ」よりも「カルマンフィルタ」のほうがより身近だと思います.確率ロボティックスを読んで初めて知ったんですが,カルマンフィルタもベイズフィルタの一種なんですね.

で,どういうときにこのフィルタを使うか?という話なんですが,下記はちょっと自分解釈になりますが...

1.直接観測できない状態を推定したい.
2.状態を推定する方法が複数ある.
3.一つの推定方法だけで状態推定するのではなく,複数の方法を用いて状態推定をして一つの方法の時よりも推定結果をベターにしたい.

というときかなと思います.文章で書くと「なんやそれ?」って話なんですが,工学的にこういう状況って結構あると思います.

1.カーナビ

カーナビが適切に自車位置を把握していないと経路案内ができないですが,車両の位置を知る方法は複数あります.
方法1:タイヤの回転数とハンドルの舵角積分して,位置を求める.
方法2:GPSを使う.

2.部屋の温度調整

エアコンを使って部屋の温度を設定温度まで温めたいとします.この時も部屋の温度を把握してないと設定温度まで温度を上げられないですが,部屋の温度を知る方法は複数あります.
方法1:これから部屋に送る温風(熱量)を計算して,どの程度温度が上がるか計算する.
方法2:エアコンについている温度計を使って温度を測る.

3.ロボットの自己位置推定

ロボットを所定の位置まで動かすために,今いる位置を求める問題.
方法1:タイヤの回転数とヨーレートを積分して,位置を求める.
方法2:LIDARやカメラのセンシング結果を用いて,位置を求める.

上記の3例を考えてみるとわかると思うのですが,どの方法を使っても「完璧!」には求めたいものが求まりません.例えばカーナビのケースですと,タイヤの回転数とハンドル舵角積分で位置を出しても,走行距離が長くなるにしたがって誤差が蓄積してしまいますし,GPSも電波状況によっては 5~10m オーダーの誤差が乗ってしまいます.で,こんな状況で離散ベイズフィルタを使えば何がうれしいのかというと,方法1で計算した値と,方法2で計算した値を少なくとも確率的には最適になるようにフュージョンしてくれます.

2.アルゴリズム

で,実際のベイズフィルタアルゴリズムとそのイメージです.ベイズフィルタでは,大きく”予測ステップ”と”修正ステップ”があります.それぞのステップでやっていることのイメージを図示してみました.

f:id:rkoichi2001:20190113035411p:plain
離散ベイズフィルタ アルゴリズムとイメージ

予測ステップでやっていること

イメージを簡単にするために,カーナビの例を考えてみます.この例では,予測ステップにタイヤの回転数を使います.

t-1 から t まで時間が進んで,この間にタイヤが一回転(一周1.5m)したとすると,ベイズフィルタの入力である ut は 1.5 になります.この時,誤差が全くなければ xt = xt-1 + 1.5 になると思いますが,誤差が全くないということはありえないので,xt は xt-1 + 1.5 を中心にひろがりをもつ確率分布になります.

f:id:rkoichi2001:20190113041126j:plain
誤差分布

上記の誤差分布をみてわかる通り,xt = xt-1 + u は xt-1 + 1.5 にピークを持つ確率分布になります.アルゴリズムに書かれている遷移確率 p(xt | ut, xt-1) はこれを表現しています.この遷移確率は 「xt-1にいるときに,utという入力を受けて,xtという場所に移動する確率」を表しているので,最終的に xt にいる確率を求めるためにはそもそも xt-1 にいた確率 bel(xt-1) をかけてあげないといけません.これが予測ステップの積分の対象となっている式です.で,次に,遷移確率に分布があるために,xの取りうる値すべてから xt に遷移する確率を求めてあげる必要があります.この積分を実行すると,p(xt | ut, xt-1) -> p(xt | ut) となります.

f:id:rkoichi2001:20190113042619j:plain
予測ステップでやっていること

アルゴリズムに "for all xt do" という記載があるように,このステップによって xt の分布がわかります.

修正ステップでやっていること

カーナビの例なので,計測ステップで GPS 信号を使います. GPS からはダイレクトに車両の位置がわかります.ただし,GPS から求まる位置も誤差を含んでいるため,この値も確率分布になります.このステップでは,予測ステップと修正ステップで得られる二つの分布を使って「答え合わせ」をします.どのようにするかというと,二つの分布を掛け算してその結果を最終分布として用います.ちょうど下記のようなイメージです.

f:id:rkoichi2001:20190113043813j:plain
修正ステップの確率分布統合イメージ

上記のステップを経て,方法1と方法2の結果を統合することができました.結局のところ,これがベイズフィルタ・カルマンフィルタのやっていることです.

3.思考実験

ということで,実際にサンプルコードを書いてみました.状況設定ちょっと変わってしまうので恐縮ですが,下記のような仮想的なケースを考えてみました.
1.歩行者Aが自由の女神像から直線距離80mのところにいます.
2.歩行者Aは自分の歩幅を数えることで自分の位置を計算します.
3.歩行者Aは自分の歩幅を大体わかっていますが,1m 歩く毎に 1σ 0.3m くらいの誤差が乗ります.
4.歩行者Aは受信機を持っていて,この受信機は10m歩くたびに自由の女神像までの距離を教えてくれます.が,こちらの受信機も1σ 2m くらいの誤差が乗ります.

f:id:rkoichi2001:20190113045347j:plain
思考実験のイメージ(絵心がないのはご愛敬...)

上記の設定で,「1.歩行者が歩幅を数えて自由の女神像までの距離を求めた時」「2.歩行者が両方の値を使って自由の女神像までの距離を求めた時」のうち,2の結果がよくなることを見たいと思います.

結果図

f:id:rkoichi2001:20190113051942p:plain
オドメトリ vs 離散ベイズフィルタによる統合結果

上記が結果になりますが,上図が歩行者の歩幅のみで計算した自分の位置で,下図が「歩行者の歩幅で計算した自分の位置」と「自由の女神像までの距離計測」を統合した結果になります.歩行者の歩幅のみで自分の位置を計測した場合,距離が大きくなるに従って分布が平たくなってしまい,位置の信頼性がなくなっていることがわかります.一方で,統合した場合,ピークの高さはほぼ変わらず,またある程度正しい位置にピークが来ていることがわかります.

参考文献

確率ロボティクス (プレミアムブックス版)

確率ロボティクス (プレミアムブックス版)