Madgwickライブラリの収束を速くする 【Arduino/MPU6050】

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にすると、収束が速くなります。ただし、ノイズが大きくなりますので、用途に合わせて調整してください。


本格派対局将棋 ぴよ将棋
本格派対局将棋アプリ ぴよ将棋
[Android] [iOS]

かわいい「ひよこ」と対局する将棋アプリ。かわいいけどAIは本格派!
対局後の検討機能や棋譜管理機能も充実!棋譜解析機能も搭載!

コメントを残す

メールアドレスが公開されることはありません。 が付いている欄は必須項目です