倒立振子を作ろう ~センサ確認編~

# 初めに
カルマンフィルタについて前回勉強したため、今回はセンサからデータを取得し、その取得したデータからカルマンフィルタを使用することができるセンサであるかの確認を行う。特に今回は倒立振子の自己の傾斜具合や安定度を確認するために用いる9軸センサに対してカルマンフィルタを使用できるか確認する。

# 用意したもの
1. ardiuno
1. 9軸センサ AE-BMX055

# 何するの？
ardiunoと9軸センサをI2Cにて接続し、センサを動作させる。この時センサは机の上に置いておく。こうすることで、角速度は常に一定値が真値として出てくるはずである。しかし、センサ出力には熱雑音や線上に電磁波ノイズなどが載ってしまうことから、センサの出力値は揺れるはずである。センサの真の値は一定である状態で、センサの出力値のヒストグラムを取得し、仮に正規分布に沿った出力が得られた場合はノイズが正規分布に沿っているといえる。つまり、カルマンフィルタを本センサに対して使用しても問題ないということが確認できるのである。
今回はセンサから10000個の角速度データを取得し、ヒストグラム化を行ったのち目視でヒストグラムが正規分布(のような形に)なっていることを確認する。あくまで確認なので目視で山なりな出力結果が出ているということが確認できれば良い。

# 読み出しプログラム
ベースとしたプログラムは秋月電子のArduinoサンプルプログラムである。元のプログラムは以下にある。[1]

[1 : ＢＭＸ０５５使用９軸センサーモジュール](https://akizukidenshi.com/catalog/g/gK-13010/)

今回使用したプログラムは以下の通り。

    #include<Wire.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の時)
    
    // センサーの値を保存するグローバル変数
    float xGyro = 0.00;
    float yGyro = 0.00;
    float zGyro = 0.00;
    
    void setup()
    {
      // Wire(Arduino-I2C)の初期化
      Wire.begin();
      // デバッグ用シリアル通信は9600bps
      Serial.begin(9600);
      //BMX055 初期化
      BMX055_Init();
      delay(300);
      Serial.println("---------------------------");
      for(int i = 0; i < 10000; i++){
        //BMX055 ジャイロの読み取り
        BMX055_Gyro();
        Serial.print(xGyro);
        Serial.println(""); 
        //Serial.print(yGyro);
        //Serial.println(""); 
        //Serial.print(zGyro);
        //Serial.println(""); 
        delay(10);
      }
      Serial.println("---------------------------");
    }
    
    void loop()
    {
    
    }
    
    //=====================================================================================//
    void BMX055_Init()
    {
      Wire.beginTransmission(Addr_Gyro);
      Wire.write(0x0F);  // Select Range register
      Wire.write(0x04);  // Full scale = +/- 125 degree/s
      Wire.endTransmission();
      delay(100);
     //------------------------------------------------------------//
      Wire.beginTransmission(Addr_Gyro);
      Wire.write(0x10);  // Select Bandwidth register
      Wire.write(0x07);  // ODR = 100 Hz
      Wire.endTransmission();
      delay(100);
     //------------------------------------------------------------//
      Wire.beginTransmission(Addr_Gyro);
      Wire.write(0x11);  // Select LPM1 register
      Wire.write(0x00);  // Normal mode, Sleep duration = 2ms
      Wire.endTransmission();
      delay(100);
    }
    //=====================================================================================//
    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
    }

実行後にTeraTermを開き、全選択を行った後、テキストに張り付ける。結果は以下の通り。
![スクリーンショット 2022-08-15 151748.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2782192/fa615b8b-8a64-b0e7-473f-34039f775364.png)
確かに上に凸の正規分布っぽい形をしていることが見て取れる。

センサからの入力データは0が基本である(机の上に置いていた)ためノイズは正規分布に従うと考えられる。以上よりカルマンフィルタを適用することが可能であることの確認ができた。

# 終わりに
センサからの入力データを取得し、入力データに載るノイズが正規分布に従いそうだという結果を目視することができた。カルマンフィルタを用いて入力値を補正し、倒立振子を作りたい。

# おまけ
大体のランダムな自然現象は正規分布に従うらしい。この世のランダムなものに対してはカルマンフィルタを用いることができるということだろうか？また、「中心極限定理」というものがある。これは母集団がどのような分布に従おうが、そこから取り出した平均は正規分布に従うという定理らしい。これを使えば、どのようなセンサであろうが入力を正規分布に従うものだとし、カルマンフィルタを適用することが可能なのだろうか？
いずれわかるといいなぁ。