动手学无人驾驶（6）：基于IMU和GPS数据融合的自车定位

在上一篇博文《动手学无人驾驶（5）：多传感器数据融合》介绍了如何使用Radar和LiDAR数据对自行车进行追踪，这是对汽车外界运动物体进行定位。
对于自动驾驶的汽车来说，有时也需要对自身进行更精确的定位，今天就介绍如何使用IMU和GPS进行自车定位（因为在上一篇文章中对kalman和Extend Kalman原理进行了比较详细的介绍，本文中理论部分不会再介绍的这么详细了，有需要的话可以回看上文）。
本文参考了Coursera自动驾驶课程项目，在此表示感谢。

大家可以先看看下面这个视频，对本项目要介绍的内容有个初步了解，视频链接为：https://www.bilibili.com/video/BV1cE411D7Y9?p=18

Coursera 自动驾驶教程：Part2 - State Estimation and Localization for Self-Driving Cars

1.IMU简介

惯性测量单元（Inertial Measurement Unit）通常由3个加速度计和3个陀螺仪组合而成，加速度计和陀螺仪安装在互相垂直的测量轴上，这里可以将其输出看作为三个方向的加速度和角速度，表示为：
$$imu=\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\\ w_x\\ w_y\\ w_z\end{array}\right]$$

---

2.GPS简介

全球定位系统（Global Positioning System）大家应该都不陌生，其输出常见为：经度，维度，和高度，表示为：
$$gps=\left[\begin{array}{c}lng\\ lat\\ alt\end{array}\right]$$

---

3.数据融合

3.1 Extend Kalman Filter

整个流程框架如下图所示，这里需要注意的是IMU与GPS的输出信号频率是不同的，IMU输出频率常见为50-500Hz不等，GPS输出频率常见为1-10Hz。因此要分为两部分来讨论：

（1）在只有IMU数据时（此时GPS还未有输出产生），IMU数据经过运动模型，得到预测状态${\check{x}}_k$；然后预测状态传送回运动模型，继续下一步预测；
（2）当有GPS数据产生时，上一时刻产生的预测状态将会和接收到的GPS位置信息进行数据融合，得到修正后的状态${\hat{x}}_k$。然后再传回运动模型，进行下一周期的运算。

---

3.2 Motion Model

运动模型如下，状态向量为10维状态向量，即$x=[p_x,p_y,p_z,v_x,v_y,v_z,q_0,q_1,q_2,q_3{]}^T$，模型可以分为三部分讨论：

（1）位置运动模型，假设载体做匀加速运动，则有：$p_k=\Delta t{v}_{k-1}+\frac{\Delta t^2}{2}(C_{ns}{f}_{k-1}-g)$，其中$f_{k-1}$为imu的测量值，$C_{ns}$为旋转矩阵，用于对imu的测量值进行坐标变换。
（2）速度运动模型，同样假设载体做匀加速运动。
（3）方向运动模型，这里$q_{k-1}$表示为四元数，关于四元数的旋转变换可以参考有关资料，这里不做展开了。

但上面的模型不是线性的，课程中将上面的模型进行线性化处理，结果如下，处理后的误差状态向量为9维的状态向量，这里需要关注的是状态转移矩阵$F_{k-1}$，噪声协方差矩阵$L_{k-1}$。

---

3.3 Measurement Model

测量模型如下，这里我们需要用到的是GPS的位置数据。

3.4 Sensor Fusion

介绍完理论部分，下面我们开始一步步实现代码部分。
（1）使用IMU数据进行更新，需要注意旋转矩阵的计算。

    # 1. Update state with IMU inputs
    C_ns = Quaternion(*q_est[k-1]).to_mat() #rotational matrix
    C_ns_dot_f_km = np.dot(C_ns, imu_f.data[k-1])
    
    # 1.1 Linearize the motion model and compute Jacobians
    p_est[k] = p_est[k-1] + delta_t * v_est[k-1] + (delta_t**2)/2.0 * (C_ns.dot(imu_f.data[k-1]) + g)
    v_est[k] = v_est[k-1] + delta_t*(C_ns.dot(imu_f.data[k-1]) + g)
    
    # Instead of using Omega, we use quaternion multiplication 
    q_est[k] = Quaternion(axis_angle = imu_w.data[k-1] * delta_t).quat_mult_right(q_est[k-1])

---

（2）状态协方差矩阵的更新

	# 2. Propagate uncertainty
    # Global orientation error, over local orientation error
    # See Sola technical report
    F = np.identity(9)
    F[:3, 3:6] = delta_t * np.identity(3)
    #F[3:6, 6:] = -(C_ns.dot(skew_symmetric(imu_f.data[k-1].reshape((3,1)))))
    F[3:6,6:9] = -skew_symmetric(C_ns_dot_f_km) *delta_t
  
    Q = np.identity(6)
    Q[:, :3] *= delta_t**2 * var_imu_f
    Q[:, -3:] *= delta_t**2 * var_imu_w
    
    p_cov[k] = F.dot(p_cov[k-1]).dot(F.T) + l_jac.dot(Q).dot(l_jac.T)  #uncertainty 

---

（3）计算kalman增益

    # 3.1 Compute Kalman Gain
    K_k = p_cov_check.dot(h_jac.T).dot(np.linalg.inv(h_jac.dot(p_cov_check).dot(h_jac.T)+np.identity(3)*sensor_var))

---

（4）计算误差状态

    # 3.2 Compute error state
    errorState = K_k.dot(y_k - p_check)

---

（5）误差状态修正

    # 3.3 Correct predicted state
    p_hat = p_check + errorState[:3]
    v_hat = v_check + errorState[3:6]
    
    q_hat = Quaternion(euler=errorState[6:]).quat_mult_left(\
                      q_check) # left or right

---

（6）修正状态协方差矩阵

    # 3.4 Compute corrected covariance
    p_cov_hat = (np.identity(9) - K_k.dot(h_jac)).dot(p_cov_check)

到这一步，就完成了整个处理过程，可以看看最终的结果，途中橙色为轨迹真值位置，蓝色为估计的轨迹位置。

也可以绘制误差分布图，如下图所示，这里使用的$3\sigma$标准。

---

至此，本文要介绍的内容就结束了。基于IMU和GPS的位置定位，关键点在于IMU的运动模型，特别是四元数更新部分，里面牵涉到的变化比较多，需要留心。

如果想深入了解IMU和GPS融合原理，可以看看这篇文章： 重读经典《Quaternion kinematics for the error-state Kalman filter》，这也是Coursera课程关于这一项目的参考文献。