おびやまロボット研究所ブログ

センサを探している間に、もうちょっと戻ってMadgwickフィルタを調べる。

そもそもMadgwickってなに？

Madgwick Filterを読んでみた
https://qiita.com/fumiya_sato/items/df28dff5ccc653183235
この中の記述
MadgwickフィルタのMadgwickは提唱した人の名前だそうだ。
原論文は、An efficient orientation filter for inertial and inertial/magnetic sensor arrays
”カルマンフィルタに比べて、高速で同程度以上の精度を実現したらしいフィルタです．”
”収束が遅いらしいとか・・・？”

ここまでわかって安心。
この記事の中で最初に出てくる、クォータニオンというのもいまいち理解できてないので検索。

クォータニオンとは何ぞや？：基礎線形代数講座
https://techblog.sega.jp/entry/2021/06/15/100000
この中の記述．．．
”クォータニオンは日本語では四元数（しげんすう）と訳されるもので、1843年にハミルトンにより発見された複素数を拡張した代数体系であり、3次元の回転の表現としても多くの利点を備えている。”

これで分かった気になって先に進む。

Sparkfunの6DOFセンサが読み出せたので、こちらの記事のMadgwickフィルタのサンプルに　FreeSixIMU　のライブラリとセンサの読み取り値を読み取った入れ込む。

MadgwickフィルタのライブラリをArduinoで使う
https://qiita.com/Ninagawa123/items/9520bad3c78ee40194fc

しかし、フィルタ動いてる気配がない。０付近の値がふらふらしてるだけ。
多分違うセンサを使ってるせい。そりゃそうだ。
数値の大きさとか、軸の並びとか、変化分の極性とか違う可能性はある。

なので、再度MPU-6050がどんな値を出してるのか、フィルタにどんな値を入れればいいのかを確認。

こちらのサイトをからMPU6050の値を確認。動画から、重力加速度を１として、角速度を度／秒くらいの値にしてやればいいようだ。傾きや変化の向きも確認した。

Arduinoから MPU6050の値を取得してみる
https://garchiving.com/how-to-use-mpu6050-in-arduino/

Sparkfunの６DOFは、重力加速度が220くらい、角速度が度／秒に直すと４倍くらいの値が出てたので、単純に値を割ってMadgwickフィルタに与える。

すると、Arduinoのシリアルポートからそれっぽい値が出だした。

次にProsessingを起動、サイトのコードをコピペ。動かしてもポートが読めないとのエラー。
ポートを調べて、０番に直す。

int available_serialport = 0;

すると画面上で平板が動き出した。でも回転してもヨー軸が回らない。



値が上手く取り込めてないのかと、dataの読み出し部分を確認しても問題なし。
画面の表示部分を読んでみたら、そもそもサイトのコードでヨー軸を回転させてない。
なので、コードに1行追加。

rotateY(radians(data[2])); //YAW軸追加



再起動すると、画面上で平板がセンサを動かした通りに動き出した。



やってみて分かったのは、初期状態を水平にしてから起動した方がいい。ひっくり返した状態から起動すると、正しい状態を得るのに時間がかかる。
レスポンスは早い。
これを使ってドローンを自作できるかも、と思った。