RosでDJI Drone Matrice 100とRealSense D435iのIMU加速度センサ性能の比較

非GPS環境でドローン飛行制御の開発には、空中ホバリングの制御精度を上げるには、
正確な加速度の情報が取れているかは、重要になります。

今回は、DJI Drone Matrice 100とRealSense D435iのIMU加速度センサ性能を比較して見ます。

# 比較環境
Ubuntu 16.04 + ROS Kinetic
DJI Drone Matrice 100
DJI Onboard SDK (OSDK) 3.8
DJI Onboard SDK ROS 3.8
RealSense D435i
realsense-ros 2.2.8

# 比較方法
DJI Drone Matrice 100にRealSense D435iを載せって
静止状態と手でドローンを移動する状態でROSのrosbagを取得して
rqt_bagでZ方向の加速度を比較します。
比較の参考値として、別途でLidarから取れたドローン位置差分の時間微分で
自作した加速度を参考して比較します。（別の参考できるIMU慣性センサーを持っていない）

# 比較結論
DJI IMU加速度の誤差がRealSenseより大きいそうです。

# ROSの起動
## DJI Drone Matrice 100のRos起動launch file
```xml
<launch>
    <node pkg="dji_sdk" type="dji_sdk_node" name="dji_sdk">
    <!-- node parameters -->
    <param name="serial_name" type="string" value="/dev/ttyUSB_dji_drone"/>
    <param name="baud_rate" type="int" value="230400"/>
    <param name="app_id" type="int" value="XXX"/>
    <param name="app_version" type="int" value="1"/>
    <param name="align_time" type="bool" value="false"/>
    <param name="enc_key" type="string" value="XXX"/>
    <param name="use_broadcast" type="bool" value="true"/>
    </node>
</launch>
```

## RealSense D435iのRos起動launch file
```xml
<launch>
<include file="$(find realsense2_camera)/launch/rs_camera.launch">
    <!-- node parameters -->
    <arg name="align_depth"         value="true"/>
    <arg name="linear_accel_cov"    value="1.0"/>
    <arg name="unite_imu_method"    value="linear_interpolation"/>
    <arg name="enable_fisheye"      default="false"/>
    <arg name="enable_depth"        default="false"/>
    <arg name="enable_infra1"       value="false"/>
    <arg name="enable_infra2"       value="false"/>
    <arg name="enable_color"        default="false"/>
    <arg name="enable_gyro"         default="true"/>
    <arg name="enable_accel"        default="true"/>
</include>
</launch>
```

# ROS IMU Topic rateの比較
## DJI /dji_sdk/imu Topic   100HZ
```console
$ rostopic hz /dji_sdk/imu
subscribed to [/dji_sdk/imu]
average rate: 100.752
	min: 0.001s max: 0.015s std dev: 0.00187s window: 98
```

## RealSense /camera/imu Topic   464HZ
```console
$ rostopic hz /camera/imu 
subscribed to [/camera/imu]
average rate: 464.291
	min: 0.000s max: 0.010s std dev: 0.00132s window: 455
```

# ROS IMU Topic データ内容の比較
## DJI /dji_sdk/imu Topic

| 項目       　　　   　|　項目名    | 有無|
|:-----------------   |----------:|:--:|
| orientation         | 姿勢      | 有り|
| angular_velocity    | 角速度     | 有り|
| linear_acceleration | Line加速度 | 有り|
| covariance          | 共分散     | 全部0 |

```console
$ rostopic echo /dji_sdk/imu

header: 
  seq: 237
  stamp: 
    secs: 1569478809
    nsecs: 122336009
  frame_id: "body_FLU"
orientation: 
  x: -0.00366802891774
  y: -0.00091475364914
  z: 0.773214044034
  w: 0.634133858817
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity: 
  x: 0.00093776488211
  y: 0.00052648014389
  z: 0.0036175835412
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration: 
  x: 0.484034068901
  y: -0.087910623651
  z: -0.0438347323649
linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
```

## RealSense /camera/imu Topic

| 項目       　　　   　|　項目名    | 有無|
|:-----------------   |----------:|:--:|
| orientation         | 姿勢      | 無し|
| angular_velocity    | 角速度     | 有り|
| linear_acceleration | Line加速度 | 有り|
| covariance          | 共分散     | velocity:0.01 acceleration:1.0 |

＊covariance共分散は、rs_camera.launch設定ファイルより固定設定

```console
$ rostopic echo /camera/imu

header: 
  seq: 864
  stamp: 
    secs: 1569478808
    nsecs: 504884720
  frame_id: "camera_imu_optical_frame"
orientation: 
  x: 0.0
  y: 0.0
  z: 0.0
  w: 0.0
orientation_covariance: [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity: 
  x: 0.00175957498141
  y: -0.00349944853224
  z: -0.00175487692468
angular_velocity_covariance: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]
linear_acceleration: 
  x: -0.128430217505
  y: 0.153774112463
  z: 9.47220611572
linear_acceleration_covariance: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
```

# IMUのZ方向加速度の比較
## 比較対象データの加工
DJI IMUとRealSense IMUそのまま比較には、見にくいので、データを加工します。

- DJI IMU：　Zの上方向がマイナスなので、「-1」を掛け算して加工します。
- RealSense IMU：　重力加速度「9.8」が入っているので、最初の値を0として、以降の値は、差分を取るように加工します。
- 自作IMU：　LiDARから位置差分の時間微分で計算します。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/b10c74d7-68de-72fc-a4b8-704c12d4811a.png)

## rqt_bagで比較
静止状態と手でドローンを移動する状態の比較結果
![imu_diff.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/8c7e3b4a-3747-b915-dbfb-71577effb1b2.png)

## 静止状態部分の拡大比較
ドローンが静止状態で置いています。

| IMU       | 値 |
|:----------|------------------:|
| DJI       | ３つIMUのノイズ振動最大値の傾向が大体同じ |
| RealSense | ３つIMUのノイズ振動最大値の傾向が大体同じ |
| 自作       | ３つIMUのノイズ振動最大値の傾向が大体同じ |

![static_imu_diff.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/1052a522-ec65-ca10-a5ed-b719c719a743.png)


## 手動移動状態部分の拡大比較
手でドローンを上に移動してから地面に戻る状態の比較結果

| IMU       | 値 |
|:----------|------------------:|
| DJI       | 最大値とRealSense、自作最大値の差分が大きい |
| RealSense | 最大値と自作最大値の傾向が大体同じ |
| 自作       | 最大値とRealSense最大値の傾向が大体同じ |

![movement_imu_diff.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/386259/06df86ac-4281-48c1-1234-8d3ad8861062.png)