android gps 锁屏更新坐标_IMU & GPS定位 Quaternion kinematics for ESKF[part 3]

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

1. 原理

1.1 坐标系

全局坐标系采用东北天(ENU)坐标系,即X指向东,Y轴指向北,Z轴指向天空。坐标原点为初始时刻位置。GPS到IMU有一个外参, 即GPS在IMU坐标系下的位置:

,即杆臂值。

1.2 error-state

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

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

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

1.3 ESKF GPS更新

GPS可以提供位置量测信息。一般GPS提供的数据为WGS84坐标系,而我们的state是定义在ENU坐标系下的。为此,我们参考了VINS-Fusion的做法,采用了 GeographicLib 库,先把GPS数据转换至ENU笛卡尔坐标系:

。这样我们就可以定义GPS位置观测方程为:

相对于error state的Jacobian为：

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

1.4 初始化

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

2. 实现

请见github:

ydsf16/imu_gps_localization​github.com

3. 数据

https://epan-utbm.github.io/utbm_robocar_dataset/​epan-utbm.github.io

建议使用：

4. 参考资料

1. Quaternion kinematics for the error-state Kalman filter
2. Woosik Lee, Intermittent GPS-aided VIO: Online Initialization and Calibration.
3. A General Optimization-based Framework for Global Pose Estimation with Multiple Sensors
4. A General Optimization-based Framework for Local Odometry Estimation with Multiple Sensors