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

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

More than 1 year has passed since last update.

はじめに

授業で拡張カルマンフィルタ(Extended Kalman Filter)について触れたので、実践も兼ねてゼロから挑戦してみました。本記事はその備忘録です。
記事の最後にGithubへのリンクを載せてます。間違いなどがあればissueへ投げていただけると助かります。

開発環境

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

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

今回は6軸センサを使って、オイラー角に対して拡張カルマンフィルタを適用することで対象物の姿勢を推定することを目的としています。
ですので、状態方程式にジャイロセンサから得られる姿勢推定値、観測方程式に加速度センサから得られる姿勢推定値を入力しています。

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

推定値$\boldsymbol{x}$を次のように定めます。

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

加速度から姿勢角を算出する式を$\theta_p$, $\theta_r$に盛り込んで観測方程式を立てると次のようになります。$\boldsymbol{w}$は観測雑音です。

\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
Comments
Sign up for free and join this conversation.
If you already have a Qiita account
Why do not you register as a user and use Qiita more conveniently?
You need to log in to use this function. Qiita can be used more conveniently after logging in.
You seem to be reading articles frequently this month. Qiita can be used more conveniently after logging in.
  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