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

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

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

行動規範の内容に同意します

回答1件

私もジンバルを2年前に作った時、このカルマンフィルタに直面しました。
私が使っていたのは6軸センサのmpu6050だったので、9軸も同じようにとはいかないと思いますが・・・一様下記のようなコードが落ちていました。

C
1    float Q_angle  =  0.001;
2    float Q_gyro   =  0.003;
3    float R_angle  =  0.03; 
4    float x_angle;
5    float x_bias = 0;
6    float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
7    float  y, S;
8    float K_0, K_1;
9    float dt=0.005;
10    float kal_deg;
11
12float kalman(float newAngle, float newRate){
13                
14        x_angle += dt * (newRate - x_bias);
15       
16        P_00 +=  dt * (dt * P_11 - P_01 - P_10 + Q_angle);
17        P_01 -=  dt * P_11;
18        P_10 -=  dt * P_11;
19        P_11 +=  Q_gyro * dt;
20
21        y = newAngle - x_angle;
22        S = P_00 + R_angle;
23        K_0 = P_00 / S;
24        K_1 = P_10 / S;
25
26        x_angle +=  K_0 * y;
27        x_bias  +=  K_1 * y;
28        P_00 -= K_0 * P_00;
29        P_01 -= K_0 * P_01;
30        P_10 -= K_1 * P_00;
31        P_11 -= K_1 * P_01;
32
33    return x_angle;
34}
35

newAngleに加速度センサで得られた角度を、newRateには角速度を積分して得られた角度を入れるそうです。dtにはサンプリングタイムですね。他のパラメータ(ゲイン)のQやRなどはmpu6050ではこの値がベストらしいです。

正しいかどうかはカルマンフィルタの理論がうまく理解できてないため、分からないです。（試行錯誤しながら実験した感じではノイズがきちんと除去されていました。ちなみに、カルマンフィルタ自体はノイズに含まれている誤差が平均値0の白色ノイズという前提らしいです。)

私は制御工学を途中で挫折したので全然詳しくありませんし、これが答えになってるかどうかはわかりませんが頑張ってください。

-追記-

mpu9250では自身の姿勢角度を計算してから出力できるようになってるらしいので、DMPライブラリを使用してみてはいかがでしょうか。
こちらの記事を参考にするといいかもしれません。

投稿2019/05/02 18:02

編集2019/05/02 18:17

konataro

総合スコア37

回答へのコメント

tannu

2019/05/03 09:03

お返事ありがとうございます．以前はmpu6050を使用していたのですが，重力方向周りの絶対角を求めたかったためにこちらのセンサを使用しております．自分も制御工学などは勉強中なので苦しんでおります，，，追記部分の記事を参考にさせていただいたのですが，自分はarduino unoを使用しているためメモリ不足でコンパイル出来ませんでした，， DMPについては少し調べてみようと思います．参考になるご意見ありがとうございました．

行動規範の内容に同意します