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

#はじめに
授業で拡張カルマンフィルタ(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

次はクオータニオンに対して適用してみます。

#参考文献

• カルマンフィルタの基礎
• カルマンフィルタを用いた姿勢推定(https://qiita.com/Soonki/items/3e9ffb072ba3ba11490e#%E6%84%9F%E6%83%B3)
• 航空宇宙技術研究所資料 : クオータニオンとオイラー角によるキネマティックス表現の比較について
  (https://repository.exst.jaxa.jp/dspace/bitstream/a-is/23926/1/naltm00636.pdf)