Help us understand the problem. What is going on with this article?

# 6軸IMUを使ってオイラー角に対してEKFを組んでみた

More than 1 year has passed since last update.

• mbed LPC1768
• mpu6050

# 姿勢について

## 回転行列 R(Φ) z-y-x Euler

\boldsymbol{\Phi} = \left[
\begin{array}{ccc}
\theta_y & \theta_p & \theta_r
\end{array}
\right]^\mathrm{T}

\begin{align}
R(\boldsymbol{\Phi}) &= \left[
\begin{array}{ccc}
\cos\theta_y  & \sin\theta_y & 0 \\
-\sin\theta_y & \cos\theta_y & 0 \\
0            & 0             & 1
\end{array}
\right]
\left[
\begin{array}{ccc}
\cos\theta_p  & 0            & -\sin\theta_p \\
0             & 1            & 0 \\
\sin\theta_p & 0            & \cos\theta_p
\end{array}
\right]
\left[
\begin{array}{ccc}
1            & 0            & 0             \\
0            & \cos\theta_r & \sin\theta_r \\
0            & -\sin\theta_r & \cos\theta_r
\end{array}
\right] \\
&= \left[
\begin{array}{ccc}
\cos\theta_y\cos\theta_p & \sin\theta_y\cos\theta_r+\cos\theta_y\sin\theta_p\sin\theta_r & \sin\theta_y\sin\theta_r-\cos\theta_y\sin\theta_p\cos\theta_r \\
-\sin\theta_y\cos\theta_p & \cos\theta_y\cos\theta_r-\sin\theta_y\sin\theta_p\sin\theta_r & \cos\theta_y\sin\theta_r+\sin\theta_y\sin\theta_p\cos\theta_r \\
\sin\theta_p            & -\cos\theta_p\sin\theta_r             & \cos\theta_p\cos\theta_r
\end{array}
\right]
\end{align}


## 姿勢変化

ボディの角速度$\omega$とEuler角の時間微分$\dot{\Phi}$

\boldsymbol{\omega} = \boldsymbol{Q}\dot{\boldsymbol{\Phi}}

\boldsymbol{Q} = \left[
\begin{array}{ccc}
-\sin\theta_p            & 0             & 1 \\
\cos\theta_p\sin\theta_r & \cos\theta_r  & 0 \\
\cos\theta_p\cos\theta_r & -\sin\theta_r & 0
\end{array}
\right]

\begin{align}
\dot{\boldsymbol{\Phi}} = \boldsymbol{Q}^{-1}\boldsymbol{\omega}
&= \left[
\begin{array}{ccc}
0            & \sin\theta_r/\cos\theta_p & \cos\theta_r/\cos\theta_p \\
0            & \cos\theta_y              & -\sin\theta_r             \\
1            & \sin\theta_r\tan\theta_p  & \cos\theta_r\tan\theta_p
\end{array}
\right]
\left[
\begin{array}{ccc}
\omega_x \\
\omega_y \\
\omega_z
\end{array}
\right] \\
&= \left[
\begin{array}{ccc}
\omega_y\sin\theta_r/\cos\theta_p + \omega_z\cos\theta_r/\cos\theta_p \\
\omega_y\cos\theta_r-\omega_z\sin\theta_r \\
\omega_x+\omega_y\sin\theta_r\tan\theta_p+\omega_z\cos\theta_r\tan\theta_p
\end{array}
\right]
\end{align}


# 拡張カルマンフィルタの実装

ですので、状態方程式にジャイロセンサから得られる姿勢推定値、観測方程式に加速度センサから得られる姿勢推定値を入力しています。

ちなみにyaw角に関しては加速度センサから角度を正確に算出できないので今回は省略します。コンパスの特性を持った9軸センサを用いればyaw角の推定値を観測方程式に盛り込むことで推定できるかもしれません。

\boldsymbol{x} = \left[
\begin{array}{ccc}
\theta_p & \theta_r & \omega_x & \omega_y & \omega_z & a_x & a_y & a_z
\end{array}
\right]^\mathrm{T}


$\dot{\Phi}$をEuler法により離散化し、状態方程式を立てると次のようになります。$\boldsymbol{v}$はシステム雑音です。

\boldsymbol{x}_{n+1} = \boldsymbol{f}(\boldsymbol{x}_{n})+\boldsymbol{v}
= \left[
\begin{array}{ccc}
\theta_p+(\omega_y\cos\theta_r-\omega_z\sin\theta_r)T_s                                  \\
\theta_r+(\omega_x+\omega_y\sin\theta_r\tan\theta_p+\omega_z\cos\theta_r\tan\theta_p)T_s \\
\omega_x \\
\omega_y \\
\omega_z \\
a_x      \\
a_y      \\
a_z      \\
\end{array}
\right]+\boldsymbol{v}


また、観測値$\boldsymbol{y}$を次のように定めます。

\boldsymbol{y} = \left[
\begin{array}{ccc}
\theta_p & \theta_r & \omega_x & \omega_y & \omega_z & a_x & a_y & a_z
\end{array}
\right]^\mathrm{T}


\boldsymbol{y}_{n} = h(\boldsymbol{x}_{n})+\boldsymbol{w}
= \left[
\begin{array}{ccc}
-\tan^{-1}\frac{a_x}{\sqrt{a_y^2+a_z^2}} \\
\tan^{-1}\frac{a_y}{a_z}                 \\
\omega_x \\
\omega_y \\
\omega_z \\
a_x      \\
a_y      \\
a_z      \\
\end{array}
\right]+\boldsymbol{w}


## 時間更新式

### (step1)予測ステップ

• 事前状態推定値
\hat{\boldsymbol{x}}^-(k) = \boldsymbol{f}(\hat{\boldsymbol{x}}(k-1))

• 線形近似
\boldsymbol{A}(k-1)
= \left. \frac{\partial\boldsymbol{f}(\boldsymbol{x})}{\partial\boldsymbol{x}}\right|_{\boldsymbol{x}=\hat{\boldsymbol{x}}(k-1)}
= \left[
\begin{array}{ccc}
1 & (-\hat{\omega_y}\sin\hat{\theta_r}-\hat{\omega_z}\cos\hat{\theta_r})T_s & 0 & T_s\cos\hat{\theta_r} & -T_s\sin\hat{\theta_r} & 0 & 0 & 0\\
(\hat{\omega_y}\sin\hat{\theta_r}+\hat{\omega_z}\cos\hat{\theta_r})T_s/\cos^2\hat{\theta_p} & 1+(\hat{\omega_y}\cos\hat{\theta_r}-\hat{\omega_z}\sin\hat{\theta_r})T_s\tan\hat{\theta_p} & T_s & T_s\sin\hat{\theta_r}\tan\hat{\theta_p} & T_s\cos\hat{\theta_r}\tan\hat{\theta_p} & 0 & 0 & 0\\
0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 \\
0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 \\
0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 \\
0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 \\
0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 \\
0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 \\
\end{array}
\right]

\boldsymbol{C}^\mathrm{T}(k)
= \left. \frac{\partial h(\boldsymbol{x})}{\partial\boldsymbol{x}}\right|_{\boldsymbol{x}=\hat{\boldsymbol{x}^-}(k)}
= \left[
\begin{array}{ccc}
0 & 0 & 0 & 0 & 0 & -\frac{\sqrt{\hat{a^-_y}^2+\hat{a^-_z}^2}}{\hat{a^-_x}^2+\hat{a^-_y}^2+\hat{a^-_z}^2} & \frac{\hat{a^-_x}\hat{a^-_y}}{\sqrt{\hat{a^-_y}^2+\hat{a^-_z}^2}(\hat{a^-_x}^2+\hat{a^-_y}^2+\hat{a^-_z}^2)} & \frac{\hat{a^-_x}\hat{a^-_z}}{\sqrt{\hat{a^-_y}^2+\hat{a^-_z}^2}(\hat{a^-_x}^2+\hat{a^-_y}^2+\hat{a^-_z}^2)} \\
0 & 0 & 0 & 0 & 0 & 0 & \frac{\hat{a^-_z}}{\hat{a^-_y}^2+\hat{a^-_z}^2} & -\frac{\hat{a^-_y}}{\hat{a^-_y}^2+\hat{a^-_z}^2}\\
0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 \\
0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 \\
0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 \\
0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 \\
0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 \\
0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 \\
\end{array}
\right]

• 事前誤差共分散行列
\boldsymbol{P}^-(k) = \boldsymbol{A}(k-1)\boldsymbol{P}(k-1)\boldsymbol{A}^\mathrm{T}(k-1) + \sigma_v^2(k-1)\boldsymbol{b}\boldsymbol{b}^\mathrm{T}


### (step2)フィルタリングステップ

• カルマンゲイン
\boldsymbol{g}(k) = \frac{\boldsymbol{P}^-(k)\boldsymbol{C}(k)}{\boldsymbol{C}^\mathrm{T}(k)\boldsymbol{P}^-(k)\boldsymbol{C}(k)+\sigma_\omega^2\boldsymbol{I}}

• 状態推定式
\hat{\boldsymbol{x}}(k) = \hat{\boldsymbol{x}}^-(k)+\boldsymbol{g}(k)\{\boldsymbol{y}(k)-h(\hat{\boldsymbol{x}}^-(k))\}

• 事後誤差共分散行列
\boldsymbol{P}(k) = \{ \boldsymbol{I}-\boldsymbol{g(k)}\boldsymbol{C}^\mathrm{T}(k) \}\boldsymbol{P}^-(k)


# 実装したプログラム

それっぽい値を返してくれますが、まだまだドリフトは補正できてません。。。
https://github.com/calm0815/mpu6050_EKF/tree/eigen

# 参考文献

Why do not you register as a user and use Qiita more conveniently?
1. We will deliver articles that match you
By following users and tags, you can catch up information on technical fields that you are interested in as a whole
2. you can read useful information later efficiently
By "stocking" the articles you like, you can search right away