IMU & GPS定位 Quaternion kinematics for ESKF[part 3]

最近基本把《QuaternionkinematicsforESKF》看完了,采用书中的方法实现了一个IMU+GPS的组合定位。

1. 原理

1.1 坐标系

全局坐标系采用东北天(ENU)坐标系,即X指向东,Y轴指向北,Z轴指向天空。坐标原点为初始时刻位置。GPS到IMU有一个外参, 即GPS在IMU坐标系下的位置: ${}^I \mathbf p_{Gps}$ ,即杆臂值。

1.2 error-state

我们维护位置、速度、方向、加速度和陀螺的bias,5种,共计15维的状态。

$$\left[\begin{matrix} \delta \mathbf p^T & \delta \mathbf v^T &\delta \mathbf \theta^T & \delta \mathbf a_b ^T&\delta \mathbf \omega_b^T\end{matrix}\right]^T \\$$

重力方向没有放入到状态之中,因为我们用了东北天坐标系,重力直接设为Z轴的反方向。

如书中所述,采用IMU进行EKF的预测。需要propagatenominalstate和error-state的协方差,如书中p58、p59。

$$\begin{align} \mathbf p &\leftarrow \mathbf p + \mathbf v\Delta t + \frac1 2 (\mathbf R (\mathbf a_m - \mathbf a_b) + \mathbf g) \Delta t^2 \\ \mathbf v &\leftarrow \mathbf v + (\mathbf R (\mathbf a_m - \mathbf a_b) + \mathbf g) \Delta t \\ \mathbf q &\leftarrow \mathbf q \otimes\mathbf q{(\mathbf \omega_m - \mathbf \omega_b)\Delta t} \\ \mathbf a_b &\leftarrow \mathbf a_b \\ \mathbf \omega_b &\leftarrow \mathbf \omega_b \\ \mathbf P & \leftarrow \mathbf F_x \mathbf P \mathbf F_x^T + \mathbf F_i \mathbf Q_i \mathbf F_i^T \end{align} \\$$

1.3 ESKF GPS更新

GPS可以提供位置量测信息。一般GPS提供的数据为WGS84坐标系,而我们的state是定义在ENU坐标系下的。为此,我们参考了VINS-Fusion的做法,采用了 GeographicLib 库,先把GPS数据转换至ENU笛卡尔坐标系: ${}^G \mathbf p_{Gps}$ 。这样我们就可以定义GPS位置观测方程为:

$${}^G \mathbf p_{Gps} = {}^G \mathbf p_{I} + {}^G _I \mathbf R \cdot {}^I \mathbf p_{Gps} \\$$

相对于error state的Jacobian为：

$$\mathbf H = \left[\begin{matrix} \mathbf I & \mathbf 0 & -{}^G_I \mathbf R[{}^I \mathbf p _{Gps}]_\times & \mathbf 0 & \mathbf 0\end{matrix}\right] \\$$

剩下的就是套用ESKF的更新公式了,如书中P61⻚。

1.4 初始化

初始位置为0,方向的roll和pitch可由加速度的重力方向确定,bias设置为0.

2. 实现

请见github:

ydsf16/imu_gps_localization

3. 数据

https://epan-utbm.github.io/utbm_robocar_dataset/ 建议使用：

4. 参考资料

5. Quaternion kinematics for the error-state Kalman filter

6. Woosik Lee, Intermittent GPS-aided VIO: Online Initialization and Calibration.

7. A General Optimization-based Framework for Global Pose Estimation with Multiple Sensors

8. A General Optimization-based Framework for Local Odometry Estimation with Multiple Sensors