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

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

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

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

表記ルール

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

  • 細字の小文字(\(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}} & cos{\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}\)を代入する必要があります。

g_to_roll_right_tilt
g_to_roll_left_tilt

LaTexによる表記

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

状態ベクトル

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

入力ベクトル

\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}

状態方程式

\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}} & cos{\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}
\begin{equation}
	\boldsymbol{w}_{k-1} \sim N(0, \boldsymbol{Q}_{k-1})
\end{equation}

観測ベクトル

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

観測方程式

\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}
\begin{equation}
	\boldsymbol{v}_k \sim N(0, \boldsymbol{R}_k)
\end{equation}

予測推定

\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}
\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}
\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}

さいごに

6軸IMUを用いたEKFについてまとめてみました。参考になれば幸いです。


以上です。

Ad.