ROS2でEKFによるGNSS/IMU/Odometryを複合した三次元自己位置推定

# はじめに
今回はROS2でGNSS/IMU/Odom(Visual Odometry/Lidar Odometry)をExtended Kalman Filterで複合した自己位置推定をしました！

書いたコードはGithubにあります。
<a href="https://github.com/rsasaki0109/kalman_filter_localization"><img src="https://github-link-card.s3.ap-northeast-1.amazonaws.com/rsasaki0109/kalman_filter_localization.png" width="460px"></a> 
# パッケージ
パッケージ名：kalman_filter_localization
ノード名：ekf_localization

- 入力
/initial_pose (geometry_msgs/PoseStamed) 
/gnss_pose  (geometry_msgs/PoseStamed) 
/imu  (sensor_msgs/Imu)
/odom (nav_msgs/Odometry)
/tf(/base_link(robot frame) → /imu_link(imu frame))

- 出力
/curent_pose (geometry_msgs/PoseStamped)

## パラメータ

|Name|Type|Default value|Description|
|---|---|---|---|
|pub_period|int|10|publishする周期[ms]|
|var_gnss|double|0.1|GNSSの分散[m^2]|
|var_odom|double|0.1|オドメトリの分散[m^2]|
|var_imu_w|double|0.01|imuの角速度の分散[(deg/sec)^2]|
|var_imu_acc|double|0.01|imuの加速度の分散[(m/sec^2)^2]|
|use_gnss|bool|true|gnssを使うか|
|use_odom|bool|false|odom(lo/vo)を使うか|

# アルゴリズム
## 記号
$k$：離散時間k
$dt$：時間刻み
$\boldsymbol{x}$：状態量
$\boldsymbol{p}$：位置
$\boldsymbol{v}$：速度
$\boldsymbol{a}$：加速度
$\boldsymbol{q}$：クォータニオン
$\omega$：角速度
$u_d$：動作値
$f$：運動モデル
$l$：動作値を状態空間に写す関数
$r_w$：運動ノイズ
$y$：観測量
$h$：観測モデル
$q_w$：観測ノイズ
$\boldsymbol{K}$：カルマンゲイン
$\boldsymbol{I_{n\times n}}$：$n\times n$の単位行列
$\sigma^2$：分散
$\boldsymbol{\nu}$：歪対称行列
$\boldsymbol{R}$：回転行列
$\boldsymbol{g}$：重力
## モデル
 - 状態量(位置姿勢)  
$ \boldsymbol{x_k} =[p_{xk} \quad p_{yk} \quad p_{zk} \quad v_{xk} \quad v_{yk} \quad v_{zk} \quad q_{xk} \quad q_{yk} \quad q_{zk} \quad q_{wk}]^T$  
$p$：位置、$v$：速度、$q$：クォータニオン
 - 誤差状態量 
$ \boldsymbol{\delta x_k} =[\delta p_{xk} \quad \delta p_{yk} \quad \delta p_{zk} \quad \delta v_{xk} \quad \delta v_{yk} \quad \delta v_{zk} \quad \delta \theta_{xk} \quad \delta \theta_{yk} \quad \delta \theta_{zk} ]^T$  

 - 動作値  
$\boldsymbol{u_{dk}}= [a_{xk} \quad a_{yk} \quad a_{zk} \quad \omega_{xk} \quad \omega_{yk} \quad \omega_{zk}]^T$   
$a$：imuで計測した加速度、$\omega$：imuで計測した角速度
 - 観測値  
$\boldsymbol{y_k}=[p_{xk}^{obs} \quad p_{yk}^{obs} \quad p_{zk}^{obs}]^T$  
$p$：GPSもしくはOdometryで計測した位置  
 - 運動モデル  
$\boldsymbol{x_{k}} = f(x_{k-1}) + l(u_{dk}) + q_w$  
$f$：運動モデル、$l$:動作値を状態空間に写す関数,$q_w$：運動ノイズ
 - 観測モデル  
$\boldsymbol{y_{k}} = h(x_{k}) + r_w$  
$h$：観測モデル、$r_w$：観測ノイズ

## アルゴリズム
### 予測更新
 - 状態量  
$ \boldsymbol{p_k} = \boldsymbol{p_{k-1}} + \boldsymbol{v_k} dt +\frac{1}{2}  \cdot (\boldsymbol{R(q_{k-1})}\boldsymbol{a_{k-1}^{imu} - g})(dt)^2 $  
$ \boldsymbol{v_k} = \boldsymbol{v_{k-1}} +  (\boldsymbol{R(q_{k-1})a_{k-1}^{imu} -g})d t$  
$ \boldsymbol{q_k} = \mathbf{R(\omega_{k-1
}^{imu}dt )} \cdot \mathbf{q_{k-1}}$

ここで、
$\boldsymbol{g}=[0 \quad 0 \quad -g]^T$

 - 共分散  
$\boldsymbol{P_k = F_k P_{k-1}F_k^T + L Q_k L^T}$  
ここで、

```math
\boldsymbol{F_k} = 
\begin{pmatrix}
I_{3 \times 3} & dt \cdot I_{3 \times 3} & 0  \\
0              & I_{3 \times 3}          & - R(q_{k-1}) \cdot \nu(\omega_{k-1}^{imu}) \cdot dt\\
0              &                       0 & I_{3 \times 3} \\
\end{pmatrix}  
```


```math
\boldsymbol{Q_k} = 
\begin{pmatrix}
\sigma^2_{a}  \cdot I_{3 \times 3} & 0 \\
0              & \sigma^2_{\omega} \cdot I_{3 \times 3}          \\
\end{pmatrix}  
(dt)^2
```
```math
\boldsymbol{L} = 
\begin{pmatrix}
0 & 0\\
I_{3 \times 3} & 0 \\
0              & I_{3 \times 3}

\end{pmatrix}  
```
### 観測更新

 - カルマンゲイン  
$\boldsymbol{K} = \boldsymbol{P_k H^T (H P_k H^T + R)}^{-1}$  

ここで、  

```math
\boldsymbol{R} = \sigma^2_{obs} \cdot \boldsymbol{I_{3 \times 3}}  
```

```math
\boldsymbol{H} = 
\begin{pmatrix}
\boldsymbol{I_{3 \times 3}} & \boldsymbol{0} & \boldsymbol{0}
\end{pmatrix}  
```

 - 誤差状態量  
$\boldsymbol{\delta x} = \boldsymbol{K} \cdot (\boldsymbol{y_k}-\boldsymbol{p_{k}})$  




 - 状態量  
$ \boldsymbol{p_k} = \boldsymbol{p_{k-1}} + \boldsymbol{\delta p_{k}} $  
$ \boldsymbol{v_k} = \boldsymbol{v_{k-1}} + \boldsymbol{\delta v_{k}}$  
$ \boldsymbol{q_k} = \boldsymbol{R}(\boldsymbol{\delta \theta_{k}}) \cdot \boldsymbol{q_{k-1}}$

 - 共分散  
$\boldsymbol{P_k = (I_{9 \times 9} - K  H) \cdot P_k}$ 

# デモ

データは[このGNSS/IMUの入ったROS1のdata](https://drive.google.com/file/d/1CYuip5dApvcF-xrB2f5s8pdBu7MGCDxP/view)を使います。


ノードを起動します

```
ros2 launch kalman_filter_localization ekf.launch.py
```

適当に初期位置を指定します

```
ros2 topic pub ekf_localization/initial_pose geometry_msgs/PoseStamped '{header: {stamp: {sec: 1532228824, nanosec: 55000000}, frame_id: "map"}, pose: {position: {x: 0, y: 0, z: 10}, orientation: {z: 1, w: 0}}}' --once
```

rosbagを再生します

```
ros2 bag play -s rosbag_v2 test.bag
```


結果は以下のようになります。
青が初期位置姿勢、赤がGNSSの位置姿勢、緑が拡張カルマンフィルタでGNSSとIMUを複合した位置姿勢です。
![Peek 2020-01-04 22-48.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/229952/c252c7d5-341b-3c81-be46-725992792932.gif)


現状は、imuが6軸必須だったり、tf周りが適当なのですが、そのうち改良していきたいですね！(他にもIMUのバイアス推定・UKFの実装・時間合わせなどなど・・・）。とりあえず、今回は以上です。

最後に補足すると、gnssがnavsatfix型じゃないんですが、autowareの[これ](https://gitlab.com/autowarefoundation/autoware.ai/common/tree/master/gnss)や[これ](https://gitlab.com/autowarefoundation/autoware.ai/core_perception/tree/master/gnss_localizer)のようなlatlonheightからenuへの測地系変換ライブラリが転がっているので、それらを使えばなんとかなります。