# IMU使用方法、校正以及視覺化
###### tags: `IMU` `控制組`

#### 以下所有的code已放在github上的Controller/IMU_read/imu的資料夾裡
https://github.com/NCTU-AUV/Controller/tree/team/2020/IMU_read/imu

## 使用方法
#### 在使用之前必須先在arduino上安裝以下兩個程式庫

##### 此為接收IMU資訊以及校正IMU的程式庫

##### 此為Madgwick/Mahony/Kalman filter的程式庫

---

#### 在理解code之前先了解一點旋轉座標表示法(只需要知道表示方法即可)
1. quaternion
https://zh.wikipedia.org/wiki/%E5%9B%9B%E5%85%83%E6%95%B8
2. euler angle
https://zh.wikipedia.org/zh-tw/%E6%AC%A7%E6%8B%89%E8%A7%92
3. rotation matrix
https://zh.wikipedia.org/wiki/%E6%97%8B%E8%BD%AC%E7%9F%A9%E9%98%B5
#### 而底下的code選擇quaternion的方式僅僅是因為視覺化較方便，在控制的使用上還是能改成euler angle較為直觀。
#### 只是euler angle有一個需要的點，當pitch的值超過90°後會變成-89°而不是91°，而且row和yaw會變為-180°

#### 再來是filter的簡單比較
| | Madgwick | Mahony | Kalman |
| --------- |:--------:|:------:|:------:|
| effective | Medium | Low | High |
| effort | Medium | Low | High |
#### 考慮到arduino的計算力以及filter的準確率，我們選擇折衷的Madgwick filter
---

### IMU的取值方法

#include "MPU9250.h" //讀IMU值的library
#include <Adafruit_AHRS_Madgwick.h> //filter的library

MPU9250 IMU(Wire, 0x68);
Adafruit_Madgwick filter;
int state, hz = 100; //state用來確認IMU的狀態，hz為取IMU資料的頻率
float ax, ay, az; //a: Accelerometer
float gx, gy, gz; //g: Gyroscope
float mx, my, mz; //m: Magnetometer
float q0, q1, q2, q3; //q: Quaternion
float StartTime; //控制輸出時間

void setup() {
  Serial.begin(115200);
  state = IMU.begin();
  if(state < 0) {
    while(1) {
      Serial.println(state);
      delay(1000);
      }
  }
  filter.begin(hz); //設定filter的頻率
  
  //以下要更改可以參考reference
  IMU.setAccelRange(MPU9250::ACCEL_RANGE_16G); //設定Accelerometer的值的範圍
  IMU.setGyroRange(MPU9250::GYRO_RANGE_2000DPS); //設定Gyroscope的值的範圍
  IMU.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_20HZ); //設定Digital Low Pass Filter的bandwidth
  
  IMU.setSrd(1000/hz-1); //hz(output rate) = 1000 / (1+SRD)
  
  //IMU校正的部分，此部分因應不同的IMU而改
  IMU.setAccelCalX(0.0848, 1.0008);
  IMU.setAccelCalY(0.0848, 1.0057);
  IMU.setAccelCalZ(0.5504, 0.9842);
  
  IMU.setGyroBiasX_rads(0.0136);
  IMU.setGyroBiasY_rads(-0.0669);
  IMU.setGyroBiasZ_rads(0.0121);
  
  IMU.setMagCalX(33.6164, 1.1795); 
  IMU.setMagCalY(65.2783, 0.5858); 
  IMU.setMagCalZ(-16.7558, 2.2467);
  
  StartTime = millis()+1000/hz; //固定輸出的時間
}

void loop() {
  IMU.readSensor(); //讀IMU測到的值
  
  //將取出的值已m/(s*s)的單位存起來
  ax = IMU.getAccelX_mss();
  ay = IMU.getAccelY_mss(); 
  az = IMU.getAccelZ_mss();
  
  //將取出的值已degree的單位存起來
  gx = IMU.getGyroX_rads()/M_PI*180; 
  gy = IMU.getGyroY_rads()/M_PI*180; 
  gz = IMU.getGyroZ_rads()/M_PI*180;
  
  //將取出的值已uT的單位存起來
  mx = IMU.getMagX_uT(); 
  my = IMU.getMagY_uT(); 
  mz = IMU.getMagZ_uT();
  
  //將值送入filter內
  filter.update(gx, gy, gz, ax, ay, az, mx, my, mz); 
  
  //將濾過的值以Quaternion的表示法存起來
  filter.getQuaternion(&q0, &q1, &q2, &q3); 
  
  //當到達需要輸出的時間後就輸出
  if(millis() > StartTime) {
    Serial.print(q0); Serial.print(", "); Serial.print(q1); Serial.print(", "); Serial.print(q2); Serial.print(", "); Serial.println(q3);
    StartTime += 1000/hz;
  }
}

---

## 校正

1. accelerometer
#### 在校正時，必須讓IMU的用不同的面(立方體)保持不動10秒，共六面

#include "MPU9250.h" //讀IMU值的library

MPU9250 IMU(Wire,0x68);
int state, hz = 100; //state用來確認IMU的狀態，hz為取IMU資料的頻率

//accelerometer校正分為兩部分，bias和scale
float axb, ayb, azb; //bias
float axs, ays, azs; //scale

void setup() { //此部分和IMU的code相同
  Serial.begin(115200);
  state = IMU.begin();
  if(state < 0) {
    while(1) {
      Serial.println(state);
      delay(1000);
      }
  }
  IMU.setAccelRange(MPU9250::ACCEL_RANGE_16G);
  IMU.setGyroRange(MPU9250::GYRO_RANGE_2000DPS);
  IMU.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_20HZ);
  IMU.setSrd(1000/hz-1);
}

void loop() {
  for(int i = 1; i <= 6; i++) { //收集六面資料
    Serial.print(i);
    state = IMU.calibrateAccel(); //收集資料以測量bias和scale
    Serial.println(state); //如果此面收集資料成功則螢幕會顯示1
    delay(10000); //每一面為10秒
  }
  
  //X軸的bias和scale
  axb = IMU.getAccelBiasX_mss(); 
  axs = IMU.getAccelScaleFactorX();
  
  //Y軸的bias和scale
  ayb = IMU.getAccelBiasY_mss(); 
  ays = IMU.getAccelScaleFactorY();
  
  //Z軸的bias和scale
  azb = IMU.getAccelBiasZ_mss(); 
  azs = IMU.getAccelScaleFactorZ();
  
  //將值顯示出來
  Serial.print(axb, 4); Serial.print(", "); Serial.print(axs, 4); Serial.print(", ...");
  Serial.print(ayb, 4); Serial.print(", "); Serial.print(ays, 4); Serial.print(", ...");
  Serial.print(azb, 4); Serial.print(", "); Serial.println(azs, 4);
  
  delay(10000);
}

2. gyroscope
#### 在校正時，只須讓IMU在平穩的地方保持不動即可

#include "MPU9250.h" //讀IMU值的library

MPU9250 IMU(Wire,0x68);
int state, hz = 100; //state用來確認IMU的狀態，hz為取IMU資料的頻率

//gyroscope的校正只有bias
float gxb, gyb, gzb; 

void setup() { //此部分和IMU的code相同
  Serial.begin(115200);
  state = IMU.begin();
  if(state < 0) {
    while(1) {
      Serial.println(state);
      delay(1000);
      }
  }
  IMU.setAccelRange(MPU9250::ACCEL_RANGE_16G);
  IMU.setGyroRange(MPU9250::GYRO_RANGE_2000DPS);
  IMU.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_20HZ);
  IMU.setSrd(1000/hz-1);
}

void loop() { 
  state = IMU.calibrateGyro(); //收集資料以測量bias
  
  gxb = IMU.getGyroBiasX_rads(); //X軸的bias
  gyb = IMU.getGyroBiasY_rads(); //Y軸的bias
  gzb = IMU.getGyroBiasZ_rads(); //Z軸的bias
  
  //將值顯示出來
  Serial.print(gxb, 4); Serial.print(", ");
  Serial.print(gyb, 4); Serial.print(", ");
  Serial.println(gzb, 4);
  
  delay(10000);
}

3. magnetometer
#### 在校正時必須持續且緩慢地將IMU以8的字型擺動(大約2~3分鐘)，直到顯示數字為止

#include "MPU9250.h" //讀IMU值的library

MPU9250 IMU(Wire,0x68);
int state, hz = 100; //state用來確認IMU的狀態，hz為取IMU資料的頻率

//magnetometer校正分為兩部分，bias和scale
float hxb, hyb, hzb; //bias
float hxs, hys, hzs; //scale

void setup() { //此部分和IMU的code相同
  Serial.begin(115200);
  state = IMU.begin();
  if(state < 0) {
    while(1) {
      Serial.println(state);
      delay(1000);
      }
  }
  IMU.setAccelRange(MPU9250::ACCEL_RANGE_16G);
  IMU.setGyroRange(MPU9250::GYRO_RANGE_2000DPS);
  IMU.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_20HZ);
  IMU.setSrd(1000/hz-1);
}

void loop() {
  //開始測量bias和scale
  Serial.println("Start");
  state = IMU.calibrateMag();
  Serial.println(state); // 如果測量結果成功則會顯示1
  
  //X軸的bias和scale
  hxb = IMU.getMagBiasX_uT(); 
  hxs = IMU.getMagScaleFactorX();
  
  //Y軸的bias和scale
  hyb = IMU.getMagBiasY_uT(); 
  hys = IMU.getMagScaleFactorY();
  
  //Z軸的bias和scale
  hzb = IMU.getMagBiasZ_uT(); 
  hzs = IMU.getMagScaleFactorZ();
  
  //將值顯示出來
  Serial.print(hxb, 4); Serial.print(", "); Serial.print(hxs, 4); Serial.print(", ...");
  Serial.print(hyb, 4); Serial.print(", "); Serial.print(hys, 4); Serial.print(", ...");
  Serial.print(hzb, 4); Serial.print(", "); Serial.println(hzs, 4);
  
  delay(10000);
}

## 視覺化
#### 此部分是使用matlab來完成

clc
clear
close force all
%%
A = serialport('COM4', 115200); %對應到arduino的COM和Baud rate
%% 
Quaternion = readline(A); %取值
Quaternion = str2num(Quaternion); %#ok<*ST2NM> %string to num
RM = quat2rotm(Quaternion); %quaternion to rotation matrix

%畫圖
axis equal
xlim([-1 1]);
ylim([-1 1]);
zlim([-1 1]);
hold on

%取值
X = plot3([0 RM(1, 1)], [0 RM(2, 1)], [0 RM(3, 1)], 'r-');
Y = plot3([0 RM(1, 2)], [0 RM(2, 2)], [0 RM(3, 2)], 'g-');
Z = plot3([0 RM(1, 3)], [0 RM(2, 3)], [0 RM(3, 3)], 'b-');

pause(0.1);

%%{
while 1
    if(A.NumBytesAvailable>20)
        read(A, A.NumBytesAvailable-20, 'char'); %將要取值的那行之前的所有資料
        %先read，不然會一直取到第一行
        readline(A); %將要取值的前一行取乾淨
        
        Quaternion = readline(A); %取值
        Quaternion = str2num(Quaternion); %#ok<*ST2NM> %string to num
        RM = quat2rotm(Quaternion); %Quaternion to rotation matrix
        
        %更新X軸座標值
        X.XData = [0 RM(1, 1)];
        X.YData = [0 RM(2, 1)]; 
        X.ZData = [0 RM(3, 1)];
        
        %更新Y軸座標值
        Y.XData = [0 RM(1, 2)]; 
        Y.YData = [0 RM(2, 2)]; 
        Y.ZData = [0 RM(3, 2)];
        
        %更新Z軸座標值
        Z.XData = [0 RM(1, 3)]; 
        Z.YData = [0 RM(2, 3)]; 
        Z.ZData = [0 RM(3, 3)];
        
        pause(0.01);
    end
end
%
}