回転行列+簡易相補フィルター（Arduino＋MPU6050）

　ドローン（クアッドコプター）の姿勢制御プログラム中で6軸センサーから姿勢角を算出する演算がわりと時間を使ってしまいます。今回はソースを簡素化して（なるべく精度を落とさずに）処理時間の高速化をできないか確認してみました。

・前提
　　マイコン：ArduinoUNO
　　6軸センサー：MPU6050

　Arduinoライブラリにあるmadgwickフィルターを試したのですが、1ループの処理時間が2.5ms（2500μs）程とおもったよりかかります。

　他にいろいろな算出方法・補正方法を試したのですが、回転行列使って加速度ベクトル算出して相補フィルター使って補正するのが処理が早く（430μs程）、精度もまぁまぁ。（ドローンの制御を目的としてます）

　というか私にはこの方法でしか高速化できなかったのですけどね・・。とりあえずライブラリにあるmadgwickフィルターと比較してみました。

▼ビジュアルで比較▼

※シリアル経由、Processingで可視化してます。madgwickフィルターはいまいち理解が不十分なので係数はライブラリデフォルトのままです・・。ブレッドボードにいろいろ刺さってますが、使ってるのはMPU6050だけです。

　左が回転行列と相補フィルターの組合せ、右がMadgwickフィルター、です。表示している時間は姿勢角算出している部分の1ループの処理時間です。見た目では違いはほとんど無いように見えます。

▼roll軸グラフ▼

　適当にセンサーを揺らしてるときの算出角度をグラフ化（roll軸のみ）してます。madgwickフィルターと比べてみてもほとんど差は無いです。

　処理の中身を少し紹介します。6軸センサーを使ってますので、基本はジャイロからの算出値を加速度値で補正してます。

　流れはこんな感じ。
・ジャイロ値からその時に検出されるだろう加速度ベクトルを算出
・加速度センサーから検出した加速度ベクトルと算出した値を補正
・補正した姿勢ベクトルからroll、pitch、yaw角を算出

やってることはmadgwickフィルターと似てる？？ただクォーターニオン（四元数）ではなく、回転行列使って加速度ベクトル算出してます。

　まず姿勢ベクトルの算出ですが、加速度センサーからの値をそのまま（加工せずに）使いため、ジャイロ値からは加速度ベクトルを算出してます。今回のMPU6050設定では加速度値、1G=4096（生値）が検出される設定です。

　ですので初期のベクトルを（0,0,4096）とおいて、回転行列から現在の加速度ベクトルを算出してます。

　ジャイロ値から回転行列使って現在の加速度ベクトルを算出しようとすると結構な処理が必要です。三角関数と乗算を多用、しかも小数演算。ちなみにそのままの処理を試したのですが、ベクトル計算だけで1500μs程使ってました。

　ジャイロ値からの角度計算ですが、今回はMPU6050のジャイロ感度を500deg/sに設定してます。全体ループ（サンプリング周期）を250Hz(4ms)で考えてますので最大で検出できる角度は2度/ループが限界。

　ですのでsinθ≒θ、cosθ≒1で置き換えてしまってもそれほど大きな誤差は出ません。

　またジャイロ値からの角度[rad]計算では65536倍（16bit左シフト）したスケールを使って、なるべく整数計算で誤差がすくなるなるように処理してます。

　その際、ビットシフト処理が多数発生しますが、少しでもオーバーヘッド避けるために共用体（union）使って変数準備してます。（速度効果は不明ですが・・。）

　こんな感じで算出したジャイロからの加速度ベクトルを、加速度センサー値使って補正します。

相補フィルターは

補正値 = (1 - k) * ジャイロからの算出ベクトル + k * 加速度ベクトル

　で表現できますが、ここでも整数演算（bit演算）したいため少し式を変更します。

補正値 = ジャイロ + k * 加速度 - k * ジャイロ
　　　 = ジャイロ + k * (加速度ージャイロ)
　　　+= k *(加速度ージャイロ)
　　　+=(加速度-ジャイロ) >> c  
(c = a^-n)

　という具合に変形してbit演算で補正してます。ここでもスケール使って、切り捨て分の精度悪化を極力避けるようにしてます。

　最後にベクトルを角度へ変換するのですが、ここで三角関数のatan2を使ってます。この処理も時間がかかるため、近似式を使った自作関数で処理を高速化。

　平方根も高速化したくて、有名なinvSqrt関数使ったのですが、なぜか早くならなかったです・・。（以前使用したときは早くなったと記憶してるのですが・・。）

　こんな感じで処理したもので1ループ450μs程（CPU：16MHz）の処理時間となりました。

　まぁまぁの高速化はできたのですが、ここまでやるとかなり見通しの悪いソースで・・。間違いの元になりそうです。