EKF Localization ~ 拡張カルマンフィルタを用いた自己位置推定（実験編）

ということで，先月末からかじりついている確率ロボティクスですが，７章の拡張カルマンフィルタを用いた自己位置推定を実装してみたので書き留めておきます．

実験結果

問題設定のイメージは，確率ロボティクスにも出ていた RoboCup にしてみました．下記の写真は aibo がサッカーをする Field の写真ですが，Field の端に 6 本の色付きのバーが立っていることがわかります．ロボットは事前に位置のわかっている 6 本のバーを計測することによって，自分がフィールド上でどこにいるかを推定してサッカーをします．

昔の RoboCup の様子．

で，上記のサッカーフィールドで楕円の軌跡を描いて移動するロボットが自己位置推定をするイメージで EKF Localization のサンプルコードを作ってみました．

youtu.be

で，ビデオの説明ですが，黒の点が真値，青の点が入力（速度，角速度）を単純に積分した位置，赤の点が EKF による自己位置推定の結果です．入力を積分した結果ですが，どうも楕円ぽい軌道にならないのでここは自分の間違いがあるかもしれないです．ただ，結果的に EKF を使って正しく自己位置推定ができているようにみえるので，良しとします．計測されたポールは画面上で黄色で表示されます．

先日のエントリで書いたように，カルマンフィルタを用いると，異なる二つの推定結果を「いい感じ」に統合してくれます．
daily-tech.hatenablog.com

ただし，異なる二つの推定がどんな確率分布になっていても，無理やり一つの正規分布で表現します．ここでは，

１．入力を積分したロボットの存在確率分布
２．計測から求まるロボットの存在確率分布

の二つがあって，この異なる二つの正規分布を最適に統合して一つの正規分布を作っています．で，この統合後の正規分布の平均値が赤でプロットされた点になっています．

理論とか式展開とか．．．

上記の内容ですが，ほぼ確率ロボティックスに書いてある内容になります．
またどこかで時間があるときに，理論編のエントリも書きたいと思います．

ソースコード

代わりにソースコードを Github に置いておいたので，もしよければ使ってみてください．
github.com

上記のリポジトリを落として，

cd tests
python3 ekf_localization_example.py

とやれば，うごくはず！

参考文献＆サイト

確率ロボティクス

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

困ったときはこのサイト！
myenigma.hatenablog.com