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

gggggraziegrazie

graizegrazieさんのやったこと、学んだことを記録する雑記帳です

Probabilistic Robotics - Chap.9 Occupancy Grid Mapping

9.1 Introduction

 今まではロボットが動作を開始する前に、環境地図が既知であることを仮定していた。ただこれは現実世界では稀なケースである。そこでロボットが自律的に環境地図を知る必要がある。ただし、環境地図の獲得には、下記2つの理由から困難な課題の1つである。

  • 地図の候補空間は巨大である
  • 地図を得ることは"鶏と卵"の問題がある

困難度合いを決める要因には、下記4つがある。

  1. 地図のサイズ(地図を作る上で対象となる空間の大きさ)
  2. 認識・動作におけるノイズの程度
  3. 認識の曖昧さ(特徴量が少ない、似たような光景が広がっているなど)
  4. 周回路(ループクロージャの問題)

 本章では、ロボットの姿勢は既知と仮定({mappig \  with \  known \  pose}問題)。Grid Mappingは、ロボットの姿勢が既知という仮定の元、ノイズを含む不確かなセンサデータから環境地図を生成する技術である。地図中の各Gridは、{occupied}{free}のどちらかの値を取る。どちらの値を取るかは、近似的に実装する事後確率で表現する。
Occupancy Grid Mapは、SLAMやパスプランニングに使うことは少ない。

9.2 The Occupancy Grid Mapping Algorithm

環境地図に関する事後確率は
{
p(m | z_{1:t}, x_{1:t}) \tag{9.1}
}
で表される。ここで{x_{1:t}}は、計測開始後から現在までのロボットの経路(各時刻でのロボットの姿勢)を、{z_{1:t}}は計測開始後から現在までのセンサデータを、それぞれ表す。
なおOccupancy Grid Mapは一般的に2次元の地図を表すが、何層にも重ねることで3次元の地図を形成することもできる。ただし計算コストが膨大なため、通常Occupancy Grid Mapで3次元地図は構成しない。

環境地図{m}は、沢山のセルで構成されることから、
{
m \  = \  \{m_i\}
}
とできる。このとき{m_i \in \{0, \  1\}}{p(m_i \  = \  1) \  = \  p(m_i)}とする。

式(9.1)の問題は、次元数である。もしも地図が10,000個のセルで構成される場合、地図の次元は{2^{10000}}となる。そのため、地図がセルの集合体だと考えると、計算量が膨大となる。

そこで標準的なOccupancy Grid Mapは、各セルに注目して事後確率を計算する。つまり{p(m_i | z_{1:t}, x_{1:t})}を計算し、
{
\displaystyle p(m | z_{1:t}, x_{1:t}) = \prod_i p(m_i | z_{1:t}, x_{1:t})
}
のように統合することで、環境地図の事後確率を求める。

Occupancy Grid Mappingでは、各グリッドの状態を表すのに下記式(9.5)で表される対数オッズを使う。
{
\displaystyle l_{t, i} \  = \  log \frac{p(m_i | z_{1:t}, x_{1:t})}{1 \  - \  p(m_i | z_{1:t}, x_{1:t})} \tag{9.5}
}
対数オッズを使うメリットは、事後確率が計算結果が不安定になる0、1の近傍を取らなくなることである。

{
1: \  Algorithm \  occupancy\_grid\_mapping(\{l_{t-1, i}\}, x_t, z_t): \\
2: \  \quad  for\  all \  cells \  m_i \  do \\
3: \  \qquad if \  \  m_i \  \  in \  perceptual \  field \  of \  z_t \  then \\
4: \  \qquad  \quad l_{t, i} = l_{t-1, i} + inverse\_sensor\_model(m_i, x_t, z_t) - l_0 \\
5: \  \qquad else \\
6: \  \qquad  \quad l_{t,i} = l_{t-1, i}\\
7: \  \qquad endif \\
8: \  \quad  endfor \\
9: \  \quad return \{l_{t,i}\}
}

なお式中の{l_0}は、対数オッズで表現されるOccupancy Grid Mapの事前分布で、
{
\displaystyle l_0 = log \frac{p(m_i \  = \  1)}{p(m_i \  = \  0)} \  = \  log \frac{p(m_i)}{1 \  - \  p(m_i)}
}
とする。ここで、{p(m_i) \  = \  p(m_i \  = \  1)}であり、
{
\displaystyle inverse\_sensor\_model(m_i, x_t, z_t) \  = \  log \frac{p(m_i | z_t, x_t)}{1 - p(m_i | z_t, x_t)}
}
である。

なお、Occupancy Grid Mapを作成する時は、ロボット自身が専有する領域(ロボットの身体)を考慮することは大切である。特に通行人が多いとき等には有用である。

9.3 Learning Inverse Measurement Models

9.3.1 Inverting the Measurement Model

Occupancy Grid Mapは、各セルの逆観測モデル{p(m_i | x, z)}が必要である。
Table9.2ではとりあえずの逆観測モデルを定義した。
ここでいう逆観測モデルは、複数のマップを用意し、それぞれのマップの{m^k_i \  = \  m_i}を満たすマップを使って逆観測モデルを近似する。つまり、
{
\displaystyle p(m_i | x, z) \  = \  \eta \sum_{m:m(i)=m_i} p(z | x, m) p(m)
}
となる。上記の式は計算不可能のため、ロジスティクス回帰やニューラルネットワーク等の教師付き学習アルゴリズムを使って近似的に求める。

Probabilistic Robotics - Chap.6 Robot Perception

1.Introduction
6章に記載されているのはEnvironment Measurement Model(環境計測モデル)
確率ロボティクスではセンサデータの計測に伴うノイズもモデル化する

{ \displaystyle
p(z_t | x_t, m) \\
z_t:時刻tでのセンサデータ\\
x_t:時刻tでのロボットの位置\\
m:環境地図
}

確率ロボティクスでは、センサモデルの不正確さに対し、統計的手法を用いて対応する

{ \displaystyle
z_t = \{z^1_t, ..., z^K_t\}
z^k_t:センサAが時刻tにおいて計測した個々のデータ(LRFならば角度\thetaにおける距離)
}

{ \displaystyle
p(z_t | x_t, m) = \prod_{ k = 0 }^K p(z^k_t | x_t, m)
}
この式は、{p^k_t}がそれぞれ独立していることを仮定しているが、理想状態でしかそれはありえない。

6.2 Maps

地図{m}はオブジェクトのリストを表す
{\displaystyle
m = \{m_1, ... , m_N\}
}

地図には2種類ある

  • 特徴ベース:オブジェクトの位置は地図中に含まれている。形状情報のみ特徴として持つ。そのため、ロボット用のマップとしてはこちらの形式がよく使われる。
  • 位置ベース:{m}の要素は環境中に存在するオブジェクトの位置を表す。各オブジェクトの位置をデータとして保つ必要があるので、データサイズが大きい。空きスペースに関しての情報も保つ必要がある、つまり全セルの情報を保つ必要がある。

6.3 Beam Models of Range Finders

Range Finderは最もよく使われるセンサ

6.3.1 The Basic Measurement Algorithm

ここで紹介するモデルは4つのエラーを考慮する

  1. 分解能によって生じる誤差

正しい距離は計測できているものの、分解能の制約により誤差が乗る場合がある。
正しい距離を{z^{k*}_t}とする
本計測誤差は、平均{z^{k*}_t}、分散{\sigma_{hit}}で表される狭いガウス分布でモデル化できる。
{\sigma_{hit}}は本ノイズモデル固有のパラメータ

f:id:graziegrazie:20170405202747j:plain

  1. 予想外の物体

環境地図に元々含まれていない物体は、元々の地図に記載していたよりも短い距離で見つかることある
尤度は物体との距離に応じて減少していくことから、指数分布で表される。
この指数分布はモデル固有のパラメータ{\lambda_{short}}を使って表せる。

{
\begin{eqnarray}
p_{short}(z^k_t|x_t,m)
 =
  \begin{cases}
    \eta \lambda_{short} e^{-\lambda_{short} z^k_t} if 0 \gt z^k_t \gt z^{k*}_t \\
    0                                               otherwise
  \end{cases}
\end{eqnarray}
}

指数分布は自らの位置から本来の対象の位置までの尤度を表すため、累積確率は

  1. 計測失敗

計測可能レンジの超過
つまり計測値は最大値を示す

  1. ランダム計測

ごくたまに起きる説明の使いない計測結果

それぞれのエラーの密度関数は、それぞれ重みを掛けて合体することで、本モデルの確率密度関数を表現できる

Limitation of the Beam Model

Beamモデルには2つの主な欠点がある
滑らかさがない
 椅子を検知したら椅子の足それぞれマッピングするかもしれない
 ロボットの位置が変化したら、beamの座標変換の計算量が大変なことになる
滑らかさの欠如は2つの問題を招く
 beliefの近似が正しい状態を見失いやすい
 山登り法は局所最大値に陥りやすい
計算量が膨大

4 Liklihood Fields for Range Finders

6.3の制限を解決する尤度フィールドモデルについて述べる。
確率計算などはしないが、実際に使ってみると良い結果が求まる、アドホックアルゴリズム

キーとなるアイデアは、{z_t}の端点をグローバルマップ上に投影すること。
これにより、グローバルマップとロボットのローカルマップの位置関係がわかる。
投影される端点の座標は、
{
\left(
 \begin{array}{ccc}
  x_{z^k_t} \\
  y_{z^k_t}
 \end{array}
\right)
 = 
\left(
 \begin{array}{ccc}
  x \\
  y
 \end{array}
\right)
 +
\left(
 \begin{array}{ccc}
  cos \theta & -sin \theta \\
  sin \theta &  cos \theta
 \end{array}
\right)
\left(
 \begin{array}{ccc}
  x_{k,sens} \\
  y_{k,sens}
 \end{array}
\right)
 + z^k_t
\left(
 \begin{array}{ccc}
  cos \left(\theta + {\theta}_{k,sens} \right) \\
  sin \left(\theta + {\theta}_{k,sens} \right)
 \end{array}
\right)
\tag{6.32}
}
として表される。
このとき、{
x_t = 
\left(
 \begin{array}{ccc}
  x & y & \theta
 \end{array}
\right)^T
}はロボットの座標、
{
\left(
 \begin{array}{ccc}
  x_{k,sens} &  y_{k,sens}
 \end{array}
\right)^T
}
はセンサ検出データの端点
をそれぞれ示す。

Beam Modelと同様に、こちらは3種類のノイズを定義

  • 計測ノイズ

 計測過程で起こる、ガウス分布でモデル化されるノイズ

  • 計測失敗

 センサの最大計測可能距離の時を指す。その時、値が正しいかは不明。

  • 説明の付かない計測結果

 ランダムに発生するノイズ

5 Correlation-Based Measurement Models

文献でよく見かけるmap matchingを使うモデル。
map matchingは、連続するスキャンデータをローカルマップへ落とし込む
つまりmap matchingは、グローバルマップを{m}、ローカルマップを{m_{local}}とした時、尤度{p\left(m_{local} | x_t, m \right)}が高い場所を探すことを指す

尤度は、
{
p\left(m_{local} | x_t , m \right)
 =
max \left\{ {\rho}_{m , m_{local} , x_t} , 0 \right\}
}
で表され、グローバルマップとローカルマップの相関係数{{\rho}_{m , m_{local} , x_t}}と平均地図{\overline{m}}はそれぞれ、
{
 \rho_{m , m_{local} , x_t}
 = 
\frac
 {
  \sum_{x, y} \left(m_{x, y} - \overline{m} \right)
              \left( m_{x, y, local} \left( x_t \right) - \overline{m} \right)
 }
 {
  \sqrt
   {\sum_{x, y} \left(m_{x, y} - \overline{m} \right)^2
    \sum_{x, y} \left( m_{x, y, local} \left( x_t \right) - \overline{m} \right)^2
   }
 }
}
{
\overline{m}
 = 
\frac{1}{2N}
\sum_{x, y} \left( m_{x, y} + m_{x, y, local} \right)
}
尤度フィールドに対するmap matchingの利点は、相関係数に応じてフリースペースを決めること。これにより、センサの計測範囲外のデータも考慮してマップを作ることができる
一方map matchingの欠点は、物理的に明確な説明ができないこと

Feature-Based Measurement Model

今まではセンサの生値をベースとした話
これは一歩進んだ、センサデータから求めた特徴量をベースとした話
Featureベースの利点は、特徴量を低次元化することで計算量の削減が可能な点

ロボットの世界では、環境中の特徴量はランドマークと呼ぶことが一般的
特徴量を求める時に一般的なのは、センサがセンサを基準としたランドマークとの距離と方角を計測できるとする仮定
加えて、特徴量抽出器はサイン(本書では数値で表されるとする)を出力する可能性がある

特徴量ベクトル{f}は、距離{r}と方角{\phi}、サイン{s}それぞれを使って
{
f \left( z_t \right) = \left\{ f^1_t, f^2_t, ... \right\}
 = 
\left\{
 \left(
  \begin{array}{ccc}
   r^1_t    \\
   \phi^1_t \\
   s^1_t
  \end{array}
 \right)
 ,
 \left(
  \begin{array}{ccc}
   r^2_t    \\
   \phi^2_t \\
   s^2_t
  \end{array}
 \right)
 ,...
\right\}
}

確率ロボティクスのアルゴリズムでは下記の条件付き確率を仮定する
{ \displaystyle
 p \left( f \left( z_t \right) | x_t, m\right) = \prod_i p(r^i_t, \phi^i_t, s^i_t | x_t, m)
}

特徴量ベース地図に向けたセンサモデルの改定
特徴量の地図上での位置を{m_i}とし、ロボットの座標を{ \left( \begin{array}{ccc} x \ \  y \ \  \theta \end{array}\right)^T }とすると、特徴量の位置は
{
 \left(
  \begin{array}{ccc}
   r^i_t    \\
   \phi^i_t \\
   s^i_t
  \end{array}
 \right)
 = 
 \left(
  \begin{array}{ccc}
   \sqrt{ \left( m_{j, x} - x \right)^2 + \left(m_{j, y} - y \right)^2 } \\
   atan2 \left( m_{j, y} - y, m_{j, x} - x \right) - \theta              \\
   s_j
  \end{array}
 \right)
 +
 \left(
  \begin{array}{ccc}
   \epsilon_{\sigma^2_r}    \\
   \epsilon_{\sigma^2_\phi} \\
   \epsilon_{\sigma^2_s}
  \end{array}
 \right)
}
ここで{\epsilon_{\sigma^2_r}と\epsilon_{\sigma^2_r}, \epsilon_{\sigma^2_r}}は、平均0で、分散がそれぞれ{\sigma_r, \sigma_\phi, \sigma_s}のガウシアン誤差変数

距離・角度センサの主要な問題点は、データの関連付けである。
これを解決するために{f^i_t}{m_j}の間の一致変数{c}を導入する
{c_i = j \leq N}の時、{i}番目の特徴量は{j}番目のランドマークと一致する
{N}はランドマークの総量

Sampling Poses

今までは特徴量の確からしさについて議論してきた
一方で、特徴量と地図情報からロボットの座標を推定したい時もある
そのためには、事前確率{p \left( x_t | c^i_t, m \right)}が必要である。
あり得ないが事前確率を均一分布とするとロボットの座標の尤度は
{
 p \left( x_t | f^i_t, c^i_t, m \right) = \eta \  p \left( f^i_t | c^i_t, x_t, m \right) p \left( x_t | c^i_t, m \right) \\
 \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \  = \eta \  p \left( f^i_t | c^i_t, x_t, m \right)
}

Probabilistic Robotics - Chap.7 Mobile Robot Localization: Markov and Gaussian

7

移動ロボットのローカライゼーションとは、環境地図中でロボットの座標を決める問題である
一方で、座標変換の問題とも捉えることができる
ローカライゼーションとは、地図座標系とロボットのローカル座標系のマッチングをとるプロセスである
ただし残念なことに、ロボットの座標を直接センシングする方法はない。
ノイズの無いセンサなどない。
センサ1つでは座標を求めるには情報が足りない

7.1

ローカライゼーションの問題点を分類してみる

  • ローカル VS グローバル

ローカライゼーション問題は起動時もしくは実行中に利用できる知識で特徴づけられる
難しさに応じて3つに分類する

  1. 位置追跡

この時ロボットの初期座標が既知だとする
計測した位置の含まれる誤差は小さいと仮定
位置追跡は、ロボットの真の座標周辺で起こるローカルな問題

  1. グローバルローカライゼーション

ロボットの初期座標は不明

  1. 誘拐ロボット問題(kidnapped robot problem)

グローバルローカライゼーションの一種だが、一層難しい問題
動作中にロボットがどこかに連れ去られたり、テレポートした時の位置特定の問題

  • 静的 vs 動的

環境が静的か動的かということ
静的な環境とは、変数がロボットの座標のみ
動的な環境とは、環境中にロボット以外にも、位置や設定が可変なオブジェクトがいる。人や日光など。

動的な環境に対応するには2つの方法がある

  1. 動的な物体は状態ベクトルに含める

マルコフ仮定で正当化できるが、計算量が増える等の副作用がある

  1. ある状態下ではモデル化されていない動作を排除する
  • パッシブ vs アクティブアプローチ

ローカライゼーションアルゴリズムがロボットの動作を制御できるかどうか

  1. パッシブローカライゼーション

ロボットの動作のみ観測する
ロボットはローカライゼーションとは別の方法で制御される
例えばランダムに動いたり、日常業務をこなす

  1. アクティブローカライゼーション

ローカライゼーションアルゴリズムが観測だけでなく、ロボットの制御もする
制御はローカライズの誤差や危険地帯にローカライズの精度が悪いロボットを動かすためのコストを最小化する

アクティブローカライゼーションの重要な制約事項は、アクティブローカライゼーションがロボット全身を制御することが必要な点である。
実際の面では、ローカライゼーション以外のタスクも実行している時にローカライゼーションが出来ることが必要。

  • ロボット単体 vs ロボット複数台

ロボットの数で分類

  1. 単体ロボットのローカライゼーション

ロボット間の通信問題がなく、また全データを単体のロボットへ使えるという便利さ

  1. 複数台ロボットのローカライゼーション

結局は単体ロボットそれぞれのローカライゼーション問題

7.2 マルコフローカライゼーション

確率的ローカライゼーションのアルゴリズムは、ベイズフィルタの派生
その単純な利用例としてマルコフローカライゼーションがある。
マルコフローカライゼーションのアルゴリズムは下記の通り
{
Algorithm \  Markov_localization \left( bel \left( x_{t-1} \right), u_t, z_t, m \right):\\
\ \ \  for \  all \  x_t \  do\\
\ \ \ \ \ \  \overline{bel} \left( x_t \right) = \int p \left( x_t | u_t, x_{t-1}, m \right) bel \left( x_{t-1} \right) dx_{t-1}\\
\ \ \ \ \ \  bel \left( x_t \right) = \int \eta \  p \left( z_t | x_t, m \right) \overline{bel} \left( x_t \right)\\
\ \ \  endfor\\
\ \ \  return \  bel \left( x_t \right)
}
マルコフローカライゼーションが対応するのは、静的環境下での

  • 位置追跡問題
  • グローバルローカライゼーション問題
  • 誘拐ロボット問題

初期信念{bel \left( x_0 \right)}は、ロボットの初期姿勢を反映したもので、ローカライゼーション問題毎に別の値をセットする。

  • 位置追跡問題

初期姿勢が既知であれば、{bel \left( x_0 \right)}は質点分布として初期化される
初期姿勢を{\overline{bel} \left( x_0 \right)}とすると、
{
bel \left( x_0 \right)
 = 
\left\{
 \begin{array}{}
   1 &  if \ x_0 \  = \  \overline{x_0}\\
   0 &  otherwise
 \end{array}
\right.
}

信念{bel \left( x_0 \right)}は大抵{x_0}を中心とした、分散の小さなガウス分布で初期化する

・グローバルローカライゼーション
初期座標が未知の場合、{bel \left( x_0 \right)}は、環境地図を対象とした一様分布で初期化する
{bel \left( x_0 \right) = \frac{1}{|X|}}
ここで{|X|}は、地図内の全姿勢空間の体積(ルベーグ測度*1)である。

7.3 EKF(Extended Kalman Filter)ローカライゼーション

EKFローカライゼーションはマルコフローカライゼーションの特殊ケース
本節で紹介するEKFローカライゼーションは、特徴量の集合でマップが表されるものと仮定
特徴量のIDは、一致変数で表現する
一致変数の値は既知とする

7.4.1

EKFでの単峰な信念を順応させるために、2つのことを仮定する

  1. 求めた特徴量がどのランドマークと一致するかは既知
  2. ロボットの初期姿勢は比較的よくわかる(直訳だとこうなんだが日本語としては変だよねぇ。。)

更新後の信念の分散は、更新前の信念や観測密度(p(z|x)だから、観測モデルとしても良い気が。。)よりも小さくなる
 → 独立する推定値を纏めることで、不確かさを小さくすることができるため

7.4.2

本小節では、EFKローカライゼーションの具体例について述べる
特徴量ベースの地図で、速度ベースの運動モデルを使用する
下記はランドマークと特徴量の一致が既知で、回転運動を含む場合(直進のみ、{\omega_t = 0}の場合は除く)とする

{
Algorithm \  EKF\_Localization\_known\_correspondences \left( \mu_{t-1}, \sigma_{t-1}, u_t, z_t, c_t, m \right):\\
\theta = \mu_{t-1, \theta}\\

G_t =
\begin{pmatrix}
 1 & 0 & -\frac{v_t}{\omega_t}cos \theta + \frac{v_t}{\omega_t} cos \left( \theta + \omega_t \Delta t \right) \\
 0 & 1 & -\frac{v_t}{\omega_t}sin \theta + \frac{v_t}{\omega_t} sin \left( \theta + \omega_t \Delta t \right) \\
 0 & 0 & 1
\end{pmatrix}\\

V_t = 
\begin{pmatrix}
 \frac{-sin \theta + sin \left( \theta + \omega_t \Delta t \right)}{\omega_t} & 

 \frac{v_t sin \theta - sin \left( \theta + \omega_t \Delta t \right)}{\omega^2_t} + \frac{v_t cos \left( \theta + \omega_t \Delta t \right) \Delta t}{\omega_t} \\

 \frac{cos \theta - cos \left( \theta + \omega_t \Delta t \right)}{\omega_t} & 

 - \frac{v_t cos \theta - cos \left( \theta + \omega_t \Delta t \right)}{\omega^2_t} + \frac{v_t sin \left( \theta + \omega_t \Delta t \right) \Delta t}{\omega_t} \\

 0 & \Delta_t
\end{pmatrix}\\

M_t = 
\begin{pmatrix}
 \alpha_1 v^2_t + \alpha_2 \omega^2_t & 0 \\
 0 & \alpha_3 v^2_t + \alpha_4 \omega^2_t
\end{pmatrix}\\

\overline{\mu_t} = \mu_{t-1} + 
\begin{pmatrix}
 - \frac{v_t}{\omega_t}sin \theta + \frac{v_t}{\omega_t} sin \left( \theta + \omega_t \Delta t \right) \\
   \frac{v_t}{\omega_t}cos \theta - \frac{v_t}{\omega_t} cos \left( \theta + \omega_t \Delta t \right) \\
 \omega_t \Delta t
\end{pmatrix}\\

\overline{\Sigma_t} = G_t \Sigma_{t-1} G^T_t \  + \  V_t M_t V^T_t \\

Q_t = 
\begin{pmatrix}
 \sigma^2_r & 0               & 0            \\
 0          & \sigma^2_{\phi} & 0            \\
 0          & 0               & \sigma^2_{s}
\end{pmatrix}\\

for \  all \  observed \  features \  z^i_t \  = 
\begin{pmatrix}
 r^i_t & \phi^i_t & s^i_t
\end{pmatrix}^T
\  do\\

\  \  \  j \  = \  c^i_t \\

\  \  \  q \  = \  \left( m_{j, x} - \overline{\mu}_{t, x} \right)^2 + \left( m_{j, y} - \overline{\mu}_{t, y} \right)^2 \\

\  \  \  \hat{z}^i_t = 
\begin{pmatrix}
 \sqrt{q} \\
 atan2 \left( m_{j, y} - \overline{\mu}_{t, y}, m_{j, x} - \overline{\mu}_{t, x} \right) - \overline{\mu}_{t, \theta} \\
 m_{j, s}
\end{pmatrix}\\

\  \  \  H^i_t = 
\begin{pmatrix}
 - \frac{m_{j, x} - \overline{\mu}_{t, x}}{\sqrt{q}} &
 - \frac{m_{j, y} - \overline{\mu}_{t, y}}{\sqrt{q}} &
 0                                                   \\
 \frac{m_{j, y} - \overline{\mu}_{t, y}}{q}          &
 \frac{m_{j, x} - \overline{\mu}_{t, x}}{q}          &
 -1                                                  \\
 0 & 0 & 0
\end{pmatrix}\\

\  \  \  S^i_t = H^i_t \overline{\Sigma}_t [ H^i_t ]^T + Q_t \\

\  \  \  K^i_t = \overline{\Sigma}_t [ H^i_t ]^T [ S^i_t ]^{-1} \\

\  \  \  \overline{\mu}_t = \overline{\mu}_t + K^i_t \left( z^i_t - \hat{z}^i_t \right) \\

\  \  \  \overline{\Sigma}_t = \left( I - K^i_t H^i_t \right) \overline{\Sigma}_t \\

endfor \\

\mu_t = \overline{\mu}_t \\

\Sigma_t = \overline{\Sigma}_t \\

p_{z_t} = \prod_i det \left(2 \pi S^i_t \right)^{- \frac{1}{2}} exp \left\{ - \frac{1}{2} \left( z^i_t - \hat{z}^i_t \right)^T [S^i_t ]^{-1} \left( z^i_t - \hat{z}^i_t \right) \right\}\\

return \  \mu_t, \Sigma_t, p_{z_t}
}

7.5 Estimating Correspondences

7.5.1 EKF Localization with Unknown Correspondences

今までのEKFローカライゼーションは、ランドマークの一致が不確実性無しに決められる時のみ使えた。でもそんなの極めてまれ。
ローカライゼーションの過程でランドマークの一致を計算する方法としては、maximum likelihood correspondenceがある
ただ不安定さがあるため、2つのテクニックを使う

  1. 互いを混乱させるような十分にユニークで十分に離れているランドマークを選択することは考えにくい
  2. ロボットの姿勢の不確かさが小さいことを確認

今までのEKFローカライゼーションと今回のものの主な違いは、measurement updateである。

7.6 Multi-Hypothesis Tracking

データの関連付けが困難な時に使う手法として、Multi-Hypothesis Tracking(MHT)がある
MHTはbeliefを複数のガウス分布で表せる
MHTアルゴリズムは混合成分の数を小さく保てる
本書ではMHT自体の詳細は紹介せず、MHTを使ったアルゴリズムを紹介する

7.7 UKF Localization

UKFローカライゼーションは、unscented Kalman Filterを使った特徴量ベースのアルゴリズムアルゴリズムの詳細は下記の通り。なお下記では{z_t}で検出されるランドマークは1つであり、そのランドマークのIDは既知と仮定している。

{
1: \  \  \  Algorithm \  UKF\_Localization(\mu_{t-1}, \Sigma_{t-1}, u_t, z_t, m): \\
\  \  Generate \  augmented \  mean \  and \  covariance \\

2: \  \  \  \  \  M_t = 
\begin{pmatrix}
\alpha_1 v^2_t + \alpha_2 \omega-2_t && 0 \\
0 && \alpha_3 v^2_t + \alpha_4 \omega-2_t
\end{pmatrix}
\\

3: \  \  \  \  \  Q_t =
\begin{pmatrix}
\sigma^2_r && 0 \\
0 && \sigma^2_{\phi}
\end{pmatrix}
\\

4: \  \  \  \  \  \mu^{\alpha}_{t-1} = 
\begin{pmatrix}
\mu^T_{t-1} && (0 \  \  0)^T && (0 \  \  0)^T)^T
\end{pmatrix}
\\

5: \  \  \  \  \  \Sigma^{\alpha}_{t-1} = 
\begin{pmatrix}
\Sigma_{t-1} && 0 && 0 \\
0 && M_t && 0 \\
0 && 0 && Q_t
\end{pmatrix}
\\

\  \  Generate \  Sigma \  Points \\

6: \  \  \  \  \  \chi^{\alpha}_{t-1} = 
\begin{pmatrix}
\mu^{\alpha}_{t-1} && \mu^{\alpha}_{t-1} + \gamma \sqrt{\Sigma^{\alpha}_{t-1}} && \mu^{\alpha}_{t-1} - \gamma \sqrt{\Sigma^{\alpha}_{t-1}}
\end{pmatrix}
\\

\  \  Pass \  Sigma \  Points \  through \  motion \  model \  and \  compute \  Gaussian \  statistics \\

7: \  \  \  \  \  \chi^x_t = g(u_t + \chi^u_t , \chi^{x}_{t-1}) \\ 

8: \  \  \  \  \  \overline{u}_t = \sum^{2L}_{i=0} w^{(m)}_i \overline{\chi}^x_{i,t} \\
9: \  \  \  \  \  \overline{\Sigma}_t = \sum^{2L}_{i=0} w^{(c)}_i (\overline{\chi}^x_{i,t} - \overline{u}_t)(\overline{\chi}^x_{i,t} - \overline{u}_t)^T \\

\  \  Predict \  observations \   at \   sigma \   points \   and \   compute \   Gaussian \  statistics \\

10: \  \  \  \  \  \overline{Z}_t = h(\overline{\chi}^x_t) + \chi^z_t \\
11: \  \  \  \  \  \hat{z} = \sum^{2L}_{i=0} w^{(m)}_i \overline{Z}_{i, t} \\
12: \  \  \  \  \  S_t = \sum^{2L}_{i=0} w^{(c)}_i (\overline{Z}_{i, t} - \hat{z}_t) (\overline{Z}_{i, t} - \hat{z}_t)^T \\
13: \  \  \  \  \  \Sigma^{x, z}_t= \sum^{2L}_{i=0} w^{(c)}_i (\overline{\chi}^x_{i, t} - \hat{z}_t) (\overline{Z}_{i, t} - \hat{z}_t)^T \\

\  \  Update \  mean \  and \  covariance \\

14: \  \  \  \  \  K_t = \Sigma^{x, z}_t S^{-1}_t \\
15: \  \  \  \  \  \mu_t = \overline{\mu}_t + K_t (z_t - \hat{z}_t) \\
16: \  \  \  \  \  \Sigma_t = \overline{\Sigma}_t - K_t S_t K^T_t \\
17: \  \  \  \  \  p_{z_t} = det(2 \pi S_t)^{- \frac{1}{2}} exp \{ -\frac{1}{2} (z_t - \hat{z}_t)^T S^{-1}_t (z_t - \hat{z}_t) \} \\
18: \  \  \  \  \  return \  \mu_t, \Sigma_t, p_{z_t}
}

7.11 Exercise

 自分なりの回答を記載してみます。もしもご覧になって、「正解!」「間違っている。。」などのコメントもらえますと幸いです。

1.
(a)1つのランドマークの位置しか同定できていないと、ロボットはランドマークから距離Dだけ離れた円周上に存在することはわかるが、円周上のどこにいるかは特定できない。そのため、確度の低い事後分布が求まる。もしも2つ以上のランドマークの位置を同定すると、2つの円の交点上に存在することになり、位置を高精度に特定できる。そのため、確度の高い事後分布が求まる。

Probabilistic Robotics - Chap.8 Mobile Robot Localization: Grid And Monte Carlo

8.1 Introduction

 8章では下記2つのローカライゼーションアルゴリズムについて述べる。

  • グリッドローカライゼーション

 beliefの計算にヒストグラムフィルタを使う。グリッドローカライゼーションは、グリッドを細かくすると、計算負荷が高くなり処理が遅くなるという問題がある。

 パーティクルフィルタを使ってローカライゼーションを行う、有名なアルゴリズム。欠点は色々とあるものの、誘拐ロボット問題や動的環境下への対応が可能。

これら2つのアルゴリズムは、下記の共通する特徴がある。

  1. センサ生値を処理するため、特徴量の抽出といった加工処理が不要
  2. ノンパラメトリック
  3. グローバルローカライゼーションのための手法であり、誘拐ロボット問題に対応可能

 以降では、上記2つのアルゴリズムの詳細を述べる。

8.2 Grid Localization

8.2.1 Basic Algorithm

 グリッドローカライゼーションは、ヒストグラムフィルタを利用してグリッドマップ上でのロボットの事後beliefを近似する。つまり事後beliefは、下記の様に各グリッド{\mathrm{x}_k}での存在確率{p_{k, t}}の集合
{
bel(x_t) = \{ p_{k, t} \} \tag{1}
}
として表せる。また各グリッド{\mathrm{x_k}}の集合であるグリッドマップ{X_t}は、
{
domain(X_t) \  = \  \mathrm{x}_{1, t} \  \cup \  \mathrm{x}_{2, t} \  \cup \  ... \  \cup \mathrm{x}_{k, t} \tag{2}
}
とできる。
 グリッドローカライゼーションの擬似コードを下記に示す。グリッドローカライゼーションは、入力として各グリッドでの存在確率{\{ p_{k, t} \}}と最新の観測結果、制御量、マップ情報を必要とする。そして処理結果として、最新の事後beliefを出力する。疑似コード中の{\boldsymbol{motion\_model}}{\boldsymbol{measurement\_model}}は、Chapter5、6で紹介したどの方法を適用してもよい。

{
1: \  \  \  \boldsymbol{Algorithm Grid\_localization}(\{p_{k, t}\}, u_t, z_t, m): \\
2: \  \  \  \  \  for \  all \  k \  do \\
3: \  \  \  \  \  \  \  \overline{p}_{k, t} = \sum_i p_{i, t-1} \boldsymbol{motion\_model}(mean(\mathrm{x}_k), u_t, mean(\mathrm{x}_i)) \\
4: \  \  \  \  \  \  \  p_{k, t} = \eta \  \overline{p}_{k, t} \boldsymbol{measurement\_model}(z_t, mean(\mathrm{x}_k), m) \\
5: \  \  \  \  \  endfor \\
6: \  \  \  \  \  return \ \{p_{k, t}\}
}

8.2.2 Grid Resolution

 グリッドローカライゼーションにおける重要な変数は、グリッドの解像度である。グリッドの決め方には、トポロジカルな方法とメトリックな方法がある。トポロジカルな方法では、環境を特徴的な領域に分割する。ここで言う特徴的な領域とは、ドアや窓といったランドマークが存在する領域を指す。メトリックな方法では、環境の特性を考慮せずに均一なグリッドに分割する。分割数は、特徴的な領域は少ない場合が多いので、メトリックな方法の方が多い。
 分割数を多くすると計算負荷が高まるため、解像度は粗くなりがちである。粗さへの対応は、センサモデルと運動モデルの工夫が必要である。例えばメトリックな方法でグリッドを切った場合、グリッドが{1m \times 1m}で、ロボットの速度が{1m/s}とすると、運動モデルが頻繁に更新してもグリッドの遷移を検出できない。そのため、運動モデルの更新頻度を下げるといった対応が必要である。

8.2.3 Comuputational Consideration

 ローカライゼーション時の計算負荷を下げる方法は、沢山検討されている。

  • Model pre-caching

 予め特定の計測結果を保存しておき、現在の値とその値を比較することで、瞬時に状態を特定可能にする。

  • Sensor Subsampling

 センサデータを全て処理するのではなく、重要な一部のデータのみを処理する。

  • Delayed Motion Update

 制御や観測を行うよりも低い頻度でbeliefの更新を行う。

  • Selective Updating

 事後確率に対する閾値を設定しておき、閾値を超えているグリッドのみ更新を行う。

8.3 Monte Carlo Localization(MCL)

8.3.2 The MCL Algorithm

 ベーシックなMCLアルゴリズムでは、beliefはパーティクルのセットで表現される。beliefの更新方法は下記の擬似コードの通り。

{
\  1: \  Algorighm \  MCL(\chi_{t-1}, u_t, z_t, m): \\
\  2: \  \quad \overline{chi}_t = \chi_t = \varnothing \\
\  3: \  \quad for \  m = 1 \  to \  M \  do \\
\  4: \  \qquad x^{[m]}_t = \boldsymbol{sample\_motion\_model}(u_t, x^{[m]}_{t-1}) \\
\  5: \  \qquad w^{[m]}_t = \boldsymbol{measurement\_model}(z_t, x^{[m]}_t, m) \\
\  6: \  \qquad \overline{\chi}_t = \overline{\chi}_t + \langle x^{[m]}_t, w^{[m]}_t \rangle \\
\  7: \  \quad endfor \\
\  8: \  \quad for \  m=1 \  to \  M \  do \\
\  9: \  \qquad draw \  i \  with \  probability \  \propto \  w^{[i]}_t \\
10: \  \qquad add \  x^{[i]}_t \  to \  \chi_t \\
11: \  \quad endfor \\
12: \  \quad return \  \chi_t
}

初期beliefは、事前分布{p(x_0)}を元にランダムに生成した{M}個のパーティクルとして得る。この時各パーティクルにはimportance factor \ {M^{-1}}を割り当てる。なお上記擬似コード中の{\boldsymbol{sample\_motion\_model}}{\boldsymbol{measurement\_model}}は、グリッドローカライゼーションと同様に、5章、6章で紹介された方法を摘要できる。

8.3.4 Properties of MCL

 MCLは殆ど全ての実用上重要な分布を近似できる。一般的なパーティクル数{M}をセットするための戦略は、{u_t}{z_t}が更新されるまでリサンプリングしないことである。またMCLの利点として、ノンパラメトリックな近似ができる点がある。

8.3.5 Random Particle MCL: Recovery form Failures

 今までに述べたMCLのアルゴリズムでは、グローバルローカライゼーションは可能だが失敗時に復帰できない、誘拐ロボット問題には対処できない、といった問題がある。それは、パーティクルが一度現在の姿勢と異なる場所に集まると、正しい姿勢へ回復できないためである。
 これら問題へのシンプルな対応策として、ランダムパーティクルの注入する方法がある。これにより、運動モデルの中にランダムな部分空間が出来る。ではどうやってランダムにパーティクルを注入するべきか。ある種の推論を使って行うのがよい。
 1つの方法として、観測モデルを監視し、モデルと平均確率を関連付ける方法がある。(ToDo:よくわからん)平均値は、観測モデルを近似できる。通常数ステップにわたった平均を求めるのがよい。
 観測確率が低い理由は、ローカライゼーションの失敗以外にも、センサノイズが異常に高い、パーティクルが空間上に分散しすぎている(集まっていない)、などがある。
 パーティクスル数を決める時、数ステップの観測尤度の平均を維持し、より多くのステップの平均に関連付けるとよい。

 どの分布を使ってサンプリングするかについての2つ目の問題は、2つの方法で言える。姿勢空間全体を対象とした一様分布を元にパーティクスルをばらまき、現在の観測値を使ってそれらを重み付けする。

 例えばランドマーク検出モデルなどのセンサモデルでは、観測モデルの分布に沿ってパーティクルをばらまくことができる。この場合は尤度の沿ってパーティクルをばらまく。

 下記にランダムランプルのばらまきを考慮したMCLのアルゴリズムを述べる。
{
\  1: \  Algorighm \  MCL(\chi_{t-1}, u_t, z_t, m): \\
\  2: \  \quad static \  w_{slow}, w_{fast} \\
\  3: \  \quad \overline{chi}_t = \chi_t = \varnothing \\
\  4: \  \quad for \  m = 1 \  to \  M \  do \\
\  5: \  \qquad x^{[m]}_t = \boldsymbol{sample\_motion\_model}(u_t, x^{[m]}_{t-1}) \\
\  6: \  \qquad w^{[m]}_t = \boldsymbol{measurement\_model}(z_t, x^{[m]}_t, m) \\
\  7: \  \qquad \overline{\chi}_t = \overline{\chi}_t + \langle x^{[m]}_t, w^{[m]}_t \rangle \\
\  8: \  \qquad w_{avg} \  = \  w_{avg} + \frac{1}{M} w^{[m]}_t \\
\  9: \  \quad endfor \\
  10: \  \quad w_{slow} = w_{slow} + \alpha_{slow}(w_{avg} - w_{slow}) \\
  11: \  \quad w_{fast} = w_{fast} + \alpha_{fast}(w_{avg} - w_{fast}) \\
  12: \  \quad for \  m=1 \  to \  M \  do \\
  13: \  \qquad with \  probability \  max\{0.0, \  1.0 - w_{fast}/w_{slow} \} \  do \\
  14: \  \qquad \quad add \  random \  pose \  to \  \chi_t \\ 
  15: \  \qquad else \\
  16: \  \qquad \quad draw \  i \  \in \  \{1,...,N\} \  with \  probability \  \propto \  w^{[i]}_t \\
  17: \  \qquad \quad add \  x^{[i]}_t \  to \  \chi_t \\
  18: \  \qquad endwith \\
  19: \  \quad endfor \\
  20: \  \quad return \  \chi_t
}
このアルゴリズムは、短期・長期それぞれの平均尤度{p(z_t | z_{1:t-1}}, u_{1:t}, mをトラッキングする点で適応的である。短期・長期の平均尤度は10、11行目で求めている。なお上記{\alpha_{fast}}{\alpha_{slow}}は、短期・長期の平均を求める指数フィルタの減衰率であり、{0 \leq \alpha_{slow} \ll \alpha_{fast}}を満たすものとする。
 このアルゴリズムの肝であるランダムサンプルの追加は、13行目で行っている。13行目に示すように、短期の平均が長期の平均を下回る、つまり[tex:{1.0 - w_{fast}/w_{slow} \rt 0}}の時、ランダムサンプルが追加される。これにより、観測尤度が急に減少すると、その文ランダムサンプルの沢山追加される。

8.3.6 Modifying the Proposal Distribution

 MCLの提案分布は、MCLを非効率なものにする原因の1つである。もしも提案分布と目標分布の差が大きくなれば、精度向上のためにサンプル数を増やす必要がある。提案分布が非効率なことで、ノイズのないセンサの観測結果からロボットの姿勢が寸分の狂いもなくわかるとすると、MCLが失敗することになる。そのためMCLを使う場合、精度の良いセンサよりも悪いセンサを使うことが望ましい。別の方法として、観測モデルに人工的なノイズを乗せるようにてもよい。
 その他の方法として、サンプリングプロセスを改良も考えられる。例えば動作モデルと観測モデルの役割を入れ替える、混合モデルが考えられる。混合モデルでは、観測モデルがパーティクルの生成、重みの更新を行う。混合モデルは、誤差の低減や誘拐ロボット問題へ対応が可能だが、実装には困難が伴うという問題もある。

8.4 Localization in Dynamic Environment

 これまでに述べたローカライゼーション手法の重要な制約は、静的な環境を仮定している。そのため、人などの動体を考慮していない。ただし確率的なアプローチでは、ノイズを考慮しているため、結果としてある程度動体を考慮している。一方で、センサノイズは各ステップ毎に独立していると仮定しているが、実際には数ステップにまたがって影響している。
 こうした動的な環境に対応する方法として、状態拡張(state augmentation)と外れ値の除外(outlier rejection)という2つの基本技術がある。前者を適用することで、ロボットの姿勢だけでなく人の特性(位置、速度など)を推定できるフィルタを定義できる。ただし計算が複雑になると言った問題がある。後者は、センサデータがなぜこの値になったかの原因追求と、モデル化していない動体の影響を受けたと思われる高尤度なデータを排除する。
 この方法は、本質的にはEMアルゴリズムと同じことを行う。ここで、観測モデルとして6.3節の式(6.12)を考え、新たに変数{\overline{c}^k_t} \in \{ hit, short, max, rand \}を導入する。

{
p(z^k_t | x_t, m) = 
\begin{pmatrix}
z_{hit} \\
z_{short} \\
z_{max} \\
z_{rand}
\end{pmatrix}^T

\cdot

\begin{pmatrix}
p_{hit} (z^k_t | x_t, m) \\
p_{short} (z^k_t | x_t, m) \\
p_{max} (z^k_t | x_t, m) \\
p_{rand} (z^k_t | x_t, m)
\end{pmatrix} \tag{6.12) = (8.8}
}

ここで{p_{short}}は、{ p(\overline{c}^k_t \  = \  short \  | \  z^k_t, z_{1:t-1}, u_{1:t}, m)}を示すものとする。

{
\   1: \  Algorithm \  test\_range\_measurement(z^k_t, \hat{\chi}_t, m): \\
\   2: \  \quad  p \  = \  q \  = \  0 \\
\   3: \  \quad  for \  m \  = \  1 \  to \  M \  do \\
\   4: \  \qquad p \  = \  p + z_{short} \cdot p_{short}(z^k_t | x^{[m]}_t, m) \\
\   5: \  \qquad q \  = \  q + z_{hit} \cdot p_{hit}(z^k_t | x^{[m]}_t, m) + z_{short} \cdot p_{short}(z^k_t | x^{[m]}_t, m) + z_{max} \cdot p_{max}(z^k_t | x^{[m]}_t, m) + z_{rand} \cdot p_{rand}(z^k_t | x^{[m]}_t, m) \\
\   6: \  endfor \\
\   7: \  if \  p/q \  \leq \  \chi \  then \\
\   8: \  \qquad return \  accept \\
\   9: \  else \\
   10: \  \qquad return \  reject \\
   11: \  endif
}

 アルゴリズムは上記の通り。入力はbelief{\overline{bel}(x_t)}の代替であるパーティクルの集合{\hat{\chi}_t}とレンジファインダの観測データ{z_t}、地図{m}である。出力は入力した{\hat{\chi}_t}の"reject"か"accept"のどちらか一方である。
 reject/acceptの判断は、観測結果が想定よりも驚くほど短いかどうかで行う。地図上に人などの動体が存在する場合、壁よりも近い場所に何かを検出することになる。そのため、観測結果が想定よりも顕著に短い場合はrejectする。
 外れ値のrejectは、一般的には良い考え方である。測距計以外のセンサでも同様の考え方を使って観測モデルを更新できる。

Probabilistic Robotics - Chap.5 Robot Motion

5.1 Introduction

 モーションモデルはベイズフィルタの予測ステップにおいて肝となる状態遷移確率を構成する。本章では、ロボットの運動はある平面上での移動のみを取り上げる。

5.2 Preliminaries

5.2.1 Kinematic Configuration

 ロボットの姿勢は下記の式で表せるものとする。
{
\begin{pmatrix}
x \\
y \\
\theta
\end{pmatrix}
}
ロボットの向きはよく、{bearing}{heading direction}と呼ばれる。また姿勢から向きを取ったものは{位置}と呼ぶ。

5.2.2 Probabilistic Kinematics

 状態遷移確率は{p(x_t | u_t, x_{t-1}}として表される。{x_t, x_{t-1}}はそれぞれ、時刻{t, t-1}でのロボット状態を、{u_t}は時刻{t}でのモーションコマンドを、それぞれ表すものとする。モーションモデルにおいて、{u_t}はオドメトリを指す場合もある。
 本章では、下記2種類の確率的モーションモデルを取り上げる。

  • 速度モーションモデル

 {u_t = Velocity}なモデル。ただし速度を指令値通り制御することは、ノイズ等の都合上難しいことため、精度は良くないことが多い。

  • オドメトリモーションモデル

 {u_t = odometry}なモデル。速度モデルよりも精度が高いことが多い。
ただしオドメトリは動いた結果わかる情報である。そのため、動作の予測には使えない。そのため、速度モデルはモーションプランニングに、オドメトリモデルは状態推定に、それぞれ使うことが一般的である。以降ではそれぞれについて詳細を述べる。

5.3 Velocity Motion Model

 本モデルは、ロボットの速度{u_t}は並行運動{v_t}と回転運動{\omega_t}の2種類で構成されると過程する。なお{v_t}{\omega_t}は、それぞれ1次元のスカラ値である。

5.3.1 Closed Form Calculation

 速度モデルは、下記のアルゴリズムで表現される。
{
 1: \  \  Algorithm motion\_model\_velocity(x_t, u_t, x_{t-1}):\\
 2: \  \  \  \  \  \mu = \frac{1}{2} \frac{(x - x') cos\theta + (y - y') sin \theta}{(y - y') cos\theta - (x - x') sin \theta}\\
 3: \  \  \  \  \  x^{*} = \frac{x + x'}{2} + \mu (y - y') \\
 4: \  \  \  \  \  y^{*} = \frac{y + y'}{2} + \mu (x' - x) \\
 5: \  \  \  \  \  r^{*} = \sqrt{(x - x^{*})^2 + (y - y^{*})^2} \\
 6: \  \  \  \  \  \Delta \theta = atan2(y' - y^{*}, x' - x^{*}) - atan2(y - y^{*}, x - x^{*}) \\
 7: \  \  \  \  \  \hat{v} = \frac{\Delta \theta}{\Delta t} r^{*} \\
 8: \  \  \  \  \  \hat{\omega} = \frac{\Delta \theta}{\Delta t} \\
 9: \  \  \  \  \  \hat{\gamma} = \frac{\theta' - \theta}{\Delta t} - \hat{\omega} \\
10: \  \  \  \  return \  prob(v - \hat{v}, \alpha_1 v^2 + \alpha_2 \omega^2) \cdot prob(\omega - \hat{\omega}, \alpha_3 v^2 + \alpha_4 \omega^2) \cdot prob(\hat{\gamma}, \alpha_5 v^2 + \alpha_6 \omega^2)
}

ここで{\alpha_1}から{\alpha_6}は、ロボット固有の誤差パラメータでる。また{prob(x, b^2)}は、モーションエラーをモデル化した関数を表す。この固有パラメータの決定方法について、特に本書には記載がないです。恐らく指令値と実測値の組み合わせを記録していき、その結果が良い感じに{prob(x, b^2)}の結果とフィットするようにパラメータチューンが必要と思われます。

5.3.2 Sampling Algorithm

 5.3.1では計算で求めていましたが、本節ではパーティクルフィルタの様にサンプリングを介して解析に状態遷移確率を求める手法を紹介します。サンプル点から求める際の下記の手法を使う。
{
1: \  \  Algorithm sample\_motion\_model\_velocity(u_t, x_{t-1}):\\
2: \  \  \  \  \  \hat{v} = v + \boldsymbol{sample}(\alpha_1 v^2 + \alpha_2 \omega^2) \\
3: \  \  \  \  \  \hat{\omega} = \omega + \boldsymbol{sample}(\alpha_3 v^2 + \alpha_4 \omega^2) \\
4: \  \  \  \  \  \hat{\gamma} = \boldsymbol{sample}(\alpha_5 v^2 + \alpha_6 \omega^2) \\
5: \  \  \  \  \  x^{'} = x - \frac{\hat{v}}{\omega} sin \theta + \frac{\hat{v}}{\omega} sin (\theta + \hat{\omega} \Delta t \\
6: \  \  \  \  \  y^{'} = y + \frac{\hat{v}}{\omega} cos \theta - \frac{\hat{v}}{\omega} cos (\theta + \hat{\omega} \Delta t \\
7: \  \  \  \  \  \theta^{'} = \theta + \hat{\omega} \Delta t + \hat{\gamma} \Delta t \\
8: \  \  \  \  \  return \  x_t = (x^{'}, y^{'}, \theta^{'})^T\\
}
2~4行目は、速度パラメータにノイズを加えます。その結果を使い、5~7行目で姿勢を計算しています。サンプリング手法を使う場合、運動方程式等の物理モデルを考慮せず、サンプル点の位置からのみ姿勢が求まるという利点がある。そのため実装の観点からすると、サンプリング手法を利用ことが楽である。

5.4 Odometry Motion Model

 オドメトリは、ドリフトやスリップによってエラーは生じるものの、速度に比べて計測誤差が小さい。また実際に動いた後でないと計測できないため、モーションプランニングには使えない。

5.4.1 Closed Form Calculation

 オドメトリをベースとしたモーションモデルは、今日の確率ロボティクスベースのシステムで中心的な役割を果たしている。
オドメトリは、ロボット内部の情報量である。そのため、実世界とリンクするためには座標変換が必要である。ただしドリフトやスリップが生じるため、変換行列は一定ではない。なおこの変換行列を求めることは、自己位置推定を行うことと等価である。
 オドメトリモデルは相対動作情報を使用する。相対動作情報とは、時刻{t-1}から{t}の間で、どの程度動いたかを表す情報である。ロボットの時刻{t-1}{t}の時の姿勢のペアを{u_t}とすると、
{
u_t = 
\begin{pmatrix}
\overline{x}_{t-1} \\
\overline{x}_t
\end{pmatrix}
}
となる。
 この動作情報から相対動作情報を取り出すと、回転・直進・回転の3ステップに分割できる。つまり{(
\begin{pmatrix}
\delta_{rot1}
\delta_{trans}
\delta_{rot2}
\end{pmatrix}
}
と表せる。この情報が分かれば、ある時刻{t-1}から{t}の間の移動を再現できる。
確率的モーションモデルでは、上記3パラメータはノイズが加わっているものと仮定する。

以上を踏まえた、オドメトリモデルでの状態遷移確率を求めるアルゴリズムは下記の通りである。
{
1: \  \  \  Algorithm \  motion\_model\_odometry(x_t, u_t, x_{t-1}):\\

2: \  \  \  \  \  \delta_{rot1}  = atan2(\overline{y}^{'} - \overline{y}, \overline{x}^{'} - \overline{x}) - \overline{\theta} \\
3: \  \  \  \  \  \delta_{trans} = \sqrt{(\overline{x}^{'} - \overline{x})^2 + (\overline{y}^{'} - \overline{y})^2} \\
4: \  \  \  \  \  \delta_{rot2} = \overline{\theta}^{'} - \overline{\theta} - \delta_{rot1} \\

5: \  \  \  \  \  \hat{\delta}_{rot1}  = atan2(y^{'} - y, x^{'} - x) - {\theta} \\
6: \  \  \  \  \  \hat{\delta}_{trans} = \sqrt{(x^{'} - x)^2 + (y^{'} - y)^2} \\
7: \  \  \  \  \  \hat{\delta}_{rot2}  = \theta^{'} - \theta - \delta_{rot1} \\

8: \  \  \  \  \  p_1 = \boldsymbol{prob}(\delta_{rot1} - \hat{\delta}_{rot1}, \alpha_1 \hat{\delta}^2_{rot1} + \alpha_2 \hat{\delta}^2_{trans}) \\
9: \  \  \  \  \  p_2 = \boldsymbol{prob}(\delta_{trans} - \hat{\delta}_{trans}, \alpha_3 \hat{\delta}^2_{trans} + \alpha_4 \hat{\delta}^2_{rot1} + + \alpha_4 \hat{\delta}^2_{rot2}) \\
10: \  \  \  \  \  p_3 = \boldsymbol{prob}(\delta_{rot2} - \hat{\delta}_{rot2}, \alpha_1 \hat{\delta}^2_{rot2} + \alpha_2 \hat{\delta}^2_{trans}) \\

11: \  \  \  \  \  return \  p_1 \cdot p_2 \cdot p_3
}
なお{\boldsymbol{prob}(a, b^2)}は、平均{0}で分散{b^2}の誤差分布における{a}の発生確率を返す関数とする。また、{\alpha_1}から{\alpha_4}は、ロボットの動作を特定するロボット固有のパラメータである。

上記アルゴリズムにおいて、各行が行っている内容は下記の通り

  • 2~4行目:オドメトリ情報から相対動作情報{\begin{pmatrix} \delta_{rot1} \delta_{trans} \delta_{rot2} \end{pmatrix} }を抽出。
  • 5~7行目:入力した姿勢{x_{t-1}}{x_t}から理想とする相対動作情報を算出。
  • 8~10行目:相対動作情報の各要素に対するエラーを確率的に計算
  • 11行目:相対動作情報の各要素に対するエラー発生確率をかけ合わせ、目的姿勢である{x_t}からのずれを返す。

5.4.2 Sampling Algorithm

 サンプルベースでオドメトリモデルを求めるアルゴリズムは下記の通りである。{\boldsymbol{sample}(a,b^2}は、{a}の発生確率を返す、平均{0}で分散{b^2}な確率分布関数である。

{
1: \  \  \  Algorithm \  motion\_model\_odometry(x_t, u_t, x_{t-1}):\\

2: \  \  \  \  \  \delta_{rot1}  = atan2(\overline{y}^{'} - \overline{y}, \overline{x}^{'} - \overline{x}) - \overline{\theta} \\
3: \  \  \  \  \  \delta_{trans} = \sqrt{(\overline{x}^{'} - \overline{x})^2 + (\overline{y}^{'} - \overline{y})^2} \\
4: \  \  \  \  \  \delta_{rot2} = \overline{\theta}^{'} - \overline{\theta} - \delta_{rot1} \\

5: \  \  \  \  \  \hat{\delta}_{rot1}  = \delta_{rot1} - \boldsymbol{sample}(\alpha_1 \delta^2_{rot1} + \alpha_2 \delta^2_{trans}) \\
6: \  \  \  \  \  \hat{\delta}_{trans} = \delta_{trans} - \boldsymbol{sample}(\alpha_3 \delta^2_{trans} + \alpha_4 \delta^2_{rot1} + \alpha_4 \delta^2_{rot2}) \\
7: \  \  \  \  \  \hat{\delta}_{rot2}  = \delta_{rot2} - \boldsymbol{sample}(\alpha_1 \delta^2_{rot2} + \alpha_2 \delta^2_{trans}) \\

8: \  \  \  \  \  x^{'} = x + \hat{\delta}_{trans} cos(\theta + \hat{delta}_{rot1}) \\
9: \  \  \  \  \  y^{'} = y + \hat{\delta}_{trans} sin(\theta + \hat{delta}_{rot1}) \\
10: \  \  \  \  \  \theta^{'} = \theta + \hat{\delta}_{rot1} + \hat{\delta}_{rot2} \\

11: \  \  \  \  \  return \  x_t = (x^{'}, y^{'}, \theta^{'})^T
}

5.5 Motion And Maps

 今まで扱っていた状態遷移確率では、周囲環境を考慮していなかった。そこで状態遷移確率のパラメータとして、新たに地図{m}を導入する。地図を知ることで、壁などの到達不可能な場所にいるのか、そうでない到達可能な場所にいるのかが判別できる。これにより、ロボットの姿勢をより精度良く求める事が可能となる。
 地図の導入により、状態遷移確率は{p(x_t | u_t, x_{t-1}, m)}となる。地図を知ることによって、今まで議論してきた状態遷移確率では「遷移可能」としてきた場所に遷移出来なくなるので、
{
p(x_t | u_t, x_{t-1}) \neq p(x_t | u_t, x_{t-1}, m)
}
となる。よって新規なモデルとなることから、マップベースモーションモデルと呼ぶことにする。
 マープベースモデルを閉形式で計算することは難しい。一方で、都合の良いことに計算効率のよい近似方法がある。近似式は下記の通り。
{
p(x_t | u_t, x_{t-1}, m) = \eta \frac{p(x_t | u_t, x_{t-1}) p(x_t | m)}{p(x_t)}
}
ここで{\eta}は正規化係数である。通常{p(x_t)}は一様分布で表される。なお周囲環境は動的に変化しないことから、{m}は定数として考える。もしも{p(x_t | m) = 0}の時は、ロボットは地図上に存在する障害物上に存在することを意味する。
 なお上記近似には問題がある。モデルでは地図を考慮しているため、ロボットが存在できる姿勢は全て遷移対象となる。しかし、例えば壁の向こう側に一瞬で行くのは難しいが、そうした物理的に遷移が可能かは考慮していない。そのため、遷移先の姿勢への移動コスト加味するなどの工夫が必要である。

マハラノビス距離

統計学で用いられる、データ点が平均からどれだけ外れているかを判定するための手法。[2]によると、ユークリッド距離を標準偏差で割った値に該当する。

例えば2次元のデータを評価する時、主成分分析と共に用いる。第一主成分をX軸、第二主成分をY軸とした時、マハラノビス距離は主成分上での距離を指す。マハラノビス距離の算出式は、

{
\sqrt{(\vec{x} - \mu)^T \Sigma^{-1} (\vec{x} - \mu)}
}

である。つまり分散が大きいと距離が小さくなり、分散が小さくなると距離が大きくなる。これにより、外れ値の判定に用いられたりする。

参考までに、下記にマハラノビス距離を用いて乱数データの異常値検知をするサンプルコードを作成しました。


Probabilistic Robotics - Chap.3 Gaussian Filters

3.2 The Kalman Filter

カルマンフィルタは線形ガウスシステムでフィルタリング(つまり余計なものを省くプロセス[1])と予測を行うために開発された。カルマンフィルタは、連続状態空間でのbeliefを実装する。離散状態空間やハイブリッドな空間には対応しない。

時刻{t}でのbelief{\  bel(x_t)}は、ベイズフィルタがマルコフ過程に加えて下記3点を満たす時、平均{\mu_t}と共分散{\sigma_t}ガウス分布で表せる。

【1点目】
状態遷移確率{p(x_t | u_t, x_{t-1})}が線形関数で表わせ、状態がガウスノイズを加味した下記の式で表せる。
{x_t = A_t x_{t-1} + B_t u_t + \epsilon_t \tag{1}}
ここで、{x_t、x_{t-1}}状態ベクトル{u_t}は制御ベクトルを、それぞれ表す。また{A_t}{n \times n}の正方行列で、{n}状態ベクトルの次元を示す。そして{B_t}{n \times m}の行列で、{m}は制御ベクトルの次元を示す。このように行列{A_t、B_t}を導入することで、カルマンフィルタは入力である状態と制御の線形関数として状態遷移を表現する。
なお{\epsilon_t}は、状態遷移の不確かさを表現するためのガウスノイズで、平均は{0}、共分散は{R_t}、次元は状態ベクトルと同じ{n}とする。

{(1)}式を多変量ガウス分布の式に当てはめると、平均{A_t x_{t-1} + B_t u_t}で共分散{R_t}である
{
p(x_t | u_t, x_{t-1}) = det(2 \pi R_t)^{- \frac{1}{2}} exp\{- \frac{1}{2} (x_t - A_t x_{t-1} - B_t u_t)^T R^{-1}_t (x_t - A_t x_{t-1} - B_t u_t)\} \tag{2}
}
として表せる。

【2点目】
計測確率{p(z_t | x_t)}も、状態遷移確率と同様で入力に対して線形とする。また計測データは、ガウスノイズを加味した下記の式で表せる。
{z_t = C_t x_t + \delta_t \tag{3}}
ここで{C_t}{k \times n}の行列で、{k}は計測ベクトル{z_t}の次元とする。{\delta_t}は、平均{0}で共分散{Q_t}の計測ノイズである。計測確率は、{(2)}と同様に多変量ガウス分布の形で、
{
p(z_t | x_t) = det(2 \pi Q_t)^{- \frac{1}{2}} exp\{- \frac{1}{2} (z_t - C_t x_t)^T Q^{-1}_t (z_t - C_t x_t)\} \tag{4}
}
として表せる。

【3点目】
最後に、初期belief {bel(x_0)}は、平均{\mu_0}で共分散{\Sigma_0}ガウス分布
{
bel(x_0) = p(x_0) = det(2 \pi \Sigma_0)^{- \frac{1}{2}} exp\{- \frac{1}{2} (x_0 - \mu_0)^T \Sigma^{-1}_0 (x_0 - \mu_0)\} \tag{5}
}
として表せる。

これら3つの仮定は、{bel(x_t)}ガウス分布で表せることを確認するのに十分である。

3.2.2 The Kalman Filter Algorithm

カルマンフィルタの入力と出力は
入力:{平均\mu_{t-1}、共分散\Sigma_{t-1}のbel(x_{t-1})}と制御ベクトル{u_t}、観測ベクトル{z_t}
出力:{平均\mu_t、共分散\Sigma_tのbel(x_t)}
である。アルゴリズムは下記の数式で表せる。

{
Algorithm \  Kalman\_Filter(\mu_{t-1}, \Sigma_{t-1}, u_t, z_t):\\
\  \  \overline{\mu}_t    = A_t \mu_{t-1} + B_t u_t\\
\  \  \overline{\Sigma}_t = A_t \Sigma_{t-1} A^T_t + R_t\\
\\
\  \  K_t = \overline{\Sigma}_t C^T_t (C_t \overline{\Sigma}_t C^T_t + Q_t)^{-1} \\
\  \  \mu_t    = \overline{\mu}_t + K_t (z_t - C_t \overline{\mu}_t)\\
\  \  \Sigma_t = (I - K_t C_t) \overline{\Sigma}_t\\
\  \  return\  \mu_t, \Sigma_t
}
Table3.1 対象が線形な場合のカルマンフィルタアルゴリズム

なお{K_t}はカルマンゲインと呼ばれ、状態遷移時に観測結果をどの程度考慮するかを調節する値である。

Table3.1の2,3行目は制御を加えた結果の予測過程を、4~6行目は観測結果を元に状態を更新する過程を、それぞれ示している。
それぞれの行に対する私の解釈は、

  • 2行目:制御を加えたことによる平均の変化を予測
  • 3行目:共分散{\Sigma_t}は分散の2乗であるから、{A_t}に対する二次形式になっている
  • 5行目:予測した平均に対し、予測からどれだけずれたか、{z_t - C_t \overline{\mu_t}}を考慮して平均を更新
  • 6行目:共分散を求める式なので、{I - K_t C_t}の部分は必ず正になる。よって{0 \  \leq \  (I - K_t C_t)_i \  \leq \  1}とわかる。このことから、共分散を抑えにかかっていると言える。

下記に私がPythonで書いたカルマンフィルタのコードを載せました。参考になれば。


3.3 The Extended Kalman Filter

3.3.1 Why Liearlization?

システムが線形であることは、現実的にはかなり稀である。そのため、本節では非線形なシステムにカルマンフィルタを適用する方法を検討する。今状態{x_t}と観測結果{z_t}がそれぞれ、
{
x_t = g(u_t, x_{t-1}) + \epsilon_t \\
z_t = h(x_t) + \delta_t
}
で表せるものとする。ここで{g、h}非線形関数とする。
上記2式は、線形カルマンフィルタで使った行列{A_t、B_t}{g}で、行列{C_t}{h}にそれぞれ置き換えた式と言える。つまり拡張カルマンフィルタは、基本的には線形カルマンフィルタと同じと言える。ただ一方で、非線形関数を用いることからbeliefはガウス分布の形を成さなくなる。

3.3.2 Linealization Via Taylor Expansion

EKFの重要なアイデアは、線形化である。{g、h}を線形化すると、EKFはLKFと同様にして振る舞う。
線形化には種々の方法があるが、EKFでは1次のテイラー展開[2]を用いる。よって{g、h}はそれぞれ、

{
g(u_t, x_{t-1}) \approx g(u_t, \mu_{t-1}) + g^{'}(u_t, \mu_{t-1})(x_{t-1} - \mu_{t-1})\\
\  \  \  \  \  \  \  \  \  \  \  \  \  \  \  \  \  \  = g(u_t, \mu_{t-1}) + G_t (x_{t-1} - \mu_{t-1})\\
\\
h(x_t) \approx h(\overline{\mu_t}) + h^{'}(\overline{\mu_t})(x_t - \overline{\mu_t})\\
\  \  \  \  \  \  \  \  \  = h(\overline{\mu_t}) + H_t (x_t - \overline{\mu_t})
}

と近似できる。この結果を使い、状態遷移確率{p(x_t | u_t, x_{t-1})}と観測確率{p(z_t | x_t)}はそれぞれ、

{
p(x_t | u_t, x_{t-1}) \approx det(2 \pi R_t)^{- \frac{1}{2}} exp \{ - \frac{1}{2} [x_t - g(u_t, \mu_{t-1}) - G_t (x_{t-1} - \mu_{t-1} ]^T R^{-1}_t [x_t - g(u_t, \mu_{t-1}) - G_t (x_{t-1} - \mu_{t-1} ] \} \\
\\
p(z_t | x_t) = det(2 \pi R_t)^{- \frac{1}{2}} exp \{- \frac{1}{2} [ z_t - h(\overline{\mu}_t - H_t (x_t - \overline{\mu}_t) ]^T Q^{-1}_t [ z_t - h(\overline{\mu}_t - H_t (x_t - \overline{\mu}_t) ] \}
}

と表すことができる。

{
Algorithm \  Extended_Kalman\_Filter(\mu_{t-1}, \Sigma_{t-1}, u_t, z_t):\\
\  \  \overline{\mu}_t    = g(u_t, \mu_{t-1})\\
\  \  \overline{\Sigma}_t = G_t \Sigma_{t-1} G^T_t + R_t\\
\\
\  \  K_t = \overline{\Sigma}_t H^T_t (H_t \overline{\Sigma}_t H^T_t + Q_t)^{-1} \\
\  \  \mu_t    = \overline{\mu}_t + K_t (z_t - h(\overline{\mu}_t))\\
\  \  \Sigma_t = (I - K_t H_t) \overline{\Sigma}_t\\
\  \  return\  \mu_t, \Sigma_t
}

3.4 The Unscented Kalman Filter(UKF)

UKFも非線形なシステムに対応するカルマンフィルタの1手法。線形回帰を使うことで、統計的に線形化する。回帰に使うサンプル点として、Sigma Point[3]を設定する。Sigma Pointは、平均を中心とし、かつ共分散行列の主軸に対して線対称になるように配置する。平均
{\mu}・共分散{\Sigma}{n}次元のガウス分布については、{2n+1}個のSigma Pointを配置する。配置時のルールは下記の通り。

{
\begin{align}
\chi^{[0]} &= \mu\\
\chi^{[i]} &= \mu + (\sqrt{(n + \lambda \Sigma})_i \  \  \  for\  \  i = 1,...,n  \\
\chi^{[i]} &= \mu - (\sqrt{(n + \lambda \Sigma})_{i-n} \  \  \  for\  \  i = n+1,...,2n  \\
\end{align}
}

ここで、{\lambda = \alpha^2(n + \kappa) - n}で、{\alpha}{\kappa}{\mu}からどの程度離れるかを決めるスケールパラメータである。

また、各{\chi^{[i]}}は2種類の重み{w^{[i]}_m}{w^{[i]}_c}を持つ。{w^{[i]}_m}は平均を、{w^{[i]}_c}ガウス分布の共分散を、それぞれ計算する際に用いる。なお{w^{[i]}_m}{w^{[i]}_c}は下記の式で求める。

{
\begin{align}
w^{[0]}_m &= \frac{\lambda}{n + \lambda} \\
w^{[0]}_c &= \frac{\lambda}{n + \lambda} + (1 - \alpha^2 + \beta) \\
w^{[i]}_m &= w^{[i]}_c = \frac{1}{2(n + \lambda)} \  \  \  for\  \  i = 1,...,2n.
\end{align}
}

ここで{\beta}は、分布に関する追加情報をエンコードするためのパラメータであり、分布がガウス分布であれば{\beta = 2}が最良となる。
 Sigma Pointは、関数{g}を使って写像される。写像後の平均{\mu^{'}}と共分散{\Sigma^{'}}はそれぞれ、
{
Y^{[i]}  = g(\chi^{[i]}) \\

\displaystyle \mu^{'}    = \sum^{2n}_{i=0} w^{[i]}_m Y^{[i]}\\

\Sigma^{'} = w^{[i]}_c (Y^{[i]} - \mu^{'} ) (Y^{[i]} - \mu^{'} )^T
}
として表せる。

3.4.2 The UKF Alogorithm

UKFとEKFの入出力は同じだが、計算負荷はUFKの方が少し高い。また線形システムでは、UFKとLKFは同じ結果を示す。UKFの利点は、ヤコビアンの計算が不要な点である。ヤコビアンの計算には微分が必要であり、時には計算が難しい。そのためヤコビアンの計算を回避できることは計算上好ましい。なおUKFはパーティクルフィルタに似ているが、パーティクルフィルタはサンプル点をランダムに取るのに対し、UKFではサンプル点の取り方が固定という違いがある。

{
Algorithm\  \  Unscented\_Kalman\_Filter(\mu_{t-1}, \Sigma_{t-1}, u_t, z_t):\\

\  \  \  \chi_{t-1} = (\mu_{t-1}  \  \  \  \mu_{t-1} + \gamma \sqrt{\Sigma_{t-1}} \  \  \  \mu_{t-1} - \gamma \sqrt{\Sigma_{t-1}})\\

\  \  \  \overline{\chi}^{*}_t = g(u_t, \chi_{t-1})\\

\  \  \  \overline{\mu}_t = \sum^{2n}_{i=0} w^{[i]}_m \overline{\chi}^{*[i]}_t \\

\  \  \  \overline{\Sigma}_t = \sum^{2n}_{i=0} w^{[i]}_c (\overline{\chi}^{*[i]}_t - \overline{\mu}_t) (\overline{\chi}^{*[i]}_t - \overline{\mu}_t)^T + R_t \\

\  \  \  \overline{\chi}_t = (\overline{\mu}_t  \  \  \  \overline{\mu}_t + \gamma \sqrt{\overline{\Sigma}_t} \  \  \  \overline{\mu}_t - \gamma \sqrt{\overline{\Sigma}_t})\\

\  \  \  \overline{Z}_t = h( \overline{\chi}_t)\\

\  \  \  \hat{z}_t = \sum^{2n}_{i=0} w^{[i]}_m \overline{Z}^{[i]}_t \\

\  \  \  S_t = \sum^{2n}_{i=0} w^{[i]}_c (\overline{Z}^{[i]}_t - \hat{z}_t) (\overline{Z}^{[i]}_t - \hat{z}_t)^T + Q_t \\

\  \  \  \sum^{x,z}_t = \sum^{2n}_{i=0} w^{[i]}_c (\overline{\chi}^{[i]}_t - \overline{\mu}_t) (\overline{Z}^{[i]}_t - \overline{\mu}_t)^T \\

\  \  \  K_t = \sum^{x,z}_t S^{-1}_t \\

\  \  \  \mu_t = \overline{\mu}_t + K_t ( z_t - \hat{z}_t )\\

\  \  \  \Sigma_t = \overline{\Sigma}_t - K_t S_t K_t^T \\

\  \  \  return \  \  \mu_t, \Sigma_t
}

3.5 The Information Filter(IF)

 LKFと同様に線形なシステムを仮定している。LKFとの主な違いは、ガウス分布の表現方法である。LKFとその派生は、ガウス分布をモーメント(平均と共分散)を使って表している。それに対しIFは、カノニカルな表現方法を使用する(*注意:僕、カノニカルとはどういうことかよくわかっていません!調べている途中です。。)。

 IFとLKFは、状態遷移確率と観測確率それぞれを、LKFと同じ式で求める。またLKFと同様に、予測と更新の2ステップで構成される。IFの入力は、カノニカルなパラメータ{\xi_{t-1}と\Omega_{t-1}、u_t、z_t}である。そして出力は{\xi_tと\Omega_t}である。なお{\xiと\Omega}の定義は下記の通り。
{
\begin{align}
\Omega &= \Sigma^{-1} \\
\xi    &= \Sigma^{-1} \mu
\end{align}
}

IFとLKF・その派生を比較した時、下記のようなメリット・デメリットがある。

  • メリット

LKF・その派生よりも数値的に安定している(安定が曖昧だが、他のことばへの言い換えがわからない。。)
LKF・その派生よりもロボットの実問題へ当てはめやすい

  • デメリット

更新時に状態推定をリカバーする必要がある
状態空間の次元が大きくなると計算コストが膨大になる