ベイズフィルタの導出（ロボットと環境との相互作用について）

公開:
確率 ロボティクス #ロボティクス #ベイズフィルタ

『Probabilistic Robotics』 の Chapter2. Recursive State Estimation についてのメモです。確率的にロボティクスを扱う場合に必要となる基本的な用語の整理から、ベイズフィルタの導出までをまとめました。

用語の整理

ロボットと環境に影響を与える全ての集合として状態（state）を定義します。状態は

xtx_t

と表します。具体的には、ロボットの姿勢や壁・障害物の位置など、現実世界に存在する物体に関しての情報をまとめて “状態” として扱います。 ここで問題となるのが、センサーの誤差などによってロボットは真の状態 xtx_t を知ることができないという点です。 そのため現在の状態（例えば自分自身の姿勢）は

p(xt)p(x_t)

のように確率的に広がる分布として状態を捉えることになります。

ロボットと環境との相互作用には

  • 観測（measurement）
  • 制御（control）

があります。状態 xt−1x_{t-1} を観測することで zt−1z_{t-1} を取得し、その情報を元に制御を行って次の状態へ遷移するという流れです。

図の説明

確率的な取り扱い

真の状態 xtx_t は、過去の観測値 zz 、制御 uu 、過去の状態 xx を用いて

p(zt∣x0:t,z1,t−1,u1:t)=p(zt∣xt)p(z_t | x_{0:t}, z_{1,t-1}, u_{1:t}) = p(z_t | x_t)

条件付き確率分布として表されます。

センサー値 ztz_t を使って xtx_t を推定したりします。状態は

p(xt∣x0:t−1,z1:t−1,u1:t)=p(xt∣xt−1,ut)p(x_t | x_{0:t-1}, z_{1:t-1}, u_{1:t}) = p(x_t | x_{t-1}, u_t)

と表すことができます。等式は、状態が完全であるため xt−1x_{t-1} が過去の情報をすべて要約していることを用いています。

  • 遷移確率 p(xt∣xt−1,ut)p(x_t | x_{t-1}, u_t)
  • 状態の確率

Belief Distributions

ここまでで議論してきたように状態 xtx_t は直接観測できないため、これまでの制御履歴、センサー値のデータを用いて現在の状態を推測する必要があります。現在までのすべての観測値、制御値が与えられたときに状態がどうなっているかを表現するための条件付き確率分布を belief distribution（信念分布）と呼び、

bel(xt)=p(xt∣z1:t,u1:t)bel(x_t) = p(x_t | z_{1:t}, u_{1:t})

と表します。 取得できるセンサー値からロボットの姿勢を推測したりすることで「現状の姿勢はこうなっているはず」と信じていることを表現しているので belief（信念）と呼びます。 どういった確率分布を想定するかは基本的には自由で、ガウス分布などがよく用いられます。

belief では制御が終わり ztz_t を観測したタイミングでの表現でしたが（条件で使用している時間幅が [1,t][1, t]）、制御 utu_t によってどういった状態に遷移するかを想定することもでき、

bel(xt)‾=p(xt∣z1:t−1,u1:t)\overline{bel(x_t)} = p(x_t | z_{1:t-1}, u_{1:t})

と計算できます。この場合には belief ではなく、prediction （予測）と呼ばれます。

ベイズフィルタ

状態推定のためのアルゴリズムとして広く使用されているベイズフィルタと呼ばれる考え方について導入します。この考え方を元にしてカルマンフィルタ、ヒストグラムフィルタ、離散ベイズフィルタなどが実装されています。

アルゴリズム概要

ベイズフィルタはロボットの状態推定を再帰的に行うための最も基本的な枠組みで、前時刻までの情報を用いることで現時刻の状態を逐次的に予測するために用います。使用する情報は

  • 前時刻の belief：bel(xt−1)bel(x_{t-1})
  • 最新の制御：utu_t
  • 最新の観測：ztz_t

で、これらを用いることで現時刻の状態（belief）を予測します。このようにベイズフィルタでは、前時刻の信念から bel(xt−1)bel(x_{t-1}) から現時刻の信念 bel(xt)bel(x_t) を逐次的に（再帰的に）計算する構造になっています。 アルゴリズムは大きく2ステップに分かれています。

ベイズフィルタは、最終的な更新式だけを見ると

bel‾(xt)=∫p(xt∣xt−1,ut) bel(xt−1) dxt−1\overline{bel}(x_t) = \int p(x_t \mid x_{t-1}, u_t)~bel(x_{t-1})~dx_{t-1} bel(xt)=η p(zt∣xt) bel‾(xt)bel(x_t) = \eta ~ p(z_t \mid x_t)~\overline{bel}(x_t)

という二式に集約されます。以下ではこれらを導出していきたいと思います。

measurement update

現在時刻 tt において本当に欲しいのは、観測列 z1:tz_{1:t} と制御列 u1:tu_{1:t} が与えられたもとでの状態 xtx_t の事後分布で、これはそのまま belief とみなしてよく、

bel(xt)=p(xt∣z1:t,u1:t)bel(x_t) = p(x_t \mid z_{1:t}, u_{1:t})

と書けます。ベイズフィルタの目的は、この現在時刻の belief を前時刻の belief

bel(xt−1)=p(xt−1∣z1:t−1,u1:t−1)bel(x_{t-1}) = p(x_{t-1} \mid z_{1:t-1}, u_{1:t-1})

から再帰的に求めることにあります。つまり何らかの形で

bel(xt)∝bel(xt−1)bel(x_t) \propto bel(x_{t-1})

という関係を導くのがゴールです。

事後分布に直接ベイズ則を適用すると

p(xt∣z1:t,u1:t)=p(zt∣xt,z1:t−1,u1:t) p(xt∣z1:t−1,u1:t)p(zt∣z1:t−1,u1:t)p(x_t \mid z_{1:t}, u_{1:t}) = \frac{p(z_t \mid x_t, z_{1:t-1}, u_{1:t})~p(x_t \mid z_{1:t-1}, u_{1:t})}{p(z_t \mid z_{1:t-1}, u_{1:t})}

のように展開できます。分母は xtx_t に依存しない正規化定数なので、

p(xt∣z1:t,u1:t)=η p(zt∣xt,z1:t−1u1:t) p(xt∣z1:t−1,u1:t)p(x_t \mid z_{1:t}, u_{1:t}) = \eta~p(z_t \mid x_t, z_{1:t-1} u_{1:t})~p(x_t \mid z_{1:t-1}, u_{1:t})

と書けます。

complete state の仮定

ここで状態 xtx_t が complete であることを仮定します。complete state とは、xtx_t が未来予測に必要な過去情報をすべて要約している状態です。 したがって、もし今の状態 xtx_t が与えられているなら、現在の観測 ztz_t を予測するために過去の観測列 z1:t−1z_{1:t-1} や制御列 u1:tu_{1:t} を追加で参照する必要はなく、現在の状態 xtx_t のみを参照すれば良いことになります。

この仮定は、条件付き独立

p(zt∣xt,z1:t−1,u1:t)=p(zt∣xt)p(z_t \mid x_t, z_{1:t-1}, u_{1:t}) = p(z_t \mid x_t)

として表されます。これを先ほどの式に代入すると、

bel(xt)=p(xt∣z1:t,u1:t)=η p(zt∣xt,z1:t−1u1:t) p(xt∣z1:t−1,u1:t)=η p(zt∣xt) p(xt∣z1:t−1,u1:t)\begin{aligned} bel(x_t) &=p(x_t \mid z_{1:t}, u_{1:t}) \\ &= \eta~p(z_t \mid x_t, z_{1:t-1} u_{1:t})~p(x_t \mid z_{1:t-1}, u_{1:t}) \\ &= \eta~p(z_t \mid x_t)~p(x_t \mid z_{1:t-1}, u_{1:t}) \end{aligned}

となります。 ここで右辺の第二因子は、predict と呼ばれる量であり

bel‾(xt)=p(xt∣z1:t−1,u1:t)\overline{bel}(x_t) = p(x_t \mid z_{1:t-1}, u_{1:t})

と置けば、

bel(xt)=η p(zt∣xt) bel‾(xt)bel(x_t) = \eta ~ p(z_t \mid x_t) ~ \overline{bel}(x_t)

が得られます。予測分布 bel‾(xt)\overline{bel}(x_t) を ztz_t の観測によって更新しているため、measurement update と呼ばれます。

prediction step

先ほど導出した予測分布を1時刻前の belief から求めることができれば、

bel(xt)=η p(zt∣xt) bel‾(xt)∝bel(xt−1)bel(x_t) = \eta ~ p(z_t \mid x_t) ~ \overline{bel}(x_t) \propto bel(x_{t-1})

と、欲しい式形式になります。

まず、全確率の法則を用いて xt−1x_{t-1} で周辺化し、連鎖率を用いると

p(xt∣z1:t−1,u1:t)=∫p(xt,xt−1∣z1:t−1,u1:t) dxt−1=∫p(xt∣xt−1,z1:t−1,u1:t) p(xt−1∣z1:t−1,u1:t) dxt−1\begin{aligned} p(x_t \mid z_{1:t-1}, u_{1:t}) &= \int p(x_t, x_{t-1} \mid z_{1:t-1}, u_{1:t})~dx_{t-1} \\ &= \int p(x_t \mid x_{t-1}, z_{1:t-1}, u_{1:t})~p(x_{t-1} \mid z_{1:t-1}, u_{1:t})~dx_{t-1} \end{aligned}

を得ます。

ここでも complete state 仮定を使うと、現在状態 xtx_t の生成に過去の観測列や古い制御列は直接効かず、xt−1x_{t-1} と現在の入力 utu_t だけを見れば十分だと考えられるので

p(xt∣xt−1,z1:t−1,u1:t)=p(xt∣xt−1,ut)p(x_t \mid x_{t-1}, z_{1:t-1}, u_{1:t}) = p(x_t \mid x_{t-1}, u_t)

となります。 ここで重要なのは、utu_t は残るという点です。これは utu_t が xt−1x_{t-1} より前の過去情報ではなく、xt−1→xtx_{t-1} \to x_t の遷移のために必要な現在の入力だからです。 これにより、予測分布は

bel‾(xt)=p(xt∣z1:t−1,u1:t)=∫p(xt∣xt−1,ut) p(xt−1∣z1:t−1,u1:t) dxt−1\begin{aligned} \overline{bel}(x_t) & = p(x_t \mid z_{1:t-1}, u_{1:t}) \\ &= \int p(x_t \mid x_{t-1}, u_t) ~ p(x_{t-1} \mid z_{1:t-1}, u_{1:t}) ~ dx_{t-1} \end{aligned}

となります。

制御のランダム性

ここでもう一つ、制御がランダムに選ばれる、という仮定を置きます。 この仮定の役割は、utu_t が前時刻状態 xt−1x_{t-1} に関する追加情報を持たないようにすることにあり、

p(xt−1∣z1:t−1,u1:t)=p(xt−1∣z1:t−1,u1:t−1)p(x_{t-1} \mid z_{1:t-1}, u_{1:t}) = p(x_{t-1} \mid z_{1:t-1}, u_{1:t-1})

と書けます。 これを代入すると、

bel‾(xt)=p(xt∣z1:t−1,u1:t)=∫p(xt∣xt−1,ut) p(xt−1∣z1:t−1,u1:t) dxt−1=∫p(xt∣xt−1,ut) p(xt−1∣z1:t−1,u1:t−1) dxt−1\begin{aligned} \overline{bel}(x_t) & = p(x_t \mid z_{1:t-1}, u_{1:t}) \\ &= \int p(x_t \mid x_{t-1}, u_t) ~ p(x_{t-1} \mid z_{1:t-1}, u_{1:t}) ~ dx_{t-1} \\ &= \int p(x_t \mid x_{t-1}, u_t) ~ p(x_{t-1} \mid z_{1:t-1}, u_{1:t-1}) ~ dx_{t-1} \end{aligned}

となります。ここで積分計算内の第二因子は

bel(xt−1)=p(xt−1∣z1:t−1,u1:t−1)bel(x_{t-1}) = p(x_{t-1} \mid z_{1:t-1}, u_{1:t-1})

なので

bel‾(xt)=∫p(xt∣xt−1,ut) bel(xt−1) dxt−1\overline{bel}(x_t) = \int p(x_t \mid x_{t-1}, u_t) ~ bel(x_{t-1}) ~ dx_{t-1}

を得ます。これが prediction step です。

まとめ

以上の導出をまとめると、ベイズフィルタは

bel‾(xt)=∫p(xt∣xt−1,ut) bel(xt−1) dxt−1\overline{bel}(x_t) = \int p(x_t \mid x_{t-1}, u_t)~bel(x_{t-1})~dx_{t-1} bel(xt)=η,p(zt∣xt),bel‾(xt)bel(x_t) = \eta,p(z_t \mid x_t),\overline{bel}(x_t)

という二段階の処理として表されます。 第一式は prediction step であり、前時刻の belief を運動モデル p(xt∣xt−1,ut)p(x_t \mid x_{t-1}, u_t) によって前向きに伝播しています。 第二式は measurement update であり、新しい観測 ztz_t のもとで各状態仮説を尤度 p(zt∣xt)p(z_t \mid x_t) によって再重み付けしています。

ここまで見てきたように、導出の背後には

  • ベイズ則
  • 周辺化
  • 条件付き独立
  • complete state 仮定

という、確率ロボティクスの中核概念がすべて詰まっています。 この二式をただの更新公式としてではなく、情報の流れとして理解できるようになると、カルマンフィルタやパーティクルフィルタなど、その後に出てくる多くの手法も同じ骨格の上に立っていることが見えてきます。