カルマンフィルタ

編集

制御

概要

  • EKF(Extended Kalman Filter)の立式方法について。
    • カルマンフィルタは、状態量の平均値と分散共分散行列を推定

参考

手順

  • センサ情報を、状態量に対して位置的な役割になりそうなものをz, 速度的な役割になりそうなものをuと振り分ける。
  • 観測方程式をたてる
    • 観測方程式のうち、カルマンフィルタで推定したくない観測可能な項を移行したものをzとする。
    • 観測方程式を移行した後、残った推定したい項に含まれる推定したい変数を状態muとする。
    • 観測方程式の関数をh(mu)をする。h(mu)のヤコビアンHを計算する。
  • 予測方程式をたてる
    • muのシステム方程式g(mu, u)を立式する。
    • システム方程式を離散化する
    • g(mu, u)のmuに関するヤコビアンである状態遷移行列G, g(mu, u)のuに関するヤコビアンV、M=diag(\alpha u)を定義する。
  • カルマンフィルタの予測・観測段を書き下し、必要に応じて単純化する。

用語

状態量: 推定する目的の確率変数のベクトル。で表す。

平均値: この文書の文脈では、状態量の平均。で表す。

分散共分散行列: このページの文脈では、状態量の分散共分散行列。で表す。

制御量: 状態量の速度の観測に関する、確率変数のこと。で表す。

状態遷移関数: ある制御量が新しく入力された時、以前の平均値とその制御量によって、どの平均値に移るかを推定するベクトル関数。で表す。

予測段: カルマンフィルタは二つのステップに完全に独立に分かれている。その内、制御量を用いて、平均値と分散共分散を更新するステップ。

観測量: センサによる観測に関する、確率変数のこと。で表す。

観測関数: 平均値と何らかモデルを入力にして、理想的な観測を計算するための関数。で表す。モデルを明示する場合は、とすることもある。

観測段: カルマンフィルタは二つのステップに完全に独立に分かれている。その内、観測量を用いて、平均値と分散共分散行列を更新するステップ。

イメージ

状態量: ロボットの位置(x, y, theta)の確率変数

平均値: ロボットの自己位置の中で最も尤もらしい場所。これがいわゆるオドメトリとして扱われる。

分散共分散行列: ロボットの自己位置の不確かさ。例えば、ロボットが「自分は大体x方向に関して標準偏差5cmくらいはずれてるかも…」とか思ってることを表現している。

制御量: エンコーダの情報。確かにこれらはx, y, thetaの速度を司る。

状態遷移関数: オドメトリ更新式。これは、「一つ前のオドメトリとエンコーダ」を入力に、「直後のオドメトリ」を得るベクトル関数である。

予測段: エンコーダによる更新。平均値と分散共分散行列の更新の2本の式に依るが、今までの更新式はまさに平均値の更新にあたる。

観測量: 測距センサによる観測や、カメラや測域センサによるランドマーク検出などによる距離情報や角度情報。

観測関数: オドメトリと測距センサがついている位置から、センサの出力の理想的な出力を計算するための関数。

観測段: 測距、カメラ、測域などによる情報を組みこむ更新。

予測段

予測段では、状態量の速度に関する情報を更新に組みます。

時間の添え字について=

更新前の状態量:

更新後の状態量:

更新に使う制御量:

時刻tで得られた制御量によって、時刻t-1の状態量が、時刻tの状態量に更新される、というイメージです。

式

式は以下のとおりです。

G: に関するヤコビ行列。

R: の分散共分散行列。その1回の予測により、どれくらい状態量の不確かさが増加するのか、を表す。

…(1)

例: 二輪ロボットのオドメトリ

状態量と制御量を以下のように設定します。