センサのドリフトを抑えたい
Arduinoボードと9軸センサ(MPU9250)を利用してセンサの絶対角度を算出しようとしています.
(角度のみをシリアルモニタに表示させたいです)
ドリフトを抑えようとMadgwickフィルタもしくはカルマンフィルタを使用しようと考えているのですが,プログラミングの経験が浅く上手く適用することができません.
具体的にどのようにプログラムを変更すれば正しく適用できるのか教えていただきたく存じます.
現在使用しているプログラム
現在は以下のプログラムを使用しています.
インターネット上に上がっていたプログラムをほとんど引用して使用しています.
Arduino
1#include <Wire.h> 2#include <Servo.h> 3#define SerialPort Serial 4#include <SparkFunMPU9250-DMP.h> 5MPU9250_DMP imu; 6double roll , pitch, yaw; 7long int pre_ts=0; 8 9void setup() 10{ 11 12 { 13 SerialPort.begin(115200); 14 15 if (imu.begin() != INV_SUCCESS) 16 { 17 while (1) 18 { 19 SerialPort.println("Unable to communicate with MPU-9250"); 20 SerialPort.println("Check connections, and try again."); 21 SerialPort.println(); 22 delay(3000); 23 } 24 } 25 26 27 imu.setSensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS); 28 29 30 imu.setGyroFSR(250); // Set gyro to 2000 dps 31 // Accel options are +/- 2, 4, 8, or 16 g 32 imu.setAccelFSR(2); // Set accel to +/-2g 33 imu.setLPF(10); // Set LPF corner frequency to 5Hz 34 imu.setSampleRate(10); // Set sample rate to 10Hz 35 imu.setCompassSampleRate(50); // Set mag rate to 10Hz 36} 37 38 pre_ts=millis(); 39} 40 41//} 42void loop() 43{ 44 if ( imu.dataReady() ) 45 { 46 imu.update(UPDATE_ACCEL | UPDATE_GYRO | UPDATE_COMPASS); 47 printIMUData(millis()-pre_ts); 48 pre_ts=millis(); 49 } 50} 51 52void printIMUData(long int dt) 53{ 54 55 float accelX = imu.calcAccel(imu.ax); 56 float accelY = imu.calcAccel(imu.ay); 57 float accelZ = imu.calcAccel(imu.az); 58 float gyroX = imu.calcGyro(imu.gx)/57.3; 59 float gyroY = imu.calcGyro(imu.gy)/57.3; 60 float gyroZ = imu.calcGyro(imu.gz)/57.3; 61 float magX = imu.calcMag(imu.mx); 62 float magY = imu.calcMag(imu.my); 63 float magZ = imu.calcMag(imu.mz); 64 65 66 pitch = atan2 (accelY ,( sqrt ((accelX * accelX) + (accelZ * accelZ)))); 67 roll = atan2(-accelX ,( sqrt((accelY * accelY) + (accelZ * accelZ)))); 68 69 // yaw from mag 70 71 float Yh = (magY * cos(roll)) - (magZ * sin(roll)); 72 float Xh = (magX * cos(pitch))+(magY * sin(roll)*sin(pitch)) + (magZ * cos(roll) * sin(pitch)); 73 74 yaw = atan2(Yh, Xh); 75 76 77 78 roll = roll*57.3; 79 pitch = pitch*57.3; 80 yaw = yaw*57.3; 81 82 // -180~180 to 0~360 83 roll = roll + 180; 84 pitch = pitch + 180; 85 yaw = yaw + 180; 86 87 Serial.print(pitch); 88 Serial.print(','); 89 Serial.print(roll); 90 Serial.print(','); 91 Serial.println(yaw); 92 93 94}
お聞きしたいこと
上記プログラムにどのようにフィルタを適用すればよいのか
もしくは
上記プログラムは関係なくフィルタを適用した上での角度算出ができるプログラム
をお伺いしたいです.
知識不足な部分が多いかと思いますがよろしくお願いします.
追記
DMPを利用した角度を算出するプログラムを見つけることができたのですが,絶対角度で算出することができません.
こちらのコードから絶対角度を算出する方法があれば教えていただきたいです.
(加速度,ジャイロ,地磁気の値を取り出せれば回転行列によって求められるのですが,,,)
以下にそのコードを添付いたします.(必要ないものはコメントアウトしています)
参照サイト
Arduino
1#include "freeram.h" 2 3#include "mpu.h" 4#include "I2Cdev.h" 5 6int ret; 7void setup() { 8 Fastwire::setup(400,0); 9 Serial.begin(9600); 10 ret = mympu_open(200); 11 //Serial.print("MPU init: "); Serial.println(ret); 12 //Serial.print("Free mem: "); Serial.println(freeRam()); 13 14} 15 16unsigned int c = 0; //cumulative number of successful MPU/DMP reads 17unsigned int np = 0; //cumulative number of MPU/DMP reads that brought no packet back 18unsigned int err_c = 0; //cumulative number of MPU/DMP reads that brought corrupted packet 19unsigned int err_o = 0; //cumulative number of MPU/DMP reads that had overflow bit set 20 21void loop() { 22 ret = mympu_update(); 23 24 switch (ret) { 25 case 0: c++; break; 26 case 1: np++; return; 27 case 2: err_o++; return; 28 case 3: err_c++; return; 29 default: 30 Serial.print("READ ERROR! "); 31 Serial.println(ret); 32 return; 33 } 34 35 if (!(c%25)) { 36 // Serial.print(np); Serial.print(" "); Serial.print(err_c); Serial.print(" "); Serial.print(err_o); 37 //Serial.print(" Yaw: "); 38 Serial.print(mympu.ypr[0]); 39 Serial.print(','); 40 //Serial.print(" Pitch: "); 41 Serial.print(mympu.ypr[1]); 42 Serial.print(','); 43 //Serial.print(" Roll: "); 44 Serial.println(mympu.ypr[2]); 45 46 47 //Serial.print("\tgy: "); Serial.print(mympu.gyro[0]); 48 //Serial.print(" gp: "); Serial.print(mympu.gyro[1]); 49 //Serial.print(" gr: "); Serial.println(mympu.gyro[2]); 50 } 51} 52

下記のような回答は推奨されていません。
このような回答には修正を依頼しましょう。
また依頼した内容が修正された場合は、修正依頼を取り消すようにしましょう。
2019/05/03 09:03