MatcherNetと拡張カルマンフィルタ

1. 連続時間の非線形ダイナミクスモデルと離散時間化線形化

線形のダイナミクスモデルと観測モデルのペアを、線形状態空間モデルと呼ぶ。

線形の連続系ダイナミクス

$ \dot{x}=Ax

$ x_{t+\Delta t} = Fx_t

また、非線形の連続系ダイナミクス

$ \dot{x}=f(x)

$ A_t = \left. \frac{\partial f(x)}{\partial x}\right|_{x=x_t}

を求めて線形化することにより、

$ \dot{x}=f(x)\approx A_t f(x)

と書くことができる。これを十分に小さな時間幅を用いて離散時間化すると、

$ x_{t+\Delta t} = F_tx_t

連続系ダイナミクスにノイズを加えたモデルは、確率微分方程式 (stochastic differential equation; SDE)と呼ばれる。

$ \dot{x}=f(x)+dw

$ x_{t+\Delta t} = F_t x_t + z_t,

$ z_t\sim \mathcal{N}(0, \Delta t\Sigma_Z)

2. 状態空間モデル

状態の（確率的な）時間発展を定義するダイナミクスモデルと、状態の（確率的な）観測値生成過程を定義する観測モデルのペアを、状態空間モデルと呼ぶ。拡張カルマンフィルタでは、状態空間モデルとして、（確率的な）非線形ダイナミクスモデル

$ \dot{x} = f(x) + dw

と、（確率的な）非線形観測モデル

$ y_t = g( x_t ) + w_t

$ x_{t+\Delta t} = F_t x_t + z_t

$ y_{t} = C_t x_t + w_t

$ A_t = \left. \frac{\partial f(x)}{\partial x}\right|_{x=x_t}, C_t = \left. \frac{\partial g(x)}{\partial x}\right|_{x=x_t},

はそれぞれダイナミクス関数、観測関数に関するヤコビ行列である。

3. 拡張カルマンフィルタ

拡張カルマンフィルタは、微分可能な非線形状態空間モデルを各時刻で局所線形化し、カルマンフィルタとして解くアルゴリズムである。

拡張カルマンフィルタのパラメタ更新アルゴリズム (EKF algorithm)

2. ダイナミクスの局所離散化・線形化を行う

$ A_{t'} = \left. \frac{\partial f(x)}{\partial x}\right|_{x=\mu_{t'}}

$ F_{t'} = \exp( \Delta t A_{t'} ) \approx I + \Delta t A_{t'}

3. 状態にダイナミクスを適用する

$ \bar{\mu}_{t} = F_{t'} \mu_{t'}

$ \bar{\Sigma}_{t'} = \Delta t \Sigma_X + F_{t'} \Sigma_{t'} F_{t'}^T

4. 観測モデルの局所線形化を行う

$ C_t = \left. \frac{\partial g(x)}{\partial x}\right|_{x=\bar{\mu}_{t}}

5. カルマンゲインと観測誤差を計算する

$ S = \Sigma_Y + C_t \bar{\Sigma}_t C_t^T

$ K = \bar{\Sigma}_t C_t^T S_t^{-1}

$ z_t = y_t - g(\bar{\mu}_t)

6. 観測誤差にカルマンゲインを適用する

$ \Delta \mu_t = K_t z_t

$ \Delta \Sigma_t = - K_t C_t \bar{\Sigma}_t

7. 状態を更新する

$ \mu_{t}=\bar{\mu}_{t}+\Delta\mu_t

$ \Sigma_{t} = \bar{\Sigma}_t + \Delta\Sigma_t

8. 1-7を観測毎に繰り返す

MatcherNet は、上記の拡張カルマンフィルタの計算過程を、bundle と matcher に分割することで、拡張カルマンフィルタと等価な更新過程を実現する。具体的には上記アルゴリズム の 1,2,3,7 を bundle で行い、4,5,6 をmatcherで行う。このさい、計算順が変わっていることに注意する。

dynamics bundle の更新

2. ダイナミクスの線形化を行う(EKF algorithm の 2. に対応)

3. 現状態にダイナミクスを適用する(EKF algorithm の 3. に対応)

observation bundle の更新

matcher の更新

2. 観測モデルの線形化を行う(EKF algorithm の 4. に対応)

3. カルマンゲインと観測誤差を計算する(EKF algorithm の 5. に対応)

4. 観測誤差にカルマンゲインを適用する(EKF algorithm の 6. に対応)

4. MatcherNet EKF

MatcherNet による拡張カルマンフィルタの取り扱いを以下のようにさらに拡張し、これを MatcherNet EKF と呼ぶ。

観測に対応する bundle （observation bundle）を複数持つ

ダイナミクスに対応する bundle (dynamics bundle) を複数持つ

matcher は observer と dynamics をつなぐことができるだけでなく、2つの dynamics bundle をつなぐことができるものとする

MatcherNet EKF はこれらの bundle を matcher を介して任意に接続した構造を持つ。

2. 観測誤差を計算する

$ z_t = g_1( \bar{\mu}_{1t} ) - g_2( \bar{\mu}_{2t} )

3. 観測モデルの線形化を行う

$ C_{1t} = \left. \frac{\partial g_1(x_1)}{\partial x_1}\right|_{x_1=\bar{\mu}_{1t}}, C_{2t} = \left. \frac{\partial g_2(x_2)}{\partial x_2}\right|_{x_2=\bar{\mu}_{2t}}

3. カルマンゲインを計算する

$ S = \Sigma_Y + C_{1t} \bar{\Sigma}_{1t} C_{1t}^T + C_{2t} \bar{\Sigma}_{2t} C_{2t}^T

$ K_1 = \bar{\Sigma}_{1t} C_{1t}^T S^{-1}

$ K_2 = \bar{\Sigma}_{2t} C_{2t}^T S^{-1}

4. 観測誤差にカルマンゲインを適用する

$ \Delta \mu_{1t} = -K_{1t} z_t, \Delta \Sigma_{1t} = - K_{1t} C_{1t} \bar{\Sigma}_{1t}

$ \Delta \mu_{2t} = K_{2t} z_t, \Delta \Sigma_{2t} = - K_{2t} C_{2t} \bar{\Sigma}_{2t}

次に MatcherNet EKF において、一般に dynamics bundle は任意個数の matcher と接続することができる。このことは 3章の dynamics bundle の更新アルゴリズムの第一行を以下のように変更することで実現する。

$ \mu_{t}=\bar{\mu}_{t}+\sum_{i\in M_B} \Delta\mu_{i}

$ \Sigma_{t} = \bar{\Sigma}_t + \sum_{i\in M_B} \Delta\Sigma_i

Observation bundle のバリエーション（観測精度と欠測）

$ S = \Sigma_Y + C_{1t} \bar{\Sigma}_{1t} C_{1t}^T + C_{2t} \bar{\Sigma}_{2t} C_{2t}^T

$ K_2 = \bar{\Sigma}_{2t} C_{2t}^T S^{-1}

5. モデルパラメタの更新

$ \mathcal{E} = \frac{1}{2}||z_t||^2

の勾配を求める。パラメタの更新は、勾配法を用いて予測誤差を小さくする方向に向けておこなえばよい。

$ \theta_g \leftarrow \theta_g + \eta \frac{\partial \mathcal{E}}{\partial \theta_g}

$ \theta_f \leftarrow \theta_f + \eta \frac{\partial \mathcal{E}}{\partial \theta_f}

次にこれらの勾配の数値計算は、

$ z_t = g(\mu_t)-y_t

$ \mu_t = \mu_{t-\Delta t} + \Delta t f(\mu_{t-\Delta t})

であることを利用して chain rule を用いると、

$ \frac{\partial \mathcal{E}}{\partial \theta_g}=z_t^T \frac{\partial g(\mu_t)}{\partial \theta_g},

$ \frac{\partial \mathcal{E}}{\partial \theta_f}=z_t^T \frac{\partial g(\mu_t)}{\partial \mu_t}\frac{\partial \mu_t}{\partial \theta_f} = z_t^T \frac{\partial g(\mu_t)}{\partial \mu_t} \frac{\partial f(\mu_{t-\Delta t})}{\partial \theta_f}\Delta t = \Delta t \Delta \mu_t^T \frac{\partial f(\mu_{t-\Delta t})}{\partial \theta_f}

のように計算可能である。

MatcherNetEKF における bundle および matcher の更新アルゴリズム（本ドキュメントの3章）に対して、以下の拡張を施すことによってパラメタ更新を行う。

6. デモンストレーション

画像認識に基づく姿勢推定を含むデモンストレーションを行う。制御は行わない。

https://gyazo.com/bd6b49ae741924a7841f9cb859713e66

6-1. 画像認識に基づく 2次元平面上 N-link robot hand の姿勢推定と、精度評価

N-link robot hand として、2次元平面原点を中心とした多重振り子モデルを与える。

多重振り子の運動方程式の導出は以下がわかりやすい。

6-2. 複数ビューからの画像認識に基づいて姿勢推定を行うデモを行い、ビュー固有の精度変動に対するロバスト性を評価する

6-3. 画像入力を用いたMPC

6-4. 環境変動に対するロバスト性向上を示すデモを行う