動手學無人駕駛（6）：基于IMU和GPS資料融合的自車定位

在上一篇博文《動手學無人駕駛（5）：多傳感器資料融合》介紹了如何使用Radar和LiDAR資料對自行車進行追蹤，這是對汽車外界運動物體進行定位，
對于自動駕駛的汽車來說，有時也需要對自身進行更精確的定位，今天就介紹如何使用IMU和GPS進行自車定位（因為在上一篇文章中對kalman和Extend Kalman原理進行了比較詳細的介紹，本文中理論部分不會再介紹的這么詳細了，有需要的話可以回看上文），
本文參考了Coursera自動駕駛課程專案，在此表示感謝，

大家可以先看看下面這個視頻，對本專案要介紹的內容有個初步了解，視頻鏈接為：https://www.bilibili.com/video/BV1cE411D7Y9?p=18

1.IMU簡介

慣性測量單元（Inertial Measurement Unit）通常指由3個加速度計和3個陀螺儀組合而成，加速度計和陀螺儀安裝在互相垂直的測量軸上，這里可以將其輸出看作為三個方向的加速度和角速度，表示為：
i m u = [ a x a y a z w x w y w z ] imu=\begin{bmatrix}a_x \\ a_y\\ a_z \\ w_x \\ w_y \\ w_z\end{bmatrix} imu=?????????ax?ay?az?wx?wy?wz???????????

2.GPS簡介

全球定位系統（Global Positioning System）大家應該都不陌生，其輸出常見為：經度，維度，和高度，表示為：
g p s = [ l n g l a t a l t ] gps=\begin{bmatrix} lng\\lat\\alt \end{bmatrix} gps=???lnglatalt????

3.資料融合

3.1 Extend Kalman Filter

整個流程框架如下圖所示，這里需要注意的是IMU與GPS的輸出信號頻率是不同的，IMU輸出頻率常見為50-500Hz不等，GPS輸出頻率常見為1-10Hz，因此要分為兩部分來討論：

（1）在只有IMU資料時（此時GPS還未有輸出產生），IMU資料經過運動模型，得到預測狀態 x ˇ k \check {x}_k xˇk?；然后預測狀態傳送回運動模型，繼續下一步預測；
（2）當有GPS資料產生時，上一時刻產生的預測狀態將會和接收到的GPS位置資訊進行資料融合，得到修正后的狀態 x ^ k \hat {x}_k 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 x=[p_x,p_y,p_z,v_x,v_y,v_z,q_0,q_1,q_2,q_3]^T x=[px?,py?,pz?,vx?,vy?,vz?,q0?,q1?,q2?,q3?]T，模型可以分為三部分討論：

（1）位置運動模型，假設載體做勻加速運動，則有： p k = Δ t v k ? 1 + Δ t 2 2 ( C n s f k ? 1 ? g ) p_k=\Delta{t}v_{k-1}+\frac{\Delta{t}^2}{2}(C_{ns}f_{k-1}-g) pk?=Δtvk?1?+2Δt2?(Cns?fk?1??g)，其中 f k ? 1 f_{k-1} fk?1?為imu的測量值， C n s C_{ns} Cns?為旋轉矩陣，用于對imu的測量值進行坐標變換，
（2）速度運動模型，同樣假設載體做勻加速運動，
（3）方向運動模型，這里 q k ? 1 q_{k-1} qk?1?表示為四元數，關于四元數的旋轉變換可以參考有關資料，這里不做展開了，

但上面的模型不是線性的，課程中將上面的模型進行線性化處理，結果如下，處理后的誤差狀態向量為9維的狀態向量，這里需要關注的是狀態轉移矩陣 F k ? 1 F_{k-1} Fk?1?，噪聲協方差矩陣 L k ? 1 L_{k-1} Lk?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 σ 3\sigma 3σ標準，

至此，本文要介紹的內容就結束了，基于IMU和GPS的位置定位，關鍵點在于IMU的運動模型，特別是四元數更新部分，里面牽涉到的變化比較多，需要留心，