倒立振子を作ろう ~傾斜計作成編~

# 初めに
前回の記事にて9軸センサから出力されるノイズは正規分布に則ったものであり、カルマンフィルタを適用できることが確認できた。今回9軸センサの出力データにカルマンフィルタを適用し、自身がどれだけ傾いているかを計測する傾斜計の作成を行う。カルマンフィルタが適用されているかどうかは前回同様傾斜計を(ある程度)平らな箇所に置いた角度の測定から求められるか？

# 前提
カルマンフィルタを使用するためにはモデルが必要であることは前の記事にて話した通りである。
今回作成する傾斜計の前提は以下のとおりである。
1. 倒立振子を作成するための傾斜計である
1. 重力加速度は下方に対して一定である
![前提.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2782192/8f7d9621-203d-cf18-fdde-8cf64dac23bc.png)

上記図におけるθが今回の傾斜計で求められる角度である。以下の式で求めることができる。

```math
\theta = \arctan(\frac{g_z}{g_y})
```

また、この傾斜計は9軸センサを使用する。以下に9軸センサの前提も示す。
1. 測定間隔は十分に短く、その時間内では測定した角速度は一定である。
1. 角速度のオフセットは時間によらず一定である。
これらの前提が守られる限りは今回作成する傾斜計は正常に動作すると考えられる。

# モデル
上記前提を元にカルマンフィルタで使用するモデルを作成する。ここで、以下に使用する変数を示す。

```math
\theta_k : 時刻kにおける先述の式にて求めた角度 \\
\omega_k : 時刻kにおけるセンサから得られた角速度 \\
\omega_{offset} : 時刻0におけるセンサから得られた角速度の平均 \\
```
これらの変数を用いて
時刻k+1の時の角度θは以下のように表すことができる。
```math
\theta_{k+1} = \theta_k + (\omega_k - \omega_{offset})\Delta t
```
もう一つの前提から
```math
\omega_{offset}(k + 1) = \omega_{offset}(k)
```
という式が得られる。
これらが今回使用するカルマンフィルタのモデルである。
これらのモデルを元にカルマンフィルタを使用した傾斜計を作成する。

# コード
いかに、今回作成したコードを挙げる。ベースは秋月電子にある9軸センサ（過去記事参照）用のコードである。また、参考としてトランジスタ技術2019年7月号を使用した。[トランジスタ技術2019年7月号](https://toragi.cqpub.co.jp/tabid/892/Default.html)

```
//================================================================//
//  AE-BMX055             Arduino UNO                             //
//    VCC                    +5V                                  //
//    GND                    GND                                  //
//    SDA                    A4(SDA)                              //
//    SCL                    A5(SCL)                              //
//                                                                //
//   (JP4,JP5,JP6はショートした状態)                                //
//   http://akizukidenshi.com/catalog/g/gK-13010/                 //
//================================================================//

#include<Wire.h>
#include<math.h>
// BMX055 加速度センサのI2Cアドレス  
#define Addr_Accl 0x19  // (JP1,JP2,JP3 = Openの時)
// BMX055 ジャイロセンサのI2Cアドレス
#define Addr_Gyro 0x69  // (JP1,JP2,JP3 = Openの時)
// BMX055 磁気センサのI2Cアドレス
#define Addr_Mag 0x13   // (JP1,JP2,JP3 = Openの時)

// 加速度、ジャイロセンサの固定値
#define SAMPLE_NUM 100
#define SAMPLE_INTERVAL 10

// delta t 微小時間t I2Cの時間
#define DELTA_T (double)(1.0/400)

double theta_array[SAMPLE_NUM];      // 角度データを100個蓄えるための配列
double omega_array[SAMPLE_NUM];     // 角速度データを100個蓄えるための配列

// センサーの値を保存するグローバル変数
double xAccl = 0.00;
double yAccl = 0.00;
double zAccl = 0.00;
double xGyro = 0.00;
double yGyro = 0.00;
double zGyro = 0.00;

// カルマンフィルタを使用するうえで必要な変数
double theta_offset = 0.00;     // 加速度センサから取得した角度データのオフセット(角度データの平均)
double theta_Var = 0.00;      // 加速度センサから取得した角度データの分散
double omega_offset = 0.00;     // ジャイロセンサから取得した角速度データのオフセット(角速度データの平均)
double omega_Var = 0.00;      // ジャイロセンサから取得した角速度データの分散

// 各種行列
// thetadata
double theta_data_predict[2][1];
double theta_data[2][1];

// 共分散行列
double P_theta_predict[2][2];
double P_theta[2][2];

//状態方程式における"A"
double A_theta[2][2] = {{1, -DELTA_T}, {0, 1}};

//状態方程式における"B"
double B_theta[2][1] = {{DELTA_T}, {0}};

//状態方程式における"C"
double C_theta[1][2] = {{1, 0}};

void setup()
{
  // Wire(Arduino-I2C)の初期化
  Wire.begin();
  // デバッグ用シリアル通信は9600bps
  Serial.begin(9600);
  //BMX055 初期化
  BMX055_Init();
  delay(SAMPLE_INTERVAL);
  
  //---------------------------------------
  // 角速度のoffset算出
  //---------------------------------------
  BMX055_Accl();
  // 100個データを蓄える
  for(int i = 0; i < SAMPLE_NUM; i++){
    theta_array[i] = get_theta();
    delay(SAMPLE_INTERVAL);
  }
  // 100個のデータの和を求める
  for(int i = 0; i < SAMPLE_NUM; i++){
    theta_offset += theta_array[i];
    delay(SAMPLE_INTERVAL);
  }
  // 角速度の平均値を算出
    theta_offset = (double)(theta_offset / SAMPLE_NUM);
  
  // 角速度の分散算出
  double temp_theta;
  for(int i = 0; i < SAMPLE_NUM; i++){
    temp_theta = theta_array[i] - theta_offset;
    theta_Var += temp_theta * temp_theta;
  }
  // 角速度の分散
  theta_Var = theta_Var / SAMPLE_NUM;
  
  //---------------------------------------
  // ジャイロのoffset算出
  //---------------------------------------
  BMX055_Gyro();
  // 100個データを蓄える
  for(int i = 0; i < SAMPLE_NUM; i++){
    omega_array[i] = xGyro;
    delay(SAMPLE_INTERVAL);
  }
  // 100個のデータの和を求める。
  for(int i = 0; i < SAMPLE_NUM; i++){
    omega_offset += omega_array[i];
    delay(SAMPLE_INTERVAL);
  }
  // 角速度の平均値を算出
    omega_offset = (double)(omega_offset / SAMPLE_NUM);
  
  // 角速度の分散算出
  double temp_omega;
  for(int i = 0; i < SAMPLE_NUM; i++){
    temp_omega = theta_array[i] - omega_offset;
    omega_Var += temp_omega * temp_omega;
  }
  // 角速度の分散
  omega_Var = omega_Var / SAMPLE_NUM;
  
  //---------------------------------------
  // カルマンフィルタの初期化
  //---------------------------------------
  // theta_data_predictの初期値
  theta_data_predict[0][0] = 0;
  theta_data_predict[1][0] = omega_offset;
  
  // P_theta_predictの初期値
  P_theta_predict[0][0] = 1;
  P_theta_predict[0][1] = 0;
  P_theta_predict[1][0] = 0;
  P_theta_predict[1][1] = omega_Var;
  
}

void loop()
{
  
  Serial.println("--------------------------------------"); 

  Serial.print("theta_data = ");
  Serial.println(theta_data[0][0]); 
  
  // カルマンフィルタによる値更新
  update_karman_theta();
  delay(SAMPLE_INTERVAL);
}

//=====================================================================================//
void BMX055_Init()
{
  //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Accl);
  Wire.write(0x0F); // Select PMU_Range register
  Wire.write(0x03);   // Range = +/- 2g
  Wire.endTransmission();
  delay(SAMPLE_INTERVAL);
 //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Accl);
  Wire.write(0x10);  // Select PMU_BW register
  Wire.write(0x08);  // Bandwidth = 7.81 Hz
  Wire.endTransmission();
  delay(SAMPLE_INTERVAL);
  //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Accl);
  Wire.write(0x11);  // Select PMU_LPW register
  Wire.write(0x00);  // Normal mode, Sleep duration = 0.5ms
  Wire.endTransmission();
  delay(SAMPLE_INTERVAL);
 //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Gyro);
  Wire.write(0x0F);  // Select Range register
  Wire.write(0x04);  // Full scale = +/- 125 degree/s
  Wire.endTransmission();
  delay(SAMPLE_INTERVAL);
 //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Gyro);
  Wire.write(0x10);  // Select Bandwidth register
  Wire.write(0x07);  // ODR = 100 Hz
  Wire.endTransmission();
  delay(SAMPLE_INTERVAL);
 //------------------------------------------------------------//
  Wire.beginTransmission(Addr_Gyro);
  Wire.write(0x11);  // Select LPM1 register
  Wire.write(0x00);  // Normal mode, Sleep duration = 2ms
  Wire.endTransmission();
  delay(SAMPLE_INTERVAL);
}
//=====================================================================================//
void BMX055_Accl()
{
  unsigned int data[6];
  for (int i = 0; i < 6; i++)
  {
    Wire.beginTransmission(Addr_Accl);
    Wire.write((2 + i));// Select data register
    Wire.endTransmission();
    Wire.requestFrom(Addr_Accl, 1);// Request 1 byte of data
    // Read 6 bytes of data
    // xAccl lsb, xAccl msb, yAccl lsb, yAccl msb, zAccl lsb, zAccl msb
    if (Wire.available() == 1)
      data[i] = Wire.read();
  }
  // Convert the data to 12-bits
  xAccl = ((data[1] * 256) + (data[0] & 0xF0)) / 16;
  if (xAccl > 2047)  xAccl -= 4096;
  yAccl = ((data[3] * 256) + (data[2] & 0xF0)) / 16;
  if (yAccl > 2047)  yAccl -= 4096;
  zAccl = ((data[5] * 256) + (data[4] & 0xF0)) / 16;
  if (zAccl > 2047)  zAccl -= 4096;
  xAccl = xAccl * 0.0098; // range = +/-2g
  yAccl = yAccl * 0.0098; // range = +/-2g
  zAccl = zAccl * 0.0098; // range = +/-2g
}

double get_theta()    // theta_kを取得する関数
{
  double theta_deg; // θ計算結果
  BMX055_Accl();    // g_y, g_zを取得
  
  //return theta_deg = atan( double(zAccl) / double(yAccl));  // 
  theta_deg = atan( double(zAccl) / double(yAccl)); // 
  return theta_deg * 57.29578f;   //degree
}

//=====================================================================================//
void BMX055_Gyro()
{
  unsigned int data[6];
  for (int i = 0; i < 6; i++)
  {
    Wire.beginTransmission(Addr_Gyro);
    Wire.write((2 + i));    // Select data register
    Wire.endTransmission();
    Wire.requestFrom(Addr_Gyro, 1);    // Request 1 byte of data
    // Read 6 bytes of data
    // xGyro lsb, xGyro msb, yGyro lsb, yGyro msb, zGyro lsb, zGyro msb
    if (Wire.available() == 1)
      data[i] = Wire.read();
  }
  // Convert the data
  xGyro = (data[1] * 256) + data[0];
  if (xGyro > 32767)  xGyro -= 65536;
  yGyro = (data[3] * 256) + data[2];
  if (yGyro > 32767)  yGyro -= 65536;
  zGyro = (data[5] * 256) + data[4];
  if (zGyro > 32767)  zGyro -= 65536;

  xGyro = xGyro * 0.0038; //  Full scale = +/- 125 degree/s
  yGyro = yGyro * 0.0038; //  Full scale = +/- 125 degree/s
  zGyro = zGyro * 0.0038; //  Full scale = +/- 125 degree/s
}
//=====================================================================================//

//---------------------------------------
// θ_k & ω_kのカルマンフィルタ
//---------------------------------------

void update_karman_theta()
{
  // get theta_k
  double theta_k = get_theta();
  
  BMX055_Gyro();
  // get omega_k
  double omega_k = xGyro;
  
  //Kalman gainの算出 G = P'C^T(W+CP'C^T)^-1
  double P_CT[2][1] = {};
  // Cの転置行列
  double trans_C_theta[2][1] = {};
  // Cの転置行列計算
  mat_trans(C_theta[0], trans_C_theta[0], 1, 2);
  mat_mul(P_theta_predict[0], trans_C_theta[0], P_CT[0], 2, 2, 2, 1); // P'C^T
  // 状態方程式のG計算
  double G_temp1[1][1] = {};
  mat_mul(C_theta[0], P_CT[0], G_temp1[0], 1, 2, 2, 1);       // CP'C^T
  double G_temp2 = 1.0f / (G_temp1[0][0] + theta_Var);        // (W + CP'C^T)^(-1)
  double G[2][1] = {};
  mat_const_mul(G_temp2, P_CT[0], G[0], 2, 1);            // P'C^T(W + CP'C^T)^(-1)
  
  //theta_dataの推定 : theta = theta' + G(y-C_theta')
  double C_theta_theta[1][1] = {};
  mat_mul(C_theta[0], theta_data_predict[0], C_theta_theta[0], 1, 2, 2, 1); // C_theta'
  double delta_y = theta_k - C_theta_theta[0][0];               // y - C_theta'
  double delta_theta[2][1] = {};
  mat_const_mul(delta_y, G[0], delta_theta[0], 2, 1);
  mat_add(theta_data_predict[0], delta_theta[0], theta_data[0], 2, 1);
  
  // 共分散行列の演算 : P = (I - GC)P'
  double GC[2][2] = {};
  // 単位行列 2x2
  double I2[2][2] = {{1,0},{0,1}};
  mat_mul(G[0], C_theta[0], GC[0], 2, 1, 1, 2);           // GC
  double I2_GC[2][2] = {};
  mat_sub(I2[0], GC[0], I2_GC[0], 2, 2);                // I - GC
  mat_mul(I2_GC[0], P_theta_predict[0], P_theta[0], 2, 2, 2, 2);    // (I - GC)P'
  
  // 次の推定データ : theta' = A_theta + Bu
  double A_theta_theta[2][1] = {};
  double B_theta_omega[2][1] = {};
  mat_mul(A_theta[0], theta_data[0], A_theta_theta[0], 2, 2, 2, 1); // A_theta
  mat_const_mul(omega_k, B_theta[0], B_theta_omega[0], 2, 1);     // Bu
  mat_add(A_theta_theta[0], B_theta_omega[0], theta_data_predict[0], 2, 1); // A_theta + Bu
  
  // 共分散行列の推定 : P' = APA^T + BUB^T
  double AP[2][2] = {};
  double APAT[2][2] = {};
  double trans_A_theta[2][2] = {};
  // A_thetaの転置行列
  mat_trans(A_theta[0], trans_A_theta[0], 2, 2);  // A^T
  // A_theta * P_thetaの行列積
  mat_mul(A_theta[0], P_theta[0], AP[0], 2, 2, 2, 2); // AP
  // AP * A^Tの行列積
  mat_mul(AP[0], trans_A_theta[0], APAT[0], 2, 2, 2, 2);  // APA^T
  double BBT[2][2];
  double trans_B_theta[1][2] = {};
  // B_thetaの転置行列
  mat_trans(B_theta[0], trans_B_theta[0], 2, 1);  // B^T
  mat_mul(B_theta[0], trans_B_theta[0], BBT[0], 2, 1, 1, 2);  // BB^T
  double BUBT[2][2] = {};
  // 定数の掛け算
  mat_const_mul(omega_Var, BBT[0], BUBT[0], 2, 2);  // BUB^T
  // APA^T + BUB^Tの行列の足し算
  mat_add(APAT[0], BUBT[0], P_theta_predict[0], 2, 2);  // APA^T + BUB^T
}

//---------------------------------------
// 行列演算
//---------------------------------------
// 転置行列の計算 (元の行列, 転置後の行列 , 行数, 列数)
void mat_trans(double *m1, double *sol, int row, int col){
    for(int i = 0; i < row; i++){
        for(int j = 0; j < col; j++){
            sol[j * row + i] = m1[i * col + j];    
        }
    }
    return 0;
}

// 行列の足し算　(足し算対象の行列1, 足し算対象の行列2, 足し算後の行列, 行数, 列数)
void mat_add(double *ml, double *m2, double *sol, int row, int col){
  for(int i = 0; i < row; i++){
    for(int j = 0; j < col; j++){
      sol[i * col + j] = ml[i * col + j] + m2[i * col + j];
    }
  }
  return 0;
}

// 行列の引き算 (引かれる方の行列1, 引く方の行列2, 引き算後の行列, 行数, 列数)
void mat_sub(double *m1, double *m2, double *sol, int row, int col){
  for(int i = 0; i < row; i++){
    for(int j = 0; j < col; j++){
      sol[i * col + j] = m1[i * col + j] - m2[i * col + j];
    }
  }
  return 0;
}

// 行列同士の掛け算 (左側から掛ける行列1, 右から掛ける行列2, 掛け算の結果行列, 行列1の行数, 行列1の列数, 行列2の行数, 行列2の列数)
void mat_mul(double *m1, double *m2, double *sol, int row1, int col1, int row2, int col2){
  for(int i = 0; i < row1; i++){
    for(int j = 0; j < col2; j++){
      sol[i * col2 + j] = 0;
      for(int k = 0; k < col1; k++){
        sol[i * col2 + j] += m1[i * col1 + k] * m2[k * col2 + j];
      }
    }
  }
  return 0;
}

// 定数と行列の掛け算 (定数, 行列1, 計算結果, 行数, 列数)
void mat_const_mul(double Const, double *m1, double *sol, int row, int col){
  for(int i = 0; i < row; i++){
    for(int j = 0; j < row; j++){
      sol[i * col + j] = Const * m1[i * col + j];
    }
  }
  return 0;
}

```

これをArduinoに入力した結果は以下のようになる。

![結果.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2782192/0014da66-6971-aba6-7e8a-6f0f1de922ac.png)

傾けるとその傾きに応じて出力値が変化していることが確認できたため、(おそらく)傾斜計は正しく動作している。

Arduinoを水平に置いたときはおおよそ90°の値が出力され、垂直に近づけるごとに角度が減少する。また、カルマンフィルタの特性から水平面においてデータを獲得し、ヒストグラムを取得しても正規分布に沿った値が出力されると考えられる。そのため、カルマンフィルタが適用されているかどうかの確認を今回は特に行っていない。