読者です 読者をやめる 読者になる 読者になる

MyEnigma

とあるエンジニアのブログです。#Robotics #Programing #C++ #Python #MATLAB #Vim #Mathematics #Book #Movie #Traveling #Mac #iPhone

Unscentedカルマンフィルタを使用した自己位置推定MATLABサンプルプログラム

Robot MATLAB

はじめに

以前、自律ロボットにおける

拡張カルマンフィルタを使用した自己位置推定の

MATLABサンプルプログラムを公開しました。

拡張カルマンフィルタを使用した自己位置推定MATLABサンプルプログラム - MY ENIGMA


今回はカルマンフィルタの一種である

Unscented Kalman Filter (UKF)

(シグマポイントカルマンフィルタとも呼ばれます)

のMATLABサンプルプログラムを公開したいと思います。


UKFのアルゴリズムの詳しい導出は、

下記のwikiか

カルマンフィルター - Wikipedia

下記の文献を参考にしてください。

確率ロボティクス (Mynavi Advanced Library)

確率ロボティクス (Mynavi Advanced Library)

Probabilistic Robotics (Intelligent Robotics and Autonomous Agents series)

Probabilistic Robotics (Intelligent Robotics and Autonomous Agents series)

  • 作者: Sebastian Thrun,Wolfram Burgard,Dieter Fox
  • 出版社/メーカー: The MIT Press
  • 発売日: 2005/08/19
  • メディア: ハードカバー
  • 購入: 1人 クリック: 6回
  • この商品を含むブログを見る


UKFの利点と欠点

UKFは、EKFと比べて幾つかの利点と欠点があり、

それらを考慮して利用することが重要です。

利点1: ヤコビ行列の導出が不要

EKFは非線形モデルをヤコビ行列で近似しますが、

モデルが複雑な場合は、ヤコビ行列を導出することが難しいです。

(例えば、レーザのビームモデルを微分することは難しいです.)


一方、UKFでは非線形モデルをUnscented Transform (UT)

というサンプリングベースの近似手法を使用するため、

非常に複雑なモデルでも推定を行うことができます。

利点2: 非線形モデルの近似精度が高い

EKFは非線形モデルをヤコビ行列で近似しています。

ヤコビ行列はテイラー展開の一次項近似ですので、

モデルの非線形性が高いと、

どうしても近似誤差が大きくなってしまいます。


一方、UKFはUTを使用して近似を実施し、

UTはテイラー展開の2~3次項程度の近似精度が

あることがわかっています。


よって、非線形性の高いモデルを使用する場合、

EKFよりもUKFの方が高い推定精度を実現することができます。

欠点1:計算コストが高い

EKFはヤコビ行列を掛け合わせるだけで、

共分散の遷移を実施していますが、

UKFはシグマポイントというサンプリング点を

複数個生成し、それらをモデル遷移して、

重み付き足し算して共分散の遷移を実施します。


なのでEKFと比べると計算コストが高く、

組み込みシステムなどでは、少し問題が生じるかもしれません。

欠点2:調整すべきパラメータの数が多い

EKFはフィルタをチューニングするために、

それぞれの共分散行列のみをチューニングすれば良いですが、

UKFの場合は共分散行列に加えて、

UTの分布を決定するHyper Parameterというパラメータも

チューニングする必要があります。


UKFではα、β、κという3種類のハイパーパラメータが存在しており、

それらを調整する必要があります。


今回のサンプルプログラムでは、

大部分のシステムにおいて、

最適値と言われているパラメータの値を

使用していますが、

当然システムによって最適値は変わるので

チューニングが必要になります。


カルマンフィルタのシステムを開発する際には、

パラメータのチューニングは、大変な作業ですので、

調整すべきパラメータが増えるというのは、

UKFにとって大きな欠点であるといえます。

誤差楕円の計算方法

サンプルコード内の誤差楕円の計算方法に関しては

下記の記事をご覧下さい。

カルマンフィルタにおける誤差楕円の計算方法 - MY ENIGMA

参考資料

確率ロボティクス (Mynavi Advanced Library)

確率ロボティクス (Mynavi Advanced Library)

Probabilistic Robotics (Intelligent Robotics and Autonomous Agents series)

Probabilistic Robotics (Intelligent Robotics and Autonomous Agents series)

  • 作者: Sebastian Thrun,Wolfram Burgard,Dieter Fox
  • 出版社/メーカー: The MIT Press
  • 発売日: 2005/08/19
  • メディア: ハードカバー
  • 購入: 1人 クリック: 6回
  • この商品を含むブログを見る