ちょこちょこ要望があるのでAruduinoにあるMadgwickライブラリの使い方を簡単ですが紹介します。
Madgwickライブラリの概要
Madgwickライブラリは6軸センサー（加速度、ジャイロ）や9軸センサー（加速度、ジャイロ、地磁気）からの取得値から、Madgwickフィルターというフィルターを通して姿勢角（roll、pitch、yaw）を算出するライブラリです。
フィルターの中身（数学的理解）は私にとっては非常に難解でいまいち消化不足です。おおざっぱには、「ジャイロ値からの算出姿勢」を「他のセンサー値」から補正してより正確な姿勢角を推定するというものです・・・。がライブラリ使うだけなら割と簡単です。
ライブラリが2016年ごろにVrUPされてますね。以前のライブラリではサンプリング周波数が512Hzになっていて、ライブラリの外からは変更できなかったです。そのため、「通信速度」や「ジャイロ値に係数」掛けて調整してました。
現在（2016年ごろ～現在2018/5）のライブラリではサンプリング周波数も設定できるため、変な調整はいらなさそうです。たまに更新見ないとだめですね。
Madgwickライブラリの使い方
まずはセットアップ部分です。
▼はじめにライブラリ取り込んでインスタンス化します▼
#include <MadgwickAHRS.h> Madgwick MadgwickFilter;
でsetup()関数内で、
MadgwickFilter.begin(100); //100Hz
▲センサーのサンプリング周波数を設定します。引数は「Hz」で記述。これで準備は完了。ライブラリの中で補正ゲインも変更できますがここでは触れません。
ここからはloop()内に記述する内容です。
▼姿勢角を計算させる関数です▼
MadgwickFilter.updateIMU(float gx, float gy, float gz, float ax, float ay, float az);
▲姿勢角をアップデート（計算）します。引数は、各軸のジャイロ値、加速値です。
ジャイロ値は[deg/s]。加速度値は、フィルターの中で正規化してますので「センサー生値」でも「G[kg/ｍ・s^2]」でもどちらでもいいはずです。
ジャイロ値、加速度値は、別途MPU6050などセンサーから取得しておきます。（MPU6050の使い方はこちら）
9軸センサーの場合は
MadgwickFilter.update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
となります。地磁気センサー分の引数が増えるだけです。
アップデートした後に、姿勢角（roll、pitch、yaw）を取得します。
ROLL = MadgwickFilter.getRoll(); PITCH = MadgwickFilter.getPitch(); YAW = MadgwickFilter.getYaw();
それぞれ軸ごとに関数が準備されてます。これでフィルター通した姿勢角が取得できます。戻り値の単位は「度」です。戻り値を「ラジアン」単位にする場合は、*****.getRollRadians()関数を使います。
制御ループ（サンプリング周波数）を最初に設定した周波数でまわせば、フィルター通過後の姿勢角が取得できます。ただ補正ゲインはライブラリの外からは変更できないので中身をいじる必要があります。ちなみにライブラリの中で設定されている係数は「0.1」です。（と思います・・。）
確認動画
（使い廻しの動画ですみません）一番右側がmadwickフィルターを通した姿勢角です。（左から順に「加速度センサーのみ」、「ジャイロのみ」、「簡易相補フィルター」で姿勢角を算出したものです。）Arduino+mpu6050で取得した値をProcessingで可視化してます。
ライブラリ使うだけで難解なMadgwickフィルターが、割と簡単に使えるので便利です。
コメント
初めまして「ArduinoのMadgwickライブラリの使い方」の記事を拝見しました。大学で現在mpu6050を用いた姿勢制御について学んでいる者ですがヨー軸のドリフトで躓いておりMadgwick
filterを用いても思うように姿勢を出せない状態です。大変恐縮ですが下記のメールアドレス宛に本記事のスケッチを添付していただくことは可能でしょうか？
かけだしさん、初めまして。サイト拝見下さり有難う御座います。
さてご質問の件、z軸（yaw）の角度ですが、6軸センサー（ジャイロ、加速度）からでは算出は非常に困難です。
比較的短い時間の相対的な角度変化であれば算出可能ですが、絶対角度を出すのは恐らくできないかと・・。
別途、地磁気センサーなどで向き（角度）を確認・補正する必要があります。
初めまして
私はMPU9250でドローン製作のためarduinoとつなげて姿勢の角度を出そうとしている
素人です。このページが大変詳しくよみやすく重宝させていただいております。
さて、今回Madgwickを使って6軸でロール、ピッチ、ヨーの角度を
出してシリアルプロッタで確認しておりました。
ところが、回転角度に対して反応はするのですが、示す値が約2倍のようでした。
つまりセンサを水平（昆虫で表現すると地面を這う状態）にするとロール、ピッチは0ですが、
垂直に近づけると（昆虫で表現すると木に登る状態）180度になる状況です。
ロールもピッチも同じように2倍で出力されています。
もしも、何か原因等で考えられることがございましたら教えていただけませんでしょうか。
クラビーアさん、サイト拝見下さり有難う御座います。
情報が少ないので何とも言えませんが、一様にずれた値が出力されるのであれば、
サンプリング周波数あたりの設定を一度疑ってみてはいかがでしょうか。
コメントいただきありがとうございます。
MadgwickFilter.beginを200Ｈｚにすると値的にそこそこ近づいてきました。
見よう見まねでやっており、ありがとうございました。
初めまして
私はMPU6050での姿勢制御を目指しているのですが、
Madgwickフィルタを通してもドリフトが発生してしまいます。
フィルタに渡す値に対して閾値などは設けられていますでしょうか？
それともセンサやプログラムの問題でしょうか。
かなり前の記事に対してですがよければ返信のほどお待ちしています。
シドウさん、サイト拝見下さり有難う御座います。
フィルタを通す際には特に閾値等は設けてません。
ドリフト発生はz軸廻り（YAW回転）でしょうか？
MPU6050のみではz軸廻り（YAW回転）のドリフト補正は不可能ですので注意が必要です。
返信ありがとうございます。
返事が遅れて申し訳ありません。
Yawだけでなくpitch,rollにも及んでいる状態です。
別のmpu6050でも同等の状態なのでプログラムを確認してみようかと思います。
「自作のいろいろ」の記事を参考させて頂き、
MPU9250、Madgwick Filter、を使用して、
ラジコン飛行機の姿勢検知に挑戦している者です。
Madgwick Filterの加速度による補正係数（0.1？）
をもっと小さく出来ないでしょうか？
もしくは、加速度データのUPの仕方で係数を小さく出来ないでしょうか？
ぜひ、ご助言を頂きたいです。
と言うのも、加速離陸時や、ゆっくり旋回飛行した場合には、重力加速度以外の加速度が、一方向に連続して（5～10秒程度）かかってきます。この加速度による不要な補正が、ジャイロの積分誤差よりも大きくなるのではないかと思います。
なお、私の挑戦が、友人によりYouTubeに投稿されています、ぜひご覧ください。
URL： https://www.youtube.com/watch?v=rPPqxp92Xl0
（２画面の再生速度を合わせるのが難しくて少しずれていますが．．．。）
メックさん、サイト拝見下さり有難う御座います。
さてご質問の件ですが、確かライブラリの外からは補正係数はいじれなかったと思います。ライブラリ.cpp内の
#define betaDef 0.1f
を直接変更するしかないと思います。
また加減速による補正具合ですが、私も過去同じような検討をしています。
https://garchiving.com/gyro-drift-part2/
参考になればと思います。
動画拝見しました。非常に面白そうなことを行っておりますね。
早速にお返事頂き有り難うございます。
ご助言頂いたMadgwick Filterライブラリーの中の係数を変えてみたいと思います。
とは言っても、私にとってライブラリーの中を見るのも、触るのも未知の世界です。
参考になるURL等、ご助言頂ければ嬉しいです。
宜しくお願い致します。
先日は有り難うございました。
Accの補正係数 0.1 を 0.01にして見ました。すると、
Accによる補正速度が 10deg/S から 1deg/S 程度になりました。
一方向に連続した加速度を加えてのテストは地上では難しいので、実際に飛行させて
試してみます。
有り難うございました。
初めまして
突然のコメント失礼いたします。
私は大学の研究でMPU6050をはじめとした慣性センサによる姿勢角の利用に関する研究に取り組んでいます。そのなかで度々こちらのサイトを参考にさせていただいております。誠にありがとうございます。
その中で解決できなかった問題なのですが，
現在人間の歩行動作に合わせて１Hzから0.9Hz程度の傾斜運動を計測しているのですが、
Madgwickフィルタを導入した際に算出値が大幅に減衰してしまう結果となってしまいました。
(±45°の傾斜運動を周期的に繰り返す運動で算出値は30°程度となっている)
Madgwickフィルタ側の周波数を上げるなどしても解決できませんでした。
これにはどのような要因があるのでしょうか。
拙い文章で申し訳ございません。何卒よろしくお願い申し上げます。
サトウさん、サイト拝見下さり有難うございます。
ご質問の件ですが、デバイスの故障、もしくはソースが間違っている、
のどちらかが濃厚かと思います。前者はどうしようも無いので後者が前提だとして、
もし、Arduinoライブラリ内のMadgwickフィルターを使用しているのであれば、
ソース内のサンプリング周波数と計算に使用する時間が異なる場合に
減衰に大きく影響がでる場合があります（一致させる必要があります）。
一度そこら辺を確認してみてはいかがでしょうか。
ご返信ありがとうございます！
前者,センサの故障については一応6軸情報そのもの出力に問題がみられなかったため後者の再確認をしていこうと思います．
(加速度は直接，角速度は積分による角度にて真値比較)
質問ばかりで申し訳ございません．
サンプリング周波数は MadgwickFilter.begin(100); //100Hz ← こちらとして
計算に使用する時間はどの部分で確認ができるのでしょうか．
また，loop()内での処理以外にも
割り込み処理内に MadgwickフィルタとPrintを記載したのですが こちらの書き方はNGだったでしょうか？
サトウさん、こんにちは。
すみません。言葉足らずで分り難かったですね。
サンプリング周波数の設定が仮に100Hzであれば、
ライブラリ内ではdt=0.01秒（ライブラリ内の変数invSampleFreq）
で姿勢角度の計算をしています。
なのできちんとMPU6050の値取得を0.01秒毎に行わないと
おかしな減衰となる可能性があります。
ただ積分で真値確認等しておられるようなので時間に関する処理は問題が無く、原因でないかもしれませんね。
割り込み処理内でMadgwickフィルタのライブラリを使用する場合、
コンパイル時に変数が最適化されて予期せぬ動きが出る場合があるため、
一度割り込みの外でフィルターを通すことを試してみてはいかがでしょうか。
度々申し訳ございません．
MPU6050の値取得はレジストの設定を変更するという意味でよろしいでしょうか．何度もお聞きしてしまい申し訳ありません．
またせっかくご返信いただいたにも関わらず，返信が遅くなり誠に申し訳ございません．
いえいえ私も返信遅いときあるので気になさらずに。。ここら辺は文面で説明は難しいですね・・。
MPU6050から加速度や角速度を取得されると思いますが、その取得間隔を0.01秒毎に行う必要があります。（サンプリング周波数100Hzの場合）レジストは変更しなくてもいいと思います。（これが真因かどうかわからないため、あまり固執しないほうがいいかもしれません）
申し訳ございません．
レジスト以外ですと,プログラムのどこでMPU6050の取得間隔を0.01秒ごとにすればよろしいのでしょうか…
https://garchiving.com/how-to-use-mpu6050-in-arduino/
こちらの記事で解説されていたコードに追加して動作の確認をしておりました．
別途所得間隔やアップデートなどの記述が必要ということでしょうか．
何卒宜しくお願い致します．ご迷惑をおかけしてしまい申し訳ありません．
レジストは触る必要は無いと思います。
「角速度は積分による角度にて真値比較」その時の数値積分に使われた時間はどのようにされましたか？
貼って頂いたリンク先にあるソースは、ループの中でサンプリングを垂れ流しにしているため、時間間隔は不明ですね。
そのソースを改造するのであれば、プログラムのループを0.01秒毎で回るようにしてあげればいいと思います。
ここら辺分り難いようであれば、加速度、角速度、角度、などの物理的理解、相補フィルターの理解、またソースの理解を深めたほうが早道になるかと思います。
何度もご対応いただき誠にありがとうございます．
また返信が大変遅くなり申し訳ございません．
角速度の積分の際には割り込み処理による時間間隔を使用しておりましたが，思えばきちんと正確であるかは断言できませんでした．
これまで加速度による傾斜角の算出や簡単な相補フィルタ，ハイパスフィルタによる補正など取り組んできましたが，うまくいかずライブラリにすがるような形で導入しようとしてきたことを反省し，きちんと原理原則を学習しようと思います．
これまで丁寧にコメントしていただき誠にありがとうございます．
また大変ご迷惑おかけしてしまったことお詫び申し上げます．
失礼致しました．またどこかの機会でご相談いただければ幸いです何卒どうぞよろしくお願い致します。
いえいえ丁寧に有難うございます。お気になさらず、いつでもご相談ください。
度々申し訳ございません
その後，いろいろ試しておりましたが新たに疑問点が生じたためご相談いただけないでしょうか．．．
ループを100Hzにするために，loopを空にして割り込み処理として実行することで一定間隔での動作が可能かと思い実験しておりました．
しかし，MPU側のシリアル通信(i2c)にて割り込み処理[Wire.beginTransmission～Wire.endTransmission]が使われているため重複し動作致しませんでした．
こちらのサイトおよびYouTube動画で実行しているプログラムではどのようにしてMPU側の取得間隔とMadgwickフィルタサンプリングを同じにしているのでしょうか？
サトウさん、こんにちは、例えばですが、ループ自体を100Hzで回るようにしてその中で処理を行ってます。本記事はそういった方法で行ってます。
例えばArduinoで記述する場合であれば、
void loop() {while (currentTime - loopTimer <= 10000)currentTime = micros();
loopTimer = currentTime;
//
//ここにMPU6050通信処理や、Madgmadgwickフィルターなどの処理を記述します。
//
}
このように記述すれば、強制的に10ms（100Hz）経過するまで待機することになるため1ループが100Hz（10ms）となります。その間に処理を記述します。当然処理に使える時間は10ms以内に抑える必要があります。
.