GY-521使用時の相補フィルターとカルマンフィルター

電子工作の実験結果。 2024年の7月頃にちょっと試した結果なのだが、全然まとめていないのでここで供養。GY-521型6軸センサーとArduino Uno R3で相補フィルターとカルマンフィルターの実験をした。

参考にしたサイトは以下のサイトと、あと2, 3のサイト*1。

arduinomakesiteasy.com

実験結果

実験のセットアップは下の写真の通り。ブレットボードに乗せたセンサーを手でくるくる回して安定性を評価した。

Arduino Uno R3とGY-521を使用した実験風景。

まずは相補フィルターの実験結果。ジャイロセンサー(黄色)のオフセットの影響を十分排除できている。

相補フィルター適用時(赤色)のジャイロセンサー(黄色)と加速度センサー(青色)の出力結果

つぎにカルマンフィルターの実験結果。こちらもジャイロセンサー(黄色)のオフセットの影響を十分排除できている。しかも、相補フィルターと比較してみると、加速度センサーの突発的な振動に対しては反応せず、安定した出力を返していることがわかる。もちろん相補フィルターでもフィルター定数の調整で同様の出力結果を出せそうだが、無調整のカルマンフィルターでここまでの出力ができたのはすごい。 センサーフュージョンでカルマンフィルターが第一選択に来るのも納得。

相補フィルター適用時(赤色)のジャイロセンサー(黄色)と加速度センサー(青色)の出力結果

コードは以下に記載する*2。

相補フィルターを使用したときのArduinoスケッチ

// MPU6050 on ESP8266

#include <Wire.h>
#include <math.h>

#define MPU6050_ADDR 0x68
#define MPU6050_AX  0x3B
#define MPU6050_AY  0x3D
#define MPU6050_AZ  0x3F
#define MPU6050_TP  0x41
#define MPU6050_GX  0x43
#define MPU6050_GY  0x45
#define MPU6050_GZ  0x47

short int AccX, AccY, AccZ;
short int Temp;
short int GyroX, GyroY, GyroZ;

float AccXmeas, AccYmeas, AccZmeas;

float angleX_acc, angleY_acc;
float angleX_gyro = 0.0, angleY_gyro = 0.0;
float angleX_comp = 0.0;
unsigned long prevTime = 0;

float gyroX_offset = 0.0, gyroY_offset = 0.0, gyroZ_offset = 0.0;

// sampling time in milliseconds
float sample_wait = 200.0;

// cut off frequency in Hz
float cutoff_freq = 1.0;

// low pass time constant in seconds
float tc = 1.0 / (2.0*M_PI*cutoff_freq);

// 相補フィルターの定数(LPF定数)、Accelに適用
const float alpha = (sample_wait/1000) / ((sample_wait/1000) + tc);

// Furntion to write a byte to a register
uint8_t MPU6050_WriteRegister_Byte (int MPUAdd, int Reg, uint8_t Value){

  Wire.beginTransmission(MPUAdd);
  Wire.write(Reg);  // Register for writing
  Wire.write(Value);  // Write value     
  Wire.endTransmission(true);
}

void calculateGyroOffsets() {
  int numReadings = 1000;
  long gyroX_sum = 0, gyroY_sum = 0, gyroZ_sum = 0;
  
  for (int i = 0; i < numReadings; i++) {
    Wire.beginTransmission(MPU6050_ADDR);
    Wire.write(MPU6050_AX);
    Wire.endTransmission();  
    Wire.requestFrom(MPU6050_ADDR, 14);

    Wire.read(); Wire.read(); // skip AccX
    Wire.read(); Wire.read(); // skip AccY
    Wire.read(); Wire.read(); // skip AccZ
    Wire.read(); Wire.read(); // skip Temp
    gyroX_sum += (Wire.read() << 8) | Wire.read();
    gyroY_sum += (Wire.read() << 8) | Wire.read();
    gyroZ_sum += (Wire.read() << 8) | Wire.read();

    delay(5);
  }

  gyroX_offset = gyroX_sum / numReadings;
  gyroY_offset = gyroY_sum / numReadings;
  gyroZ_offset = gyroZ_sum / numReadings;
}

void setup() {
  Serial.begin(9600);
//  Serial.println("*** started");
  Wire.begin();
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission();

  //MPU-6050のレジスタ(26)を'6'に設定しDLPFの設定を変更する 
  MPU6050_WriteRegister_Byte (MPU6050_ADDR, 26 ,6);

  calculateGyroOffsets();

}

void loop() {
  unsigned long currTime = millis();
  float dt = (currTime - prevTime) / 1000.0;
  prevTime = currTime;

  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(MPU6050_AX);
  Wire.endTransmission();  
  Wire.requestFrom(MPU6050_ADDR, 14);
  
  AccX = Wire.read() << 8;  AccX |= Wire.read();
  AccY = Wire.read() << 8;  AccY |= Wire.read();
  AccZ = Wire.read() << 8;  AccZ |= Wire.read();
  Temp = Wire.read() << 8;  Temp |= Wire.read();  
  GyroX = Wire.read() << 8; GyroX |= Wire.read();
  GyroY = Wire.read() << 8; GyroY |= Wire.read();
  GyroZ = Wire.read() << 8; GyroZ |= Wire.read();

  GyroX -= gyroX_offset;
  GyroY -= gyroY_offset;
  GyroZ -= gyroZ_offset;
  
// Full Scale Range +/-2g のとき 16384 LSB/gのため16384で割る
// AccX, AccY, AccZはADC出力ビット数のため、AccXmeas, AccYmeas, AccZmeasのg単位へ変換する
  AccXmeas = AccX / 16384.0;
  AccYmeas = AccY / 16384.0;
  AccZmeas = AccZ / 16384.0;

  float temperature = (Temp / 340) + 36.53;

  angleX_acc = atan2(AccY, sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180.0 / M_PI;
  angleY_acc = atan2(-AccX, sqrt(pow(AccY, 2) + pow(AccZ, 2))) * 180.0 / M_PI;

// Full Scale Range +/-250deg./s のとき 131.0 LSB/(deg./s)のため131で割る
  angleX_gyro += GyroX * dt / 131.0;
  angleY_gyro += GyroY * dt / 131.0;

  // 相補フィルターの適用
  angleX_comp = (1 - alpha) * (angleX_comp + GyroX * dt / 131.0) + (alpha) * angleX_acc;

  Serial.print("AccX_Angle[deg.]:"); Serial.print(angleX_acc); Serial.print(",");
  Serial.print("GyroX_Angle[deg.]:"); Serial.print(angleX_gyro); Serial.print(",");
  Serial.print("CompX_Angle[deg.]:"); Serial.print(angleX_comp); Serial.print("");
  Serial.println("");
  delay(sample_wait);
}

カルマンフィルターを使用したときのArduinoスケッチ

// MPU6050 on ESP8266

#include <Wire.h>
#include <math.h>

#define MPU6050_ADDR 0x68
#define MPU6050_AX  0x3B
#define MPU6050_AY  0x3D
#define MPU6050_AZ  0x3F
#define MPU6050_TP  0x41
#define MPU6050_GX  0x43
#define MPU6050_GY  0x45
#define MPU6050_GZ  0x47

short int AccX, AccY, AccZ;
short int Temp;
short int GyroX, GyroY, GyroZ;

float AccXmeas, AccYmeas, AccZmeas;

float angleX_acc, angleY_acc;
float angleX_gyro = 0.0, angleY_gyro = 0.0;
float angleX_kalman = 0.0;
unsigned long prevTime = 0;

float gyroX_offset = 0.0, gyroY_offset = 0.0, gyroZ_offset = 0.0;

// カルマンフィルター用の変数
float Q_angle = 0.001; // 加速度計による角度測定の誤差共分散
float Q_bias = 0.003;  // ジャイロバイアスの誤差共分散
float R_measure = 0.03; // 測定の誤差共分散

float angle = 0.0; // 角度
float bias = 0.0;  // バイアス
float P[2][2] = {{0, 0}, {0, 0}}; // 誤差共分散行列
float rate = 0.0; // 角速度

// sampling time in milliseconds
float sample_wait = 200.0;

// 関数: レジスタにバイトを書き込む
uint8_t MPU6050_WriteRegister_Byte (int MPUAdd, int Reg, uint8_t Value) {
  Wire.beginTransmission(MPUAdd);
  Wire.write(Reg);  // 書き込み先レジスタ
  Wire.write(Value);  // 書き込む値
  Wire.endTransmission(true);
}

void calculateGyroOffsets() {
  int numReadings = 1000;
  long gyroX_sum = 0, gyroY_sum = 0, gyroZ_sum = 0;
  
  for (int i = 0; i < numReadings; i++) {
    Wire.beginTransmission(MPU6050_ADDR);
    Wire.write(MPU6050_AX);
    Wire.endTransmission();  
    Wire.requestFrom(MPU6050_ADDR, 14);

    Wire.read(); Wire.read(); // AccXスキップ
    Wire.read(); Wire.read(); // AccYスキップ
    Wire.read(); Wire.read(); // AccZスキップ
    Wire.read(); Wire.read(); // Tempスキップ
    gyroX_sum += (Wire.read() << 8) | Wire.read();
    gyroY_sum += (Wire.read() << 8) | Wire.read();
    gyroZ_sum += (Wire.read() << 8) | Wire.read();

    delay(5);
  }

  gyroX_offset = gyroX_sum / numReadings;
  gyroY_offset = gyroY_sum / numReadings;
  gyroZ_offset = gyroZ_sum / numReadings;
}

void setup() {
  Serial.begin(9600);
  Wire.begin();
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission();

  // MPU-6050のレジスタ(26)を'6'に設定しDLPFの設定を変更する
  MPU6050_WriteRegister_Byte (MPU6050_ADDR, 26 ,6);

  calculateGyroOffsets();
}

void loop() {
  unsigned long currTime = millis();
  float dt = (currTime - prevTime) / 1000.0;
  prevTime = currTime;

  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(MPU6050_AX);
  Wire.endTransmission();  
  Wire.requestFrom(MPU6050_ADDR, 14);
  
  AccX = Wire.read() << 8;  AccX |= Wire.read();
  AccY = Wire.read() << 8;  AccY |= Wire.read();
  AccZ = Wire.read() << 8;  AccZ |= Wire.read();
  Temp = Wire.read() << 8;  Temp |= Wire.read();  
  GyroX = Wire.read() << 8; GyroX |= Wire.read();
  GyroY = Wire.read() << 8; GyroY |= Wire.read();
  GyroZ = Wire.read() << 8; GyroZ |= Wire.read();

  GyroX -= gyroX_offset;
  GyroY -= gyroY_offset;
  GyroZ -= gyroZ_offset;
  
  // Full Scale Range +/-2g のとき 16384 LSB/gのため16384で割る
  // AccX, AccY, AccZはADC出力ビット数のため、AccXmeas, AccYmeas, AccZmeasのg単位へ変換する
  AccXmeas = AccX / 16384.0;
  AccYmeas = AccY / 16384.0;
  AccZmeas = AccZ / 16384.0;

  float temperature = (Temp / 340.0) + 36.53;

  angleX_acc = atan2(AccY, sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180.0 / M_PI;
  angleY_acc = atan2(-AccX, sqrt(pow(AccY, 2) + pow(AccZ, 2))) * 180.0 / M_PI;

  // Full Scale Range +/-250deg./s のとき 131.0 LSB/(deg./s)のため131で割る
  angleX_gyro += GyroX * dt / 131.0;
  angleY_gyro += GyroY * dt / 131.0;

  // カルマンフィルターの適用
  // rateは角速度deg./s
  float rate = GyroX / 131.0 - bias;
  angleX_kalman += dt * rate;
  
  P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);
  P[0][1] -= dt * P[1][1];
  P[1][0] -= dt * P[1][1];
  P[1][1] += Q_bias * dt;
  
  float S = P[0][0] + R_measure;
  float K[2];
  K[0] = P[0][0] / S;
  K[1] = P[1][0] / S;
  
  float y = angleX_acc - angleX_kalman;
  angleX_kalman += K[0] * y;
  bias += K[1] * y;
  
  float P00_temp = P[0][0];
  float P01_temp = P[0][1];
  
  P[0][0] -= K[0] * P00_temp;
  P[0][1] -= K[0] * P01_temp;
  P[1][0] -= K[1] * P00_temp;
  P[1][1] -= K[1] * P01_temp;

  Serial.print("AccX_Angle[deg.]:"); Serial.print(angleX_acc); Serial.print(",");
  Serial.print("GyroX_Angle[deg.]:"); Serial.print(angleX_gyro); Serial.print(",");
  Serial.print("KalmanX_Angle[deg.]:"); Serial.print(angleX_kalman); Serial.println("");

  delay(sample_wait);
}

*1:もう覚えていないけれど、GY-521 カルマンフィルターなどで検索したサイトだったと思う

*2:GitHubのアカウントを作ってアップロードしておきたい