はじめに
今回は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)
パラメータ
| 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}$
ここで、
\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への測地系変換ライブラリが転がっているので、それらを使えばなんとかなります。