M5StickC(MPU6886)+ENV Hatで姿勢推定する(Kalmanフィルタ編)

こちらの記事は
[M5StickC(MPU6886)+ENV Hatで6+3軸IMUの値を取得する](https://qiita.com/hatt_takumi/items/a9691e5236a1cacafa7e)
[M5StickC(MPU6886)+ENV Hatで姿勢推定する(Madgwickフィルタ編)](https://qiita.com/hatt_takumi/items/3202ffbe18a0d179cd30)
の続きです。

※こちらの記事で使用しているM5StickCとENV Hatは、6軸IMUが**MPU6886**、3軸地磁気センサが**BMM150**という組み合わせです。SH200でも動くかと思いますが、動作未検証です。

#最初に
前回、M5StickC(MPU6886)+ENV Hatで、角速度、加速度、地磁気の6+3軸IMUの値を補正して、概ね正しく取得することができた。今回はこれらの値を使っての姿勢推定を行い、Kalmanフィルタを使ってみる。

#姿勢推定について
以下のサイトなどが参考になる。
行列計算の最終部分を参考に三角関数を用いて算出する。
http://watako-lab.com/2019/01/01/m5stack_sens_9axis/

#ソースコード
ソースコードは以下。
キャリブレーション後のsetAngle()と、getAngle()の第二引数には地磁気センサの値を代入しているが、正直これであってるのかはわかってないです。


<details><summary>ソースコード</summary><div>

```c
#include <M5StickC.h>
#include <Wire.h>
#include <Kalman.h>
#include "bmm150.h"
#include "bmm150_defs.h"
#include "BluetoothSerial.h"
#include "Adafruit_Sensor.h"
#include <Adafruit_BMP280.h>

BluetoothSerial bluetooth;

BMM150 bmm = BMM150();
bmm150_mag_data value_offset;
Adafruit_BMP280 bme;

float acc[3];
float accOffset[3];
float gyro[3];
float gyroOffset[3];
float mag[3];
float magOffset[3];
float magmax[3];
float magmin[3];
uint8_t setup_flag = 1;

float pitch = 0.0F;
float roll  = 0.0F;
float yaw   = 0.0F;

float kalAngleX;
float kalAngleY;
float kalAngleZ;
Kalman kalmanX;
Kalman kalmanY;
Kalman kalmanZ;
long lastMs = 0;

void setup() {
  M5.begin();
  Wire.begin(0,26);
  M5.IMU.Init();
  Serial.begin(115200);
  bluetooth.begin("M5StickC");
  calibrate6886();
  pinMode(M5_BUTTON_HOME, INPUT);

  M5.Lcd.setRotation(3);
  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.setTextSize(1);
  M5.Lcd.setCursor(40, 0);
  M5.Lcd.println("IMU TEST");
  M5.Lcd.setCursor(0, 10);
  M5.Lcd.println("  X       Y       Z");
  M5.Lcd.setCursor(0, 50);
  M5.Lcd.println("  Roll    Pitch   Yaw");

  if(bmm.initialize() == BMM150_E_ID_NOT_CONFORM) {
    Serial.println("Chip ID can not read!");
    while(1);
  } else {
    Serial.println("Initialize done!");
    delay(0);
  }
  if (!bme.begin(0x76)){
    Serial.println("Could not find a valid BMP280 sensor, check wiring!");
    while (1);
  }
  calibrateENV(10);
  lastMs = micros();
  float roll = getRoll();
  float pitch = getPitch();
  float yaw = getYaw(roll, pitch);
  kalmanX.setAngle(roll * RAD_TO_DEG);
  kalmanY.setAngle(pitch * RAD_TO_DEG);
  kalmanZ.setAngle(yaw * RAD_TO_DEG);
}

void calibrate6886(){
  float gyroSum[3];
  float accSum[3];
  int counter = 500;
  for(int i = 0; i < counter; i++){
    M5.IMU.getGyroData(&gyro[0],&gyro[1],&gyro[2]);
    M5.IMU.getAccelData(&acc[0],&acc[1],&acc[2]);
    gyroSum[0] += gyro[0];
    gyroSum[1] += gyro[1];
    gyroSum[2] += gyro[2];
    accSum[0] += acc[0];
    accSum[1] += acc[1];
    accSum[2] += acc[2];
    delay(2);
  }
  gyroOffset[0] = gyroSum[0]/counter;
  gyroOffset[1] = gyroSum[1]/counter;
  gyroOffset[2] = gyroSum[2]/counter;
  accOffset[0] = accSum[0]/counter;
  accOffset[1] = accSum[1]/counter;
  accOffset[2] = (accSum[2]/counter) - 1.0;//重力加速度1G、つまりM5ボタンが上向きで行う想定
}

void calibrateENV(uint32_t timeout)
{
  int16_t value_x_min = 0;
  int16_t value_x_max = 0;
  int16_t value_y_min = 0;
  int16_t value_y_max = 0;
  int16_t value_z_min = 0;
  int16_t value_z_max = 0;
  uint32_t timeStart = 0;

  bmm.read_mag_data();
  value_x_min = bmm.raw_mag_data.raw_datax;
  value_x_max = bmm.raw_mag_data.raw_datax;
  value_y_min = bmm.raw_mag_data.raw_datay;
  value_y_max = bmm.raw_mag_data.raw_datay;
  value_z_min = bmm.raw_mag_data.raw_dataz;
  value_z_max = bmm.raw_mag_data.raw_dataz;
  delay(100);

  timeStart = millis();

  while((millis() - timeStart) < timeout)
  {
    bmm.read_mag_data();
    if(value_x_min > bmm.raw_mag_data.raw_datax)
    {
      value_x_min = bmm.raw_mag_data.raw_datax;
    }
    else if(value_x_max < bmm.raw_mag_data.raw_datax)
    {
      value_x_max = bmm.raw_mag_data.raw_datax;
    }
    if(value_y_min > bmm.raw_mag_data.raw_datay)
    {
      value_y_min = bmm.raw_mag_data.raw_datay;
    }
    else if(value_y_max < bmm.raw_mag_data.raw_datay)
    {
      value_y_max = bmm.raw_mag_data.raw_datay;
    }
    if(value_z_min > bmm.raw_mag_data.raw_dataz)
    {
      value_z_min = bmm.raw_mag_data.raw_dataz;
    }
    else if(value_z_max < bmm.raw_mag_data.raw_dataz)
    {
      value_z_max = bmm.raw_mag_data.raw_dataz;
    }
    if(digitalRead(M5_BUTTON_HOME) == LOW){
      setup_flag = 1;
      while(digitalRead(M5_BUTTON_HOME) == LOW);
      break;
    }
    delay(100);
  }
  value_offset.x = value_x_min + (value_x_max - value_x_min)/2;
  value_offset.y = value_y_min + (value_y_max - value_y_min)/2;
  value_offset.z = value_z_min + (value_z_max - value_z_min)/2;
}

void applycalibration(){
  M5.IMU.getGyroData(&gyro[0],&gyro[1],&gyro[2]);
  M5.IMU.getAccelData(&acc[0],&acc[1],&acc[2]);
  gyro[0] -= gyroOffset[0];
  gyro[1] -= gyroOffset[1];
  gyro[2] -= gyroOffset[2];
  acc[0] -= accOffset[0];
  acc[1] -= accOffset[1];
  acc[2] -= accOffset[2];

  bmm150_mag_data value;
  bmm.read_mag_data();
  value.x = bmm.raw_mag_data.raw_datax - value_offset.x;
  value.y = bmm.raw_mag_data.raw_datay - value_offset.y;
  value.z = bmm.raw_mag_data.raw_dataz - value_offset.z;
  mag[0] = value.x;
  mag[1] = value.y;
  mag[2] = value.z;
}


void loop() {
  applycalibration();

  M5.Lcd.setCursor(0, 20);
  M5.Lcd.printf("%6.2f  %6.2f  %6.2f      ", gyro[0], gyro[1], gyro[2]);//deg
  M5.Lcd.setCursor(140, 20);
  M5.Lcd.print("o/s");

  M5.Lcd.setCursor(0, 30);
  M5.Lcd.printf(" %5.2f   %5.2f   %5.2f   ", acc[0], acc[1], acc[2]);
  M5.Lcd.setCursor(140, 30);
  M5.Lcd.print("G");

  float heading = atan2(mag[0], mag[1]);
  if(heading < 0)
    heading += 2*PI;
  if(heading > 2*PI)
    heading -= 2*PI;

  M5.Lcd.setCursor(0, 40);
  M5.Lcd.printf("headingDegrees: %2.1f", heading * 180/M_PI);

  float dt = (micros() - lastMs) / 1000000.0;
  lastMs = micros();
  float roll = getRoll();
  float pitch = getPitch();
  float yaw = getYaw(roll, pitch);

  kalAngleX = kalmanX.getAngle(roll * RAD_TO_DEG, gyro[0], dt);
  kalAngleY = kalmanY.getAngle(pitch * RAD_TO_DEG, gyro[1], dt);
  kalAngleZ = kalmanZ.getAngle(yaw * RAD_TO_DEG, heading * 180/M_PI, dt);

  M5.Lcd.setCursor(0, 60);
  M5.Lcd.printf("%7.2f %7.2f %7.2f\r\n", kalAngleX, kalAngleY, kalAngleZ);

  bluetooth.printf("%6.2f, %6.2f, %6.2f\r\n", kalAngleY, kalAngleX, kalAngleZ);

  delay(10);
  if(!setup_flag){
     setup_flag = 1;
     if(bmm.initialize() == BMM150_E_ID_NOT_CONFORM) {
    while(1);
  } else {
    delay(0);
  }
  if (!bme.begin(0x76)){
    while (1);
  }
  calibrate6886();
  calibrateENV(100000);
  kalmanX.setAngle(roll * RAD_TO_DEG);
  kalmanY.setAngle(pitch * RAD_TO_DEG);
  kalmanZ.setAngle(heading * 180/M_PI);
 }
 if(digitalRead(M5_BUTTON_HOME) == LOW){
  setup_flag = 0;
  while(digitalRead(M5_BUTTON_HOME) == LOW);
 }
 M5.Lcd.setCursor(140, 70);
 if (setup_flag==0) {
   M5.Lcd.printf("CAL");
 } else {
   M5.Lcd.printf("   ");
 }
}

float getRoll(){
  return atan2(acc[1], acc[2]);
}

float getPitch(){
  return atan(-acc[0] / sqrt(acc[1]*acc[1] + acc[2]*acc[2]));
}

float getYaw(float roll, float pitch) {
  double Bfy = mag[2] * sin(roll) - mag[1] * cos(roll);
  double Bfx = mag[0] * cos(pitch) + mag[1] * sin(pitch) * sin(roll) + mag[2] * sin(pitch) * cos(roll);
  return atan2(-Bfy, Bfx);
}
```
</div></details>

#使用方法
使用するに際して、キャリブレーションを行う必要がある。
方法については以下を参照してください。
[M5StickC(MPU6886)+ENV Hatで6+3軸IMUの値を取得する](https://qiita.com/hatt_takumi/items/a9691e5236a1cacafa7e)
>＊地磁気センサは環境の影響を受けやすいようなので、必ずM5StickCをPCから少し離して行うこと。
>①画像のように、M5のボタンが上向きとなるように、机の上などの水平な面に置く。
>②本体を揺らさないよう、M5ボタンを押す。(LCD右下に"CAL"と表示される)
>③約１秒後、本体を持ち上げてRoll, Pitch, Yaw方向にまんべんなくぐるぐる回す。
>④もう一度M5ボタンを押す。

**キャリブレーション後は、M5ボタンが上になるように水平な面においてしばらく待つと、RollとPitchが0に収束していく**、ここの収束に関してはMadgwickより断然早い。
Yawに関してはMadgwickの時と違い、M5ボタンが西を向く方向が0となる。
Yaw軸回転だけなぜか収束するのが遅いのだが、ここはフィルタ内で何かゲインとかかけられるのかもしれない。

実際に使ってみた動画は以下

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">M5StickCとENV Hatで姿勢推定<br>(Kalmanフィルタ)<br>なんだかyaw回転の追従が遅い。<a href="https://twitter.com/hashtag/M5StickC?src=hash&amp;ref_src=twsrc%5Etfw">#M5StickC</a> <a href="https://twitter.com/hashtag/M5Stack?src=hash&amp;ref_src=twsrc%5Etfw">#M5Stack</a><a href="https://twitter.com/hashtag/%E3%82%A8%E3%83%B3%E3%82%B8%E3%83%8B%E3%82%A2%E3%81%A8%E7%B9%8B%E3%81%8C%E3%82%8A%E3%81%9F%E3%81%84?src=hash&amp;ref_src=twsrc%5Etfw">#エンジニアと繋がりたい</a> <a href="https://t.co/rU5oggZwD5">pic.twitter.com/rU5oggZwD5</a></p>&mdash; たくみ@スカジャンのエンジニア (@hatt_takumi) <a href="https://twitter.com/hatt_takumi/status/1254600325773713409?ref_src=twsrc%5Etfw">April 27, 2020</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>


#参考
https://lang-ship.com/reference/unofficial/M5StickC/
http://watako-lab.com/2019/01/01/m5stack_sens_9axis/
http://kako.com/blog/?p=46574
https://ambidata.io/samples/m5stack/rotatem5stack/
https://lang-ship.com/blog/work/m5stickc-imu-mpu6886/
https://qiita.com/AceZeami/items/3760317d754798d5c06c
https://qiita.com/shidash/items/ad735faf23c6148f981a
https://qiita.com/foka22ok/items/53d5271a21313e9ddcbd
https://qiita.com/Google_Homer/items/d0cdb2975491a5ea8129
https://qiita.com/coppercele/items/e4d71537a386966338d0
https://shiker.hatenablog.com/entry/2019/08/24/004637