はじめに
カルマンフィルタはガウス分布を使用してロボットの位置を推定するため、
ロボットの位置だけでなく、
カルマンフィルタで推定した共分散行列を使用することで、
"99%の確率でロボットはこの範囲に存在している"
という領域を計算することもできます。
このような領域は一般的に楕円で計算されるため
カルマンフィルタの"誤差楕円(Error Ellipse)"と呼ばれます。
冒頭のグラフのように、自己位置推定時に
誤差楕円を表示できるようにしておくと、
どのぐらいの範囲に位置計測結果が収束しているのかを
確認することができるので、意外と便利です。
なので今回は、
カルマンフィルタにおける誤差楕円の計算＆描画の仕方を説明したいと思います。
誤差楕円の計算方法
x-y方向の誤差の標準偏差をそれぞれσ_x, σ_yとした時、
その誤差楕円の関数は、
で表すことができます。
ここでΧ^2は誤差楕円の信用区間を表すカイの二乗分布の値です。
例えば今回のカルマンフィルタの誤差楕円は、x-yの二次元なので、
自由度(次元)が2で、99%の信用区間(p=1-0.99=0.01)の場合は、下記の表から
Χ^2=9.21034となります。
もし信用区間をもっと狭めたい場合は、上の表から対応する値を選んで下さい。
では、実際に誤差楕円のパラメータを計算してみましょう。
まず初めに、カルマンフィルタで計算された共分散行列から、
x-yの部分の2x2の行列Pxyを取り出します。
続いて、共分散行列Pxyの固有値λと固有ベクトルvを計算します。
ここでPxyは2x2なので、固有値は２つ計算され、
その内大きい方の固有値をλ1,小さい方をλ2とします。
続いて、この固有値を使って誤差楕円の長辺a,短辺bは
式(i)より、下記の式で計算されます。
前述の通り、
Χ^2の値は自分がどれ位の信用区間の誤差楕円を計算したいかによって決まります。
ちなみにλがルートの中に入っている理由は、
λは分散であり、式(i)はの誤差は標準偏差だからです。
最後に、大きい方の固有値λ1に対応する固有ベクトルv1を使って、
下記の式で楕円の角度θを計算します。
すると、現在のロボットの推定位置を中心とし、
長径a,短径bの楕円をθ傾けることにより、
誤差楕円を描画することができるのです。
あとは、下記のサンプルコードを見てもらえると、
誤差楕円の計算方法がより理解できると思います。
MATLABサンプルコード
下記の各カルマンフィルタのサンプルコードの
ShowErrorEllipse関数が誤差円を計算して、描画する関数です。
MATLABRobotics/Localization/ExtenedKalmanFilterLocalization/ExtendedKalmanFilterLocalization.mgithub.com
冒頭のグラフは、上記のEKFでのシミュレーション結果ですが、
推定結果は真値からずれることはあるものの、
必ず信用区間99%の誤差円の中に真値が収まっていることから、
正しい推定が行えていることがわかります。
また、上記のサンプルコードにおいて、
意図的に観測値による更新(Update)をしないように
ソースをコメントアウトすると、
予測だけで確率を計算するため、共分散の値が大きくなり、
誤差楕円が徐々に大きくなっていく所も見ることができます。
参考資料
拡張カルマンフィルタを使用した自己位置推定MATLABサンプルプログラム - MY ENIGMA
Unscentedカルマンフィルタを使用した自己位置推定MATLABサンプルプログラム - MY ENIGMA
付表：カイ2乗分布表 chi-square distribution ― 中川雅央（滋賀大学）
How to draw an error ellipse representing the covariance matrix?
members3.jcom.home.ne.jp/civilyarou/f90_STAT/dir_SREG/TeX_ellipse.pdf
