【SPRESENSE】BMI160用のセンサーフュージョン・フィルターを検討する（２）

前回、SPRESENSEでセンサーフュージョン用のフィルターを検討しましたが、グラフでは効果がよく分からないので、今回は少し工夫をこらして Processing で表示しながら反応を見ることにしました。





Processingで作ったプログラムのグラフィック画面を次のようにしてみました。





Spresense のスケッチは次のようなものです。前回のものと出力形式以外は全く変わっていません。

#include <Wire.h>
#include <math.h>
#include <BMI160Gen.h>
#include <Kalman.h>
#include <MadgwickAHRS.h>

#define PRINT_PROCESSING

#define BAUDRATE  115200
#define SENSE_RATE   100
#define GYRO_RANGE   250
#define ACCL_RANGE     2

#define deg_to_rad(a) (a/180*M_PI)
#define rad_to_deg(a) (a/M_PI*180)

Kalman kalmanRoll;
Kalman kalmanPitch;
Madgwick madgwick;

float convertRawGyro(int gRaw) {
  // ex) if the range is +/-500 deg/s: +/-32768/500 = +/-65.536 LSB/(deg/s)
  float lsb_omega = float(0x7FFF) / GYRO_RANGE;
  return gRaw / lsb_omega;  // deg/sec
}

float convertRawAccel(int aRaw) {
  // ex) if the range is +/-2g ; +/-32768/2 = +/-16384 LSB/g
  float lsb_g = float(0x7FFF) / ACCL_RANGE;
  return aRaw / lsb_g;
}

static float gyro_roll = 0, gyro_pitch = 0, gyro_yaw = 0;
static float comp_roll = 0, comp_pitch = 0;
static unsigned long last_mills = 0; 
void print_roll_pitch() 
{
     
  // read raw accl measurements from device
  int rawXAcc, rawYAcc, rawZAcc; // x, y, z
  BMI160.readAccelerometer(rawXAcc, rawYAcc, rawZAcc);
  float accX = convertRawAccel(rawXAcc);
  float accY = convertRawAccel(rawYAcc);
  float accZ = convertRawAccel(rawZAcc);

  float rad_a_roll = atan2(accY, accZ);
  float rad_a_pitch = atan2(-accX, sqrt(accY*accY + accZ*accZ));
  float accl_roll = rad_to_deg(rad_a_roll);
  float accl_pitch = rad_to_deg(rad_a_pitch);

  // read raw gyro measurements from device
  int rawRoll, rawPitch, rawYaw; // roll, pitch, yaw
  BMI160.readGyro(rawRoll, rawPitch, rawYaw);
  float omega_roll  = convertRawGyro(rawRoll);
  float omega_pitch = convertRawGyro(rawPitch);
  float omega_yaw   = convertRawGyro(rawYaw);
  
  unsigned long cur_mills = micros();
  unsigned long duration = cur_mills - last_mills;
  last_mills = cur_mills;
  double dt = duration / 1000000.0; // us->s  
  if (dt > 0.1) return;

  // Gyro data
  gyro_roll  += omega_roll  * dt; // (ms->s) omega x time = degree
  gyro_pitch += omega_pitch * dt;
  gyro_yaw   += omega_yaw   * dt;
  
  // Complementary filter data
  comp_roll  = 0.93 * (comp_roll  + omega_roll  * dt) + 0.07 * accl_roll; 
  comp_pitch = 0.93 * (comp_pitch + omega_pitch * dt) + 0.07 * accl_pitch; 

  // Kalman filter data
  float kalm_roll  = kalmanRoll.getAngle(accl_roll, omega_roll, dt);
  float kalm_pitch = kalmanPitch.getAngle(accl_pitch, omega_pitch, dt); 

  // Madgwick filter data
  madgwick.updateIMU2(omega_roll, omega_pitch, omega_yaw, accX, accY, accZ, dt);
  // madgwick.updateIMU(omega_roll, omega_pitch, omega_yaw, accX, accY, accZ);
  float madw_roll  = madgwick.getRoll();
  float madw_pitch = madgwick.getPitch();
  float madw_yaw   = madgwick.getYaw();

#ifdef PRINT_PROCESSING
  static int n = 0;
  if (n != 50) {
    ++n; return; 
  }
  n = 0;

  Serial.print(accl_roll);
  Serial.print(",");
  Serial.print(accl_pitch);
  Serial.print(",");
  Serial.print(madw_yaw);
  Serial.print(",");
  Serial.print(gyro_roll);
  Serial.print(",");
  Serial.print(gyro_pitch);
  Serial.print(",");
  Serial.print(madw_yaw);
  Serial.print(",");
  Serial.print(comp_roll);
  Serial.print(",");
  Serial.print(comp_pitch);
  Serial.print(",");
  Serial.print(madw_yaw);
  Serial.print(",");
  Serial.print(kalm_roll);
  Serial.print(",");
  Serial.print(kalm_pitch);
  Serial.print(",");
  Serial.print(madw_yaw);
  Serial.print(",");
  Serial.print(madw_roll);
  Serial.print(",");
  Serial.print(madw_pitch);
  Serial.print(",");
  Serial.print(madw_yaw);
  Serial.print(",");
#endif
  Serial.println();
}


void setup() {
  Serial.begin(115200);
  BMI160.begin(BMI160GenClass::I2C_MODE);
  madgwick.begin(SENSE_RATE);

  BMI160.setGyroRate(SENSE_RATE);
  BMI160.setAccelerometerRate(SENSE_RATE);
  BMI160.setGyroRange(GYRO_RANGE);
  BMI160.setAccelerometerRange(ACCL_RANGE);

  BMI160.autoCalibrateGyroOffset();
  BMI160.autoCalibrateAccelerometerOffset(X_AXIS, 0);
  BMI160.autoCalibrateAccelerometerOffset(Y_AXIS, 0);
  BMI160.autoCalibrateAccelerometerOffset(Z_AXIS, 1);

  delay(100);

  /* Set kalman and gyro starting angle */
  int rawXAcc, rawYAcc, rawZAcc;
  BMI160.readAccelerometer(rawXAcc, rawYAcc, rawZAcc);
  float accX = convertRawAccel(rawXAcc);
  float accY = convertRawAccel(rawYAcc);
  float accZ = convertRawAccel(rawZAcc);
  
  float roll  = rad_to_deg(atan(accY / accZ));
  float pitch = rad_to_deg(atan(-accX / sqrt(accY * accY + accZ * accZ)));

  // Set starting angle
  kalmanRoll.setAngle(roll); 
  kalmanPitch.setAngle(pitch);
  gyro_roll  = comp_roll  = roll;
  gyro_pitch = gyro_pitch = pitch;
}

void loop() {
  print_roll_pitch();
}

Processing のスケッチは次のようになっています。

import processing.serial.*;
Serial myPort;  // Create object from Serial class

float [] Acc = new float[3];
float [] Gyr = new float[3];
float [] Cmp = new float[3];
float [] Klm = new float[3];
float [] Mdw = new float[3];

int lf = 10; // 10 is '\n' in ASCII
byte[] inBuffer = new byte[1024];

PFont font;
final int VIEW_SIZE_X = 1100, VIEW_SIZE_Y = 600;

void settings() {
  size(VIEW_SIZE_X, VIEW_SIZE_Y, P3D);
}  

void setup() {
  myPort = new Serial(this, "COM15", 115200); 
  font = loadFont("CourierNew36.vlw"); 
}


void readSensors() {
  if(myPort.available() > 0) {
    if (myPort.readBytesUntil(lf, inBuffer) > 0) {
      String inputString = new String(inBuffer);
      String [] inputStringArr = split(inputString, ',');
      
      Acc[0] = float(inputStringArr[0]);
      Acc[1] = float(inputStringArr[1]);
      Acc[2] = float(inputStringArr[2]);
      Gyr[0] = float(inputStringArr[3]);
      Gyr[1] = float(inputStringArr[4]);
      Gyr[2] = float(inputStringArr[5]);
      Cmp[0] = float(inputStringArr[6]);
      Cmp[1] = float(inputStringArr[7]);
      Cmp[2] = float(inputStringArr[8]);
      Klm[0] = float(inputStringArr[9]);
      Klm[1] = float(inputStringArr[10]);
      Klm[2] = float(inputStringArr[11]);
      Mdw[0] = float(inputStringArr[12]);
      Mdw[1] = float(inputStringArr[13]);
      Mdw[2] = float(inputStringArr[14]);
    }
  }
}


void normalize3DVec(float [] vector) {
  float R;
  R = sqrt(vector[0]*vector[0] + vector[1]*vector[1] + vector[2]*vector[2]);
  vector[0] /= R;
  vector[1] /= R;  
  vector[2] /= R;  
}


void buildBoxShape() {
  //box(60, 10, 40);
  noStroke();
  beginShape(QUADS);
  
  //Z+ (to the drawing area)
  fill(#00ff00);
  vertex(-30, -5, 20);
  vertex(30, -5, 20);
  vertex(30, 5, 20);
  vertex(-30, 5, 20);
  
  //Z-
  fill(#0000ff);
  vertex(-30, -5, -20);
  vertex(30, -5, -20);
  vertex(30, 5, -20);
  vertex(-30, 5, -20);
  
  //X-
  fill(#ff0000);
  vertex(-30, -5, -20);
  vertex(-30, -5, 20);
  vertex(-30, 5, 20);
  vertex(-30, 5, -20);
  
  //X+
  fill(#ffff00);
  vertex(30, -5, -20);
  vertex(30, -5, 20);
  vertex(30, 5, 20);
  vertex(30, 5, -20);
  
  //Y-
  fill(#ff00ff);
  vertex(-30, -5, -20);
  vertex(30, -5, -20);
  vertex(30, -5, 20);
  vertex(-30, -5, 20);
  
  //Y+
  fill(#00ffff);
  vertex(-30, 5, -20);
  vertex(30, 5, -20);
  vertex(30, 5, 20);
  vertex(-30, 5, 20);
  
  endShape();
}


void drawCube(int n, float vec[]) {  
  pushMatrix();
    translate(n, 450, 0);
    scale(3,3,3);
    
    rotateX(HALF_PI * vec[0]);
    rotateZ(HALF_PI * -vec[1]);
    
    buildBoxShape();
  popMatrix();
}

void draw() {  
  readSensors();
  normalize3DVec(Acc);
  normalize3DVec(Gyr);
  normalize3DVec(Cmp);
  normalize3DVec(Klm);
  normalize3DVec(Mdw);
  
  background(#000000);
  fill(#ffffff);

  textFont(font, 24);
  text("Sensor Fusion Test using Spresense with BMI160", 180, 100);
  
  textFont(font, 18);
  text("Accelerometer :\n" + Acc[0] + "\n" + Acc[1], 50, 180);
  drawCube(150, Acc);

  text("Gyroscope :\n" + Gyr[0] + "\n" + Gyr[1], 250, 180);
  drawCube(350, Gyr);
  
  text("Comp. Filter :\n" + Cmp[0] + "\n" + Cmp[1], 450, 180);
  drawCube(550, Cmp);

  text("Kalman Filter :\n" + Klm[0] + "\n" + Klm[1], 650, 180);
  drawCube(750, Klm);
  
  text("Madgwick Filter :\n" + Mdw[0] + "\n" + Mdw[1], 850, 180);
  drawCube(950, Mdw);
}

百聞は一見にしかず、ぜひ動画で見てみてください。どのフィルタがどのような性質を持っているかグラフィックで見てみるとひと目で分かります！ 😀
（＾＾）／～

---

倒立振子で学ぶ制御工学

• 出版社/メーカー: 森北出版
• 発売日: 2017/03/03
• メディア: 単行本（ソフトカバー）

SONY SPRESENSE メインボード CXD5602PWBMAIN1

• 出版社/メーカー: スプレッセンス(Spresense)
• メディア: Tools & Hardware

SODIAL 6軸レートジャイロ BMI160 6自由度重力加速度センサー モジュールIIC SPIブロード

• 出版社/メーカー: SODIAL
• メディア: その他

---