基于IMU和GPS数据融合的自车定位 （卡尔曼）

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

 

(124条消息) 动手学无人驾驶（6）：基于IMU和GPS数据融合的自车定位_自动驾驶小学生的博客-CSDN博客_基于imu和gps数据融合的自车定位

 

 

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

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

文章目录

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

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 σ 3\sigma3σ标准。

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