ジャイロのドリフト補正と比較（カルマン、相補フィルター）

　ジャイロのドリフト補正方法がいろいろあるようですが、比較的情報の集めやすいカルマンフィルターと相補フィルター（簡易）の実装と簡単な比較検証、ドリフト補正の効果確認を行ってみました。

　検証にはArduino/Genuino 101 を使用、内蔵されている6軸センサー（ジャイロ、加速度）をそのまま利用してます。

　まずはドリフトの発生具合を確認してみました。

　▼Arduino101/Genuino101 ジャイロドリフト▼
　

　ジャイロセンサーの取得値（角速度）を単純積分して角度を算出してます。水平に置いて放置しているだけですが、1分ほどで 1.0° 程度のドリフトが発生しました。ドリフトの発生具合は環境や固体差が出るかと思います。

　カルマンフィルターも相補フィルターも、ジャイロのドリフト補正には【加速度センサーからの角度算出値】を使用します。

　そこで、加速度センサーからの角度算出値を確認してみたところ、結構ノイズが乗っていたのでローパスフィルター（LPF）で平滑化しました。（加速度センサーから軸廻り角度への変換計算はこちらで紹介してます）

　▼ローパスフィルターはソフトで実装▼

angle  = 0.95 * preAngle  + 0.05 * angleAccel;

preAngle:1つ前の角度算出値　　angleAccel:加速度センサーからの角度算出値

　係数は適当です。結果みながら微調整しました。

▼角度算出値（加速度センサー）のLPFフィルター有無の比較です▼
　

　位相が少しずれてますがとりあえずノイズはカットできてます。ただ結局、ドリフト補正にしようする値は、ノイズカットしていない生値を使用しました。

カルマンフィルター

　カルマンフィルターですが、相当奥が深く、私には内容が難解です。実装するためのライブラリーがいろいろと公開されていたので利用させてもらいました。（カルマンフィルターライブラリはこちら）

　ほぼそのまま利用してます。簡単ですが使い方▼

Kalman kalmanX; // instances

kalmanX.setAngle(roll); // Set starting angle

kalAngleX = kalmanX.getAngle(angleAccel, gyroXrate, dt);

　インスタンス生成して、初期値を設定。最後にフィルターを通すだけです。初期値は0でも問題なさそうでした。

引数は、
　angleAccel：加速度センサーからの角度算出値（degree）
　gyroXrate ：ジャイロセンサーの角速度（degree/s）
　dt ：時間（s）

何も考えずとりあえず実装するだけならお手軽です。

　こちらもまともに理解しようとすると相当奥が深く、難解です。簡易的な実装方法が紹介されていたのでそれをそのまま使用してみました

angle = 0.95 * (preAngle + gy * dt) + 0.05 * angleAccel

　実装は非常にお手軽です。ジャイロセンサーからの角度算出値と加速度センサーからの角度算出値を重み付け平均してます。

　実際に両方のフィルターを試してみて、簡単な比較とドリフト補正の効果を見てみました。

▼まずは静止状態でのドリフト確認です▼
　

　当然ですが単純な積分値ではどんどんドリフトしていきます。カルマンフィルター、相補フィルターは見事にドリフトを抑えてます。あまり差はない感じ。10分ほど放置してましたがカルマン、相補ともにドリフトの発生はほとんどなかったです。

　▼60度付近まで傾ける⇒静止⇒0度に戻すを繰り返してます▼
　

　単純積分では徐々にドリフトが発生していきますが、フィルター有ではまったくドリフトが発生してません。ここでもカルマン、相補の違いはほとんどありませんでした。

▼ぐるぐる廻して水平に戻すを繰り返してます▼

単純積分は大きなドリフトが発生してます。カルマン、相補ともにきれいにドリフトを抑えてますが、廻している最中の値に少し差が出ました。カルマンフィルターのほうが少しだけ反応が鈍い感じでしょうか。

▼素早く定期的に振ってみました▼

振った後に、水平に戻したのですが、単純積分は大きくドリフト発生。カルマン、相補はきれいにドリフト補正してました。振ってる最中ですが、相補フィルターの位相が少しずれている感じでしょうか。

　簡単な比較とドリフト補正の効果を確認してみたのですが、カルマン、相補フィルターとも見事にドリフト補正できていました。

　カルマン、相補フィルターの算出値の違いは少しあるものの、その影響は良く分かりません。今後、両方とも使っていきながら、様子見ていきたいと思います。

　加速度センサーからの角度算出ではz軸（yaw軸）廻りの角度算出が困難です。。z軸回転では加速度に変化がないため回転角度算出ができないためです。ですのでz軸（yaw軸）回転の補正は別の方法で行う必要があるかと思います。

　さらにドリフト補正に加速度センサーからの値を使用しているため、センサー自体に加速が加わると算出角度がおかしなことになります。ですのでセンサー自体に加速移動が伴う場合には少し工夫が必要です。