拡張カルマンフィルタとヤコビ行列：カルマンフィルタでロボットの位置推定をしてみよう（7）

今回のシリーズでは、ロボットや自動運転車を制御する際に重要な、ロボットや車の正確な位置を得る方法として、カルマンフィルタを用いたロボットの位置推定の方法を紹介しています。

カルマンフィルタでロボットの位置推定をしてみよう！ロボットや自動運転車を制御するときに、ロボットや車の正確な位置を知ることはとても重要です。しかし、ロボットを制御...

前回の記事では、拡張カルマンフィルタによる自己位置推定を行うために、非線形動的システムを状態空間モデルで表す方法を紹介しました。

非線形動的システムの状態空間モデル：カルマンフィルタでロボットの位置推定をしてみよう（６）今回のシリーズでは、ロボットや自動運転車を制御する際に重要な、ロボットや車の正確な位置を得る方法として、カルマンフィルタを用いた...

今回の記事では、実際に拡張カルマンフィルタを用いて移動ロボットの自己位置推定を行うために必要となる、拡張カルマンフィルタの基本式に含まれるヤコビ行列の算出について紹介していきます。

Contents

拡張カルマンフィルタの基本式
ヤコビ行列とは
- 状態モデルのヤコビ行列
- 観測モデルのヤコビ行列
まとめ

拡張カルマンフィルタの基本式

拡張カルマンフィルタは、通常のカルマンフィルタと同じく予測ステップと観測ステップの２つのステップで構成されています。

拡張カルマンフィルタの予測ステップにおける基本式は

$$ \begin{eqnarray} \left\{ \begin{array} \hat{\boldsymbol{x}}_{t|t-1} = f \left( \hat{\boldsymbol{x}}_{t-1|t-1} , \boldsymbol{u}_t \right) \\ \hat{\boldsymbol{P}}_{t|t-1} = \boldsymbol{F}_t \hat{\boldsymbol{P}}_{t-1|t-1} {\boldsymbol{F}_t}^T + \boldsymbol{Q}_t \end{array} \right. \end{eqnarray} $$

の２式です。

拡張カルマンフィルタの更新ステップにおける基本式は

$$ \begin{eqnarray} \left\{ \begin{array} \tilde{\boldsymbol{y}}_t = \boldsymbol{z}_t – h \left(\hat{\boldsymbol{x}}_{t|t-1} \right) \\ \boldsymbol{S}_t = \boldsymbol{H}_t \boldsymbol{P}_{t|t-1} {\boldsymbol{H}_t}^T + \boldsymbol{R}_t \\ \boldsymbol{K}_t = \boldsymbol{P}_{t|t-1} {\boldsymbol{H}_t}^T {\boldsymbol{S}_t}^{-1} \\ \hat{\boldsymbol{x}}_{t|t} = \hat{\boldsymbol{x}}_{t|t-1} + \boldsymbol{K}_t \tilde{\boldsymbol{y}}_t \\ \hat{\boldsymbol{P}}_{t|t} = \left( \boldsymbol{I} – \boldsymbol{K}_t \boldsymbol{H}_t \right) \boldsymbol{P}_{t|t-1} \end{array} \right. \end{eqnarray} $$

の５式です。

この拡張カルマンフィルタの基本式には、状態方程式で用いている非線形関数fのヤコビアンで定義した行列F_tと出力方程式で用いられている非線形関数hのヤコビアンで定義した行列H_tの２つのヤコビ行列（ヤコビアン）が含まれています。

拡張カルマンフィルタを用いるために、このF_tとH_tの２つのヤコビ行列を求める必要があります。

ヤコビ行列とは

まず、ヤコビ行列（ヤコビアン）について説明していきます。

入力ベクトルxと入力により定まる出力ベクトルyを

$$ \begin{eqnarray} \boldsymbol{x} &=& \left( x_1, x_2, \cdots, x_n \right)^T \\ \boldsymbol{y} &=& \left( y_1, y_2, \cdots, y_m \right)^T \end{eqnarray} $$

と定義します。

この入力xのi番目であるx_iと出力yのj番目であるy_jについて、y_jをx_iで偏微分したものを

$$ \frac{\partial y_j}{\partial x_i} $$

と表します。

この偏微分をすべての組み合わせに対して行い、行列式Jの形でまとめると

$$ \boldsymbol{J} = \begin{bmatrix} \frac{\partial y_1}{\partial x_1} & \frac{\partial y_1}{\partial x_2} & \cdots & \frac{\partial y_1}{\partial x_n} \\ \frac{\partial y_2}{\partial x_1} & \frac{\partial y_2}{\partial x_2} & \cdots & \frac{\partial y_2}{\partial x_n} \\ \vdots & \vdots & \ddots & \vdots \\ \frac{\partial y_m}{\partial x_1} & \frac{\partial y_m}{\partial x_2} & \cdots & \frac{\partial y_m}{\partial x_n} \\ \end{bmatrix} $$

と表すことが出来ます。

この行列式Jをヤコビ行列式またはヤコビアンと言います。

状態モデルのヤコビ行列

予測ステップの状態方程式に含まれる非線形関数fは

$$ \begin{eqnarray} x_{t+1} &=& x_t + v \Delta t \cos \left(\theta_t + \omega \Delta t / 2\right) \\ y_{t+1} &=& y_t + v \Delta t \sin \left(\theta_t + \omega \Delta t / 2\right) \\ \theta_{t+1} &=& \theta_t + \omega \Delta t \end{eqnarray} $$

と表されます。

この非線形関数fの入力ベクトルは[x_t,y_t,θ_t]、出力ベクトルは[x_t+1,y_t+1,θ_t+1]となります。

ここで非線形関数fの中に含まれる速度vと角速度ωを求める式は

$$ \begin{eqnarray} v &=& (v_R+v_L)/2 \\ \omega &=& (v_R-v_L)/2d \end{eqnarray} $$

であるため、ロボットの状態変数[x,y,θ]は含まれていません。

よって、非線形関数fより求められるヤコビ行列式F_tは

$$ \boldsymbol{F}_t = \begin{bmatrix} 1 & 0 & – v \Delta t \sin \left(\theta_t + \omega \Delta t / 2\right) \\ 0 & 1 & -v \Delta t \cos \left(\theta_t + \omega \Delta t / 2\right) \\ 0 & 0 & 1 \end{bmatrix} $$

と求めることが出来ます。

観測モデルのヤコビ行列

更新ステップの観測方程式に含まれる非線形関数hは

$$ \begin{eqnarray} z_t (1) &=& \sqrt{({LM}_X-x_t)^2+({LM}_Y-y_t)^2} + v(1) \\ z_t (2) &=& \arctan{\left( \frac{{LM}_Y-y_t}{{LM}_X-x_t} \right)} – \theta_t + v(2) \end{eqnarray} $$

と表されます。

ここで、センサ出力zの１番目の成分z(1)は距離l_t、２番目の成分z(2)は角度β_tを表しています。

また、LM_XとLM_Yはそれぞれマーカー位置のX軸成分とY軸成分を表しています。

この時、非線形関数hの入力ベクトルは[x_t,y_t,θ_t]、出力ベクトルは[l_t,β_t]となります。

よって、非線形関数hより求められるヤコビ行列式H_tは

$$ \boldsymbol{H}_t = \begin{bmatrix} – \frac{{LM}_X-x_t}{\sqrt{({LM}_X-x_t)^2+({LM}_Y-y_t)^2}} & – \frac{{LM}_Y-y_t}{\sqrt{({LM}_X-x_t)^2+({LM}_Y-y_t)^2}} & 0 \\ – \frac{{LM}_Y-y_t}{\left({LM}_Y-y_t\right)^2+\left({LM}_X-x_t\right)^2} & – \frac{{LM}_X-x_t}{\left({LM}_Y-y_t\right)^2+\left({LM}_X-x_t\right)^2} & -1 \end{bmatrix} $$

と求めることが出来ます。