姿勢推定のためのEKF【6軸IMU】 : LiLaBoC

姿勢推定のためのEKF（拡張カルマンフィルター）についてまとめます。

姿勢推定のためのEKF【6軸IMU】

ロボットなどの姿勢を推定するために、EKFがよく使われます。

この記事では、6軸IMU（3軸角速度＋3軸加速度）を想定したEKFについてまとめていきます。

表記ルール
モデル
EKF（拡張カルマンフィルタ）
- 予測推定
- 観測更新
補足（別モデル）
- 導出：重力ベクトル→姿勢角
- 符号に注意！
LaTexによる表記
さいごに

表記ルール

この記事では以下のルールで表記を行います。

細字の小文字（\(a\)）：スカラー
太字の小文字（\(\boldsymbol{a}\)）：ベクトル
太字の大文字（\(\boldsymbol{A}\)）：行列
下付き文字（\(a_k\)）：時間ステップ
\(S_a, C_a, T_a\)：\(\sin{a}, \cos{a}, \tan{a}\)

モデル

モデルを状態方程式と観測方程式で表します。

状態ベクトル

状態ベクトル\(\boldsymbol{x}\)を示します。\(\phi\)、\(\theta\)、\(\psi\)は、それぞれロボットの姿勢角ロール、ピッチ、ヨーです。

\[ \begin{equation} \boldsymbol{x} = \begin{pmatrix} \phi \cr \theta \cr \psi \end{pmatrix} \end{equation} \]

入力ベクトル

状態ベクトル\(\boldsymbol{u}\)を示します。\(\boldsymbol{\omega}\)はセンサで計測される角速度、\(\Delta t\)はステップ時間です。

\[ \begin{equation} \boldsymbol{u} = \begin{pmatrix} u_{\mathrm{x}} \cr u_{\mathrm{y}} \cr u_{\mathrm{z}} \end{pmatrix} = \boldsymbol{\omega} \Delta t = \begin{pmatrix} \omega_{\mathrm{x}} \Delta t \cr \omega_{\mathrm{y}} \Delta t \cr \omega_{\mathrm{z}} \Delta t \end{pmatrix} \end{equation} \]

状態方程式

状態方程式を示します。\(\boldsymbol{Rot}^\mathrm{rpy}\)は姿勢角の回転行列です。

\[ \begin{equation} \begin{split} \boldsymbol{x}_{k} &= f_{(\boldsymbol{x}_{k-1}, \boldsymbol{u}_{k-1})} + \boldsymbol{w}_{k-1} \\&= \boldsymbol{x}_{k-1} + \boldsymbol{Rot}^\mathrm{rpy}_{(\boldsymbol{x}_{k-1})} \boldsymbol{u}_{k-1} + \boldsymbol{w}_{k-1} \\&= \begin{pmatrix} \phi_{k-1} \cr \theta_{k-1} \cr \psi_{k-1} \end{pmatrix} + \begin{pmatrix} 1 & S_{\phi_{k-1}} T_{\theta_{k-1}} & C_{\phi_{k-1}} T_{\theta_{k-1}} \cr 0 & C_{\phi_{k-1}} & -S_{\phi_{k-1}} \cr 0 & S_{\phi_{k-1}} / C_{\theta_{k-1}} & C_{\phi_{k-1}} / C_{\theta_{k-1}} \end{pmatrix} \begin{pmatrix} u_{\mathrm{x}_{k-1}} \cr u_{\mathrm{y}_{k-1}} \cr u_{\mathrm{z}_{k-1}} \end{pmatrix} + \boldsymbol{w}_{k-1} \\&= \begin{pmatrix} \phi_{k-1} + u_{\mathrm{x}_{k-1}} + u_{\mathrm{y}_{k-1}} S_{\phi_{k-1}} T_{\theta_{k-1}} + u_{\mathrm{z}_{k-1}} C_{\phi_{k-1}} T_{\theta_{k-1}} \cr \theta_{k-1} + u_{\mathrm{y}_{k-1}} C_{\phi_{k-1}} - u_{\mathrm{z}_{k-1}} S_{\phi_{k-1}} \cr \psi_{k-1} + u_{\mathrm{y}_{k-1}} S_{\phi_{k-1}} / C_{\theta_{k-1}} + u_{\mathrm{z}_{k-1}} C_{\phi_{k-1}} / C_{\theta_{k-1}} \end{pmatrix} + \boldsymbol{w}_{k-1} \end{split} \end{equation} \]

ここで\(\boldsymbol{w}\)は、平均0、共分散\(\boldsymbol{Q}\)の正規分布に従うノイズです。

\[ \begin{equation} \boldsymbol{w}_{k-1} \sim N(0, \boldsymbol{Q}_{k-1}) \end{equation} \]

観測ベクトル

観測ベクトル\(\boldsymbol{z}\)を示します。\(\boldsymbol{a}\)はセンサで計測される加速度です。

\[ \begin{equation} \boldsymbol{z} = \boldsymbol{a} = \begin{pmatrix} a_x \cr a_y \cr a_z \end{pmatrix} \end{equation} \]

観測方程式

観測方程式を示します。\(\boldsymbol{Rot}^\mathrm{xyz}\)はベクトル回転行列、\(\boldsymbol{g}\)は重力ベクトルです。

\[ \begin{equation} \begin{split} \boldsymbol{y}_k &= h_{(\boldsymbol{x}_k)} + \boldsymbol{v}_k \\&= \boldsymbol{Rot}^\mathrm{xyz}_{(\boldsymbol{x}_{k})} \boldsymbol{g} + \boldsymbol{v}_k \\&= \begin{pmatrix} C_{\theta_k}C_{\psi_k} & C_{\theta_k}S_{\psi_k} & -S_{\theta_k} \cr S_{\phi_k}S_{\theta_k}C_{\psi_k} - C_{\phi_k}S_{\psi_k} & S_{\phi_k}S_{\theta_k}S_{\psi_k} + C_{\phi_k}C_{\psi_k} & S_{\phi_k}C_{\theta_k} \cr C_{\phi_k}S_{\theta_k}C_{\psi_k} + S_{\phi_k}S_{\psi_k} & C_{\phi_k}S_{\theta_k}S_{\psi_k} - S_{\phi_k}C_{\psi_k} & C_{\phi_k}C_{\theta_k} \end{pmatrix} \begin{pmatrix} 0 \cr 0 \cr g \end{pmatrix} + \boldsymbol{v}_k \\&= \begin{pmatrix} -g S_{\theta_k}\cr g S_{\phi_k}C_{\theta_k} \cr g C_{\phi_k}C_{\theta_k} \end{pmatrix} + \boldsymbol{v}_k \end{split} \end{equation} \]

ここで\(\boldsymbol{v}\)は、平均0、共分散\(\boldsymbol{R}\)の正規分布に従うノイズです。

\[ \begin{equation} \boldsymbol{v}_k \sim N(0, \boldsymbol{R}_k) \end{equation} \]

EKF（拡張カルマンフィルタ）

予測推定と観測更新を繰り返すことで、状態ベクトル\(\boldsymbol{x}\)と共分散行列\(\boldsymbol{P}\)を更新します。

予測推定

予測推定で行う計算を示します。

\[ \begin{equation} \bar{\boldsymbol{x}_{k}} = f_{(\boldsymbol{x}_{k-1}, \boldsymbol{u}_{k-1})} \end{equation} \] \[ \begin{equation} \bar{\boldsymbol{P}_{k}} = {\boldsymbol{J_f}}_{k-1} \boldsymbol{P}_{k-1} {\boldsymbol{J_f}}_{k-1}^{\mathrm{T}} + \boldsymbol{Q}_{k-1} \end{equation} \]

ここで、\(\boldsymbol{J_f}\)は状態方程式のヤコビアンです。

\[ \begin{equation} \begin{split} {\boldsymbol{J_f}}_{k-1} &= \left. \frac{\partial f}{\partial \boldsymbol{x}} \right|_{\boldsymbol{x}_{k-1}, \boldsymbol{u}_{k-1}} \\&= \begin{pmatrix} 1 + u_{\mathrm{y}_{k-1}} C_{\phi_{k-1}} T_{\theta_{k-1}} - u_{\mathrm{z}_{k-1}} S_{\phi_{k-1}} T_{\theta_{k-1}} & u_{\mathrm{y}_{k-1}} S_{\phi_{k-1}} / C^2_{\theta_{k-1}} + u_{\mathrm{z}_{k-1}} C_{\phi_{k-1}} / C^2_{\theta_{k-1}} & 0 \cr -u_{\mathrm{y}_{k-1}} S_{\phi_{k-1}} - u_{\mathrm{z}_{k-1}} C_{\phi_{k-1}} & 1 & 0 \cr u_{\mathrm{y}_{k-1}} C_{\phi_{k-1}} / C_{\theta_{k-1}} - u_{\mathrm{z}_{k-1}} S_{\phi_{k-1}} / C_{\theta_{k-1}} & u_{\mathrm{y}_{k-1}} S_{\phi_{k-1}} S_{\theta_{k-1}} / C^2_{\theta_{k-1}} + u_{\mathrm{z}_{k-1}} C_{\phi_{k-1}} S_{\theta_{k-1}} / C^2_{\theta_{k-1}} & 1 \end{pmatrix} \end{split} \end{equation} \]

観測更新

観測更新で行う計算を示します。

\[ \begin{equation} \hat{\boldsymbol{x}_k} = \boldsymbol{x}_k + \boldsymbol{K}_k (\boldsymbol{z}_k - h_{(\boldsymbol{x}_k)}) \end{equation} \] \[ \begin{equation} \hat{\boldsymbol{P}_k} = (\boldsymbol{I} - \boldsymbol{K}_k {\boldsymbol{J_h}}_k)\boldsymbol{P}_k \end{equation} \]

ここで、\(\boldsymbol{K}\)はカルマンゲイン、\(\boldsymbol{J_h}\)は観測方程式のヤコビアンです。

\[ \begin{equation} \boldsymbol{K}_k = \boldsymbol{P}_k {\boldsymbol{J_h}}_k^{\mathrm{T}} ({\boldsymbol{J_h}}_k \boldsymbol{P}_k {\boldsymbol{J_h}}_k^{\mathrm{T}} + \boldsymbol{R}_k)^{-1} \end{equation} \] \[ \begin{equation} {\boldsymbol{J_h}}_k = \left. \frac{\partial h}{\partial \boldsymbol{x}} \right|_{\boldsymbol{x}_k} = \begin{pmatrix} 0 & -g C_{\theta_k} & 0 \cr g C_{\phi_k}C_{\theta_k} & -g S_{\phi_k}S_{\theta_k} & 0 \cr -g S_{\phi_k}C_{\theta_k} & -g C_{\phi_k}S_{\theta_k} & 0 \end{pmatrix} \end{equation} \]

補足（別モデル）

上記では加速度を直接観測するモデルを紹介しましたが、以下のように、加速度から算出される姿勢角を観測するモデルもよく見かけます。

\[ \begin{equation} \boldsymbol{z} = \begin{pmatrix} \phi \cr \theta \end{pmatrix} = \begin{pmatrix} \tan^{-1}{\frac{a_y}{a_z}} \cr \tan^{-1}{\frac{-a_x}{\sqrt{a_y^2 + a_z^2}}} \end{pmatrix} \end{equation} \] \[ \begin{equation} \begin{split} \boldsymbol{y}_k &= h_{(\boldsymbol{x}_k)} + \boldsymbol{v}_k \\&= \boldsymbol{I}_{2 \times 3} \boldsymbol{x}_k + \boldsymbol{v}_k \\&= \begin{pmatrix} 1 & 0 & 0 \cr 0 & 1 & 0 \end{pmatrix} \begin{pmatrix} \phi_k \cr \theta_k \cr \psi_k \end{pmatrix} + \boldsymbol{v}_k \\&= \begin{pmatrix} \phi_k \cr \theta_k \end{pmatrix} + \boldsymbol{v}_k \end{split} \end{equation} \]

導出：重力ベクトル→姿勢角

まず、ロール\(\phi\)を求めます。

\[ \begin{equation} \begin{split} &a_y = g S_{\phi} C_{\theta} ,\quad a_z = g C_{\phi} C_{\theta} \\ \Leftrightarrow \quad &C_{\theta} = \frac{a_y}{g S_{\phi}} ,\quad C_{\theta} = \frac{a_z}{g C_{\phi}} \\ \Leftrightarrow \quad &\frac{a_y}{a_z} = \frac{S_{\phi}}{C_{\phi}} \\ \Leftrightarrow \quad &T_{\phi} = \frac{a_y}{a_z} \\ \Leftrightarrow \quad &\phi = \tan^{-1}{\frac{a_y}{a_z}} \end{split} \end{equation} \]

次に、ピッチ\(\theta\)を求めます。

\[ \begin{equation} \begin{split} &a_y^2 + a_z^2 = (g C_{\theta})^2 (S_{\phi}^2 + C_{\phi}^2) ,\quad a_x = -g S_{\theta} \\ \Leftrightarrow \quad &\sqrt{a_y^2 + a_z^2} = g C_{\theta} ,\quad g = \frac{-a_x}{S_{\theta}} \\ \Leftrightarrow \quad &\sqrt{a_y^2 + a_z^2} = -a_x \frac{C_{\theta}}{S_{\theta}} \\ \Leftrightarrow \quad &T_{\theta} = \frac{-a_x}{\sqrt{a_y^2 + a_z^2}} \\ \Leftrightarrow \quad &\theta = \tan^{-1}{\frac{-a_x}{\sqrt{a_y^2 + a_z^2}}} \end{split} \end{equation} \]

符号に注意！

上記の\(\boldsymbol{a}\)を慣性力として扱う必要があることに注意が必要です。もし、\(\boldsymbol{a}\)に重力加速度ベクトル\(\boldsymbol{g}\)を代入してしまうと、算出されるロールは、下図の\(\phi\)になってしまいます。しかしながら、本当に求めたいのは、鉛直軸からの傾き\(\phi'\)なので、重力加速度ベクトル\(\boldsymbol{g}\)を反転させた慣性力\(-\boldsymbol{g}\)を代入する必要があります。

LaTexによる表記

LaTexでEKFの式を書くのが大変なので、LaTex用のソースコードを残しておきます。