管理メニュー

おべんきょうwiki

カルマンフィルタ

最終更新：

yahirohumpty

- view
管理者のみ編集可

カルマンフィルタ


誤差が正規分布であると考えられるときに現在の状態と誤差分布を求めることができる．
ここでは構成法にのみ注目する．
詳しい解説についてはWikipediaか関連書籍を読むとよい．



利用するために必要な定式化


とりあえず決めなければならないことは，
状態ベクトル x_t と計測ベクトル z_t の構成である．

それができたら，状態方程式を構成する．
x_t = A_t x_{t-1} + B_t u_t + e_t
u_tは入力ベクトル，e_tは誤差ベクトルである．
だが，ここで注目すべきはe_tは誤差項なので計算に直接は関わらないこと．
決めなければいけないのはe_tの共分散行列R_tである．
i.e. R_tはx_tの誤差共分散行列．

次に計測方程式を構成する．
z_t = C_t x_t + d_t
現在の状態から計測値を求めるということに注意．単なる推定の逆になっている．
ここでもd_tは誤差項なので決めなくてよい.
d_tの共分散行列Q_tを決める．
i.e. Q_tはz_tの誤差共分散行列．

まとめると，カルマンフィルタでするべきことは，
  • x_tとz_tを決める．ついでにu_tも決める．
  • x_tとx_{t-1}の関係からA_tを決める．
  • u_tとx_tの関係からB_tを決める．
  • x_tとz_tの関係からC_tを決める．
  • x_tの構成要素からR_tを決める．
  • z_tの構成要素からQ_tを決める．

肝はR_tとQ_tをどう決めるかである．
適当な値を入れても計算はできるが簡単に発散する．
むやみに小さな値を入れても発散する．
うまいこと調整するといい．


アルゴリズム


一時刻前の状態を{\mu}_{t-1} ,共分散を{\Sigma}_{t-1}とすると，

  1. \bar{{\mu}_t} = A_t {\mu}_{t-1} + B_t u_t
  2. \bar{{\Sigma}_{t-1}} = A_t {\Sigma}_{t-1} A_t^T + R_t
  3. K_t = \bar{{\Sigma}_{t-1}} C_t^T ( C_t \bar{{\Sigma}_{t-1}} C_t^T +Q_t )^{-1}
  4. {\mu}_t = \bar{{\mu}_t} + K_t ( z_t - C_t \bar{{\mu}_t} )
  5. {\Sigma}_t = ( I - K_t C_t ) \bar{{\Sigma}_{t-1}}


拡張カルマンフィルタ


カルマンフィルタはすべてが線形でないと構成できない．
拡張カルマンフィルタは線形化を施すことで非線形に拡張したもの．


利用するために必要な定式化


まずは状態方程式と計測方程式を立てる．カルマンが非線形なだけだから問題ないはず．
すなわち状態方程式と計測方程式はそれぞれ次のようになる．

x_t = g(x_{t-1},u_t) + e_t
z_t = h(x_t) + d_t

ここで問題なのは状態関数g(x_{t-1},u_t)と計測関数h(x_t)のヤコビアンG_t,H_tを求める必要があること．
場合によっては超難しいが高校数学を駆使すればできなくもない．
あとはMathematicaを使おう．ただし最初から使うと泥沼化しかねないので注意．

拡張カルマンフィルタですべきことは,
  • 基本的にはカルマンのときと一緒だが，
  • A_tの代わりにg(x_{t-1},u_t)を決める．
  • C_tの代わりにh(x_t)を決める．
  • g(x_{t-1},u_t)のxに関するヤコビアンG_tを求める．
  • h(x_t)のxに関するヤコビアンH_tを求める．


アルゴリズム


カルマンフィルタの一部をg,h,G,Hに直しただけ．

  1. \bar{{\mu}_t} = g( {\mu}_{t-1} . u_t )
  2. \bar{{\Sigma}_{t-1}} = G_t {\Sigma}_{t-1} G_t^T + R_t
  3. K_t = \bar{{\Sigma}_{t-1}} H_t^T ( H_t \bar{{\Sigma}_{t-1}} H_t^T +Q_t )^{-1}
  4. {\mu}_t = \bar{{\mu}_t} + K_t ( z_t - h( \bar{{\mu}_t} ))
  5. {\Sigma}_t = ( I - K_t H_t ) \bar{{\Sigma}_{t-1}}


補足


カルマンフィルタ，拡張カルマンフィルタは長いのでKF,EKFと略されることが多い．
誤差が正規分布にのっとらない場合は素直にパーティクルフィルタ使おう．


参考文献


Thrun et. al.: "確率ロボティクス" , 毎日コミュニケーションズ , 2007.
Amazon.co.jp： 確率ロボティクス (ROBOT books): Sebastian Thrun, Wolfram Burgard, Dieter Fox, 上田 隆一: 本
http://www.amazon.co.jp/%E7%A2%BA%E7%8E%87%E3%83%AD%E3%83%9C%E3%83%86%E3%82%A3%E3%82%AF%E3%82%B9-ROBOT-books-Sebastian-Thrun/dp/4839924015/ref=sr_1_1?ie=UTF8&s=books&qid=1222852098&sr=8-1

タグ：

時系列フィルタ
「カルマンフィルタ」をウィキ内検索
「時系列フィルタ」関連ページ
人気記事ランキング
  1. ロバスト推定
  2. パーティクルフィルタ
  3. RRT
  4. kD-tree
もっと見る
最近更新されたページ
もっと見る
目安箱バナー