カルマンフィルタを用いた姿勢推定 - Qiita

はじめに 6軸センサ(3軸加速度,3軸ジャイロ)でカルマンフィルタを使った姿勢推定器を実装してみました。メモ程度。 使用するハードウェア RaspberryPi3+Navio2 姿勢について 今回、姿勢表現にはオイラー角を使います。てことで回転行列はこんな感じです。 $z$軸周りに$\phi$、$y$軸周りに$\theta$、$x$軸周りに$\psi$の順で回転させています。 $$ R(\Phi)=\begin{bmatrix} C_\phi C_\theta& C_\phi S_\theta S_\psi-S_\phi C_\psi &C_\phi S_\theta C_\psi+S_\phi S_\psi \\ S_\phi C_\theta& S_\phi S_\theta S_\psi+C_\phi C_\psi &S_\phi S_\theta C_\psi-C_\phi S_\