質問をすることでしか得られない、回答やアドバイスがある。

15分調べてもわからないことは、質問しよう!

新規登録して質問してみよう
ただいま回答率
87.20%
Arduino

Arduinoは、AVRマイコン、単純なI/O(入出力)ポートを備えた基板、C言語を元としたArduinoのプログラム言語と、それを実装した統合開発環境から構成されたシステムです。

受付中

9軸センサにフィルタを用いてドリフトを抑えたい

tannu
tannu

総合スコア13

Arduino

Arduinoは、AVRマイコン、単純なI/O(入出力)ポートを備えた基板、C言語を元としたArduinoのプログラム言語と、それを実装した統合開発環境から構成されたシステムです。

1回答

0評価

2クリップ

9062閲覧

投稿2019/05/02 17:03

編集2022/01/12 10:55

センサのドリフトを抑えたい

Arduinoボードと9軸センサ(MPU9250)を利用してセンサの絶対角度を算出しようとしています.
(角度のみをシリアルモニタに表示させたいです)
ドリフトを抑えようとMadgwickフィルタもしくはカルマンフィルタを使用しようと考えているのですが,プログラミングの経験が浅く上手く適用することができません.
具体的にどのようにプログラムを変更すれば正しく適用できるのか教えていただきたく存じます.

現在使用しているプログラム

現在は以下のプログラムを使用しています.
インターネット上に上がっていたプログラムをほとんど引用して使用しています.

Arduino

#include <Wire.h> #include <Servo.h> #define SerialPort Serial #include <SparkFunMPU9250-DMP.h> MPU9250_DMP imu; double roll , pitch, yaw; long int pre_ts=0; void setup() { { SerialPort.begin(115200); if (imu.begin() != INV_SUCCESS) { while (1) { SerialPort.println("Unable to communicate with MPU-9250"); SerialPort.println("Check connections, and try again."); SerialPort.println(); delay(3000); } } imu.setSensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS); imu.setGyroFSR(250); // Set gyro to 2000 dps // Accel options are +/- 2, 4, 8, or 16 g imu.setAccelFSR(2); // Set accel to +/-2g imu.setLPF(10); // Set LPF corner frequency to 5Hz imu.setSampleRate(10); // Set sample rate to 10Hz imu.setCompassSampleRate(50); // Set mag rate to 10Hz } pre_ts=millis(); } //} void loop() { if ( imu.dataReady() ) { imu.update(UPDATE_ACCEL | UPDATE_GYRO | UPDATE_COMPASS); printIMUData(millis()-pre_ts); pre_ts=millis(); } } void printIMUData(long int dt) { float accelX = imu.calcAccel(imu.ax); float accelY = imu.calcAccel(imu.ay); float accelZ = imu.calcAccel(imu.az); float gyroX = imu.calcGyro(imu.gx)/57.3; float gyroY = imu.calcGyro(imu.gy)/57.3; float gyroZ = imu.calcGyro(imu.gz)/57.3; float magX = imu.calcMag(imu.mx); float magY = imu.calcMag(imu.my); float magZ = imu.calcMag(imu.mz); pitch = atan2 (accelY ,( sqrt ((accelX * accelX) + (accelZ * accelZ)))); roll = atan2(-accelX ,( sqrt((accelY * accelY) + (accelZ * accelZ)))); // yaw from mag float Yh = (magY * cos(roll)) - (magZ * sin(roll)); float Xh = (magX * cos(pitch))+(magY * sin(roll)*sin(pitch)) + (magZ * cos(roll) * sin(pitch)); yaw = atan2(Yh, Xh); roll = roll*57.3; pitch = pitch*57.3; yaw = yaw*57.3; // -180~180 to 0~360 roll = roll + 180; pitch = pitch + 180; yaw = yaw + 180; Serial.print(pitch); Serial.print(','); Serial.print(roll); Serial.print(','); Serial.println(yaw); }

お聞きしたいこと

上記プログラムにどのようにフィルタを適用すればよいのか
もしくは
上記プログラムは関係なくフィルタを適用した上での角度算出ができるプログラム
をお伺いしたいです.

知識不足な部分が多いかと思いますがよろしくお願いします.

追記

DMPを利用した角度を算出するプログラムを見つけることができたのですが,絶対角度で算出することができません.
こちらのコードから絶対角度を算出する方法があれば教えていただきたいです.
(加速度,ジャイロ,地磁気の値を取り出せれば回転行列によって求められるのですが,,,)
以下にそのコードを添付いたします.(必要ないものはコメントアウトしています)
参照サイト

Arduino

#include "freeram.h" #include "mpu.h" #include "I2Cdev.h" int ret; void setup() { Fastwire::setup(400,0); Serial.begin(9600); ret = mympu_open(200); //Serial.print("MPU init: "); Serial.println(ret); //Serial.print("Free mem: "); Serial.println(freeRam()); } unsigned int c = 0; //cumulative number of successful MPU/DMP reads unsigned int np = 0; //cumulative number of MPU/DMP reads that brought no packet back unsigned int err_c = 0; //cumulative number of MPU/DMP reads that brought corrupted packet unsigned int err_o = 0; //cumulative number of MPU/DMP reads that had overflow bit set void loop() { ret = mympu_update(); switch (ret) { case 0: c++; break; case 1: np++; return; case 2: err_o++; return; case 3: err_c++; return; default: Serial.print("READ ERROR! "); Serial.println(ret); return; } if (!(c%25)) { // Serial.print(np); Serial.print(" "); Serial.print(err_c); Serial.print(" "); Serial.print(err_o); //Serial.print(" Yaw: "); Serial.print(mympu.ypr[0]);   Serial.print(','); //Serial.print(" Pitch: "); Serial.print(mympu.ypr[1]);   Serial.print(','); //Serial.print(" Roll: "); Serial.println(mympu.ypr[2]); //Serial.print("\tgy: "); Serial.print(mympu.gyro[0]); //Serial.print(" gp: "); Serial.print(mympu.gyro[1]); //Serial.print(" gr: "); Serial.println(mympu.gyro[2]); } }

良い質問の評価を上げる

以下のような質問は評価を上げましょう

  • 質問内容が明確
  • 自分も答えを知りたい
  • 質問者以外のユーザにも役立つ

評価が高い質問は、TOPページの「注目」タブのフィードに表示されやすくなります。

気になる質問をクリップする

クリップした質問は、後からいつでもマイページで確認できます。

またクリップした質問に回答があった際、通知やメールを受け取ることができます。

teratailでは下記のような質問を「具体的に困っていることがない質問」、「サイトポリシーに違反する質問」と定義し、推奨していません。

  • プログラミングに関係のない質問
  • やってほしいことだけを記載した丸投げの質問
  • 問題・課題が含まれていない質問
  • 意図的に内容が抹消された質問
  • 過去に投稿した質問と同じ内容の質問
  • 広告と受け取られるような投稿

評価を下げると、トップページの「アクティブ」「注目」タブのフィードに表示されにくくなります。

まだ回答がついていません

会員登録して回答してみよう

15分調べてもわからないことは
teratailで質問しよう!

ただいまの回答率
87.20%

質問をまとめることで
思考を整理して素早く解決

テンプレート機能で
簡単に質問をまとめる

質問する

関連した質問

同じタグがついた質問を見る

Arduino

Arduinoは、AVRマイコン、単純なI/O(入出力)ポートを備えた基板、C言語を元としたArduinoのプログラム言語と、それを実装した統合開発環境から構成されたシステムです。