はじめに

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

書いたコードはGithubにあります。

パッケージ

パッケージ名：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)

パラメータ

アルゴリズム

記号

$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}$
  ここで、

\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}  

\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

\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}$
  

ここで、

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

\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を使います。

ノードを起動します

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を複合した位置姿勢です。

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

最後に補足すると、gnssがnavsatfix型じゃないんですが、autowareのこれやこれのようなlatlonheightからenuへの測地系変換ライブラリが転がっているので、それらを使えばなんとかなります。