ArduinoでMPU6050などのジャイロを使うとき、Madgwickライブラリを使うと簡単に角度の推定ができますが、デフォルトの状態だと収束に少し時間がかかるようです。
多少ノイズが大きくなっても速く追従させたかったので、収束速度=ゲインを調整できるようにしてみました。
Madgwickライブラリ: https://github.com/arduino-libraries/MadgwickAHRS
Madgwickライブラリの使い方はここでは述べませんので以下を参照してください。
ArduinoのMadgwickライブラリの使い方
https://garchiving.com/how-to-madgwick-library-of-arduino/
【Arduino 】MPU-6050を使って姿勢角を算出 （MadgwickFilterを使用）
https://shizenkarasuzon.hatenablog.com/entry/2019/02/16/181342
Madgwickライブラリは1.2.0 (2022/12/3時点の最新版)を対象とします。
Madgwickライブラリのゲイン変更
Madgwickライブラリではゲインが0.1で固定されていますので、これを変更できるようにメソッドを追加してみます。
Madgwickライブラリをインストールすると、手元の環境では以下のディレクトリにインストールされていました。
C:\Users\xxxx\Documents\Arduino\libraries\MadgwickAHRS-master\src
(※xxxxはユーザー名)
MadgwickAHRS.h にsetGain()メソッドを追加します。 (begin()メソッドの下の行)
....
public:
Madgwick(void);
void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; }
void setGain(float gain) { beta = gain; } // <- この行を追加
void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
....
begin()を呼んだ後に、以下のようにsetGain(1.0)すると、ゲインを設定できます。
MadgwickFilter.begin(100); // 100Hz MadgwickFilter.setGain(1.0);
デフォルトは0.1です。
1.0にすると、収束が速くなります。ただし、ノイズが大きくなりますので、用途に合わせて調整してください。
