MbedでMadgwickFilterで使用する方法がわかりません。

Question

mbedを使い9軸センサの値をMadgwickFiterで計算させて傾斜角を取得する方法を模索中です。
Arduinoでは

#include <MadgwickAHRS.h>
#include <math.h>
Madgwick MadgwickFilter;

setup(){
MadgwickFilter.begin(10); 
}

loop(){
//センサからの取得値をAadgwickFiterに渡す
MadgwickFilter.update(加速度x,加速度y,加速度z,ジャイロx,ジャイロy,ジャイロz,地磁気x,地磁気y,地磁気z)

 float roll  = MadgwickFilter.getRoll();//ロール角
 float pitch = MadgwickFilter.getPitch();//ピッチ角
 float yaw   = MadgwickFilter.getYaw();//ヨー角
}

というような記述法で角度を取得できてはいたのですが、
mbedに移植するのに苦戦しており、ArduinoのMadgwickFilterと互換性がないのでは？と思い試行錯誤してみましたが角度がおかしい(開始直後から誤差大)ので、解決方法を教えていただきたいです。

参考サイト
http://tomi-tomi-pon.hatenablog.com/entry/2018/11/07/012854

Answer 1

![]

青がroll
オレンジ色がpitch
灰色がyaw

左の軸はrollとpitch
右の軸はyaw

解決策１．
角度をラジアンに変換したことでセンサを傾けたときに変化するようにはならなくなったが、グラフ化すると値が徐々に変化して飽和する

Answer 2

角速度データの単位がArduionoとmbedで違うような気がします。

ArduionoのMadgwickの実装を見ると度数を受け付けているように見えます。
対して、mbedの文書を見るとラジアンを受け付けているようです。

度数をラジアンに変換してから渡すのではないかと思います。

Comments

[PHENIXa]

回答ありがとうございます。
度数をラジアンに直すということは「ラジアン＝角度*(円周率/180)」でいいですか？

プログラム的にいうと
xGyro=xGyro*(3.1415/180)
というところでしょうか？

Madgwickの中身には
「角速度データ，[rad]に変換してから入れてください」
と記述されていたので、両方試しましたが改善されなかった
ですが、各値に変化はみられません。

[nomuken]

> xGyro=xGyro*(3.1415/180)
> というところでしょうか？
はい。変換しているとしたら問題なさそうですね。

あとは関数の呼び出し間隔は同じになっているでしょうか？
Arduionoは"MadgwickFilter.begin(10)"としていると100ms単位に呼び出すことを前提となります(たとえ10msしか経過していなくても200ms経過していたとしても100ms経過したとして演算する）
mbedは関数呼び出しの間隔を実時間から取っているようです。また、複数のMadgwickFilterインスタンスを使用しているとおかしな結果になるかもしれません。

該当実装
Arduiono：https://github.com/arduino-libraries/MadgwickAHRS/blob/70fd2c93d7fa88bffdd4ace675db1d7110e80adb/src/MadgwickAHRS.cpp#L135
mbed：https://os.mbed.com/teams/PQ_Hybrid_Electrical_Equipment_Team/code/MadgwickFilter/docs/tip/MadgwickFilter_8hpp_source.html の343行目