State Estimation and Localization for Self-Driving Cars 第二周作业 EKF练习

1.运动和测量模型

1.1汽车运动模型

$${\mathbf{x}}_k={\mathbf{x}}_{k-1}+T\left[\begin{array}{cc}\cos {\theta }_{k-1}& 0\\ \sin {\theta }_{k-1}& 0\\ 0& 1\end{array}\right]\left(\left[\begin{array}{c}{v}_k\\ {\omega }_k\end{array}\right]+{\mathbf{w}}_k\right),{\mathbf{w}}_k=\mathcal{N}(0,\mathbf{Q})$$

其中，${\mathbf{x}}_k=[x，y，\theta {]}^T$是目前汽车的2D位姿。$v_k$和$w_k$是速度和角速度里程计的度数。

1.2 观测模型

  观测模型按照如下公式将LIDAR测量的距离和方位${\mathbf{y}}_k^l=[r，\varphi {]}^T$与汽车的位姿结合起来。
$${\mathbf{y}}_k^l=\left[\begin{array}{c}\sqrt{{\left(x_l-x_k-d\cos {\theta }_k\right)}^2+{\left(y_l-y_k-d\sin {\theta }_k\right)}^2}\\ \mathrm{atan}2\left(y_l-y_k-d\sin {\theta }_k,x_l-x_k-d\cos {\theta }_k\right)-{\theta }_k\end{array}\right]+{\mathbf{n}}_k^l,{\mathbf{n}}_k^l=\mathcal{N}(0,\mathbf{R})$$

其中，$x_l$和$y_l$是陆标$l$的真实坐标。$x_k$和$y_k$和${\theta }_k$是汽车的当前位姿。$d$是汽车中心和LIDAR的距离。(已知)

2. 滤波方程

2.1 一步预测

$$\begin{array}{rl}{\breve{\mathbf{x}}}_k& =\mathbf{f}\left({\hat{\mathbf{x}}}_{k-1},{\mathbf{u}}_{k-1},0\right)\\ {\check{\mathbf{P}}}_k& ={\mathbf{F}}_{k-1}{\hat{\mathbf{P}}}_{k-1}{\mathbf{F}}_{k-1}^T+{\mathbf{L}}_{k-1}{\mathbf{Q}}_{k-1}{\mathbf{L}}_{k-1}^T\\ {\mathbf{F}}_{k-1}=& {\frac{\partial \mathbf{f}}{\partial {\mathbf{x}}_{k-1}}|}_{{\hat{\mathbf{x}}}_{k-1},{\mathbf{u}}_k,0},{\mathbf{L}}_{k-1}={\frac{\partial \mathbf{f}}{\partial {\mathbf{w}}_k}|}_{{\hat{\mathbf{x}}}_{k-1},{\mathbf{u}}_k,0}\end{array}$$

2.2 状态更新

$$\begin{array}{rl}& {\mathbf{y}}_k^l=\mathbf{h}\left({\mathbf{x}}_k,{\mathbf{n}}_k^l\right)\\ & {\mathbf{H}}_k={\frac{\partial \mathbf{h}}{\partial {\mathbf{x}}_k}|}_{{\check{\mathbf{x}}}_k,0},{\mathbf{M}}_k={\frac{\partial \mathbf{h}}{\partial {\mathbf{n}}_k}|}_{{\check{\mathbf{x}}}_k,0}\\ & {\mathbf{K}}_k={\check{\mathbf{P}}}_k{\mathbf{H}}_k^T{\left({\mathbf{H}}_k{\check{\mathbf{P}}}_k{\mathbf{H}}_k^T+{\mathbf{M}}_k{\mathbf{R}}_k{\mathbf{M}}_k^T\right)}^{-1}\\ & {\breve{\mathbf{y}}}_k^l=\mathbf{h}\left({\check{\mathbf{x}}}_k,0\right)\\ & {\hat{\mathbf{x}}}_k={\breve{\mathbf{x}}}_k+{\mathbf{K}}_k\left({\mathbf{y}}_k^l-{\breve{\mathbf{y}}}_k^l\right)\\ & {\hat{\mathbf{P}}}_k=\left(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k\right){\check{\mathbf{P}}}_k\end{array}$$

3.程序代码

  附上作业所要求提交的子程序，仅供参考

3.1 测量更新函数

def measurement_update(lk, rk, bk, P_check, x_check):
    bk = wraptopi(bk)
    x_check[:,2]= wraptopi(x_check[:,2])    
    A = lk[0] - x_check[:,0] - d * math.cos(x_check[:,2])
    B = lk[1] - x_check[:,1] - d * math.sin(x_check[:,2])
    # 1. Compute measurement Jacobian
    M = np.identity(2)
    H = np.array([[-A * (A ** 2 + B ** 2) ** (-0.5),-B * (A ** 2 + B ** 2) ** (-0.5),d * (A ** 2 + B ** 2) ** (-0.5) * (A * math.sin(x_check[:,2])- B *  math.cos(x_check[:,2]))],
                 [(1 + (B/A) ** 2) ** (-1) * B / A ** 2,-(1 + (B/A) ** 2) ** (-1) / A, (1 + (B/A) ** 2) ** (-1)/ A ** 2 * (-d*math.cos(x_check[:,2]) * A - B * d * math.sin(x_check[:,2]))-1]])
    H = H.reshape(2,3)
    # 2. Compute Kalman Gain
    K = P_check @ H.T @ np.linalg.inv(H @ P_check @ H.T + M @cov_y @ M.T )
    # 3. Correct predicted state (remember to wrap the angles to [-pi,pi])
    h = math.atan2(B,A) - x_check[:,2]
    h = wraptopi(h)
    ss = K @ np.array([rk-(A ** 2 + B ** 2) ** 0.5,bk-h])
    x_check = x_check + ss.reshape(1,3)
    x_check[:,2]= wraptopi(x_check[:,2])

    # 4. Correct covariance
    P_check = (np.identity(3)-K @ H) @ P_check 

    return x_check, P_check

3.2 滤波主程序

#### 5. Main Filter Loop #######################################################################
for k in range(1, len(t)):  # start at 1 because we've set the initial prediciton
    delta_t = t[k] - t[k - 1]  # time step (difference between timestamps)
    x_check[:,2]= wraptopi(x_check[:,2])
    theta = x_check[:,2]
    # 1. Update state with odometry readings (remember to wrap the angles to [-pi,pi])
    cc = delta_t * np.array([[math.cos(theta),0],[math.sin(theta),0],[0,1]]) @ np.array([[v[k]],[om[k]]])
    x_check = x_check + cc.T

    
    x_check[:,2] = wraptopi(x_check[:,2])

    # 2. Motion model jacobian with respect to last state
    F_km = np.array([[1,0,-delta_t * v[k] *math.sin(theta)],[0,1,delta_t * v[k]* math.cos(theta)],[0,0,1]])

    # 3. Motion model jacobian with respect to noise
    L_km = delta_t * np.array([[math.cos(theta),0],[math.sin(theta),0],[0,1]])

    # 4. Propagate uncertainty
    P_check =  F_km @ P_check @  F_km.T + L_km @ Q_km @L_km.T

    # 5. Update state estimate using available landmark measurements
    for i in range(len(r[k])):
        x_check, P_check = measurement_update(l[i], r[k, i], b[k, i], P_check, x_check)
    # Set final state predictions for timestep
    x_est[k, 0] = x_check[:,0]
    x_est[k, 1] = x_check[:,1]
    x_est[k, 2] = x_check[:,2]
    P_est[k, :, :] = P_check

3.3 仿真结果

图1 地面真值

图2 估计值

个人心得，评注

  这个EKF练习要求用python进行编程。我在编程的时候犯了两个错误，卡了很久。现在分享出来，希望读者在自己学习这门课的时候多加留意，避免这些坑。

1. python做矩阵运算确实不如matlab方便，尤其是带向量的运算，一会儿是一维，一会儿是二维数组，傻傻分不清。要想要使能够按照矩阵来操作，必须把向量变成二维的。例如上面程序中有$x=x.reshape(1,3)$。这是把$x$变成一个行向量。(当然，也可以变成列向量）。
2. 文中把姿态角限定在了$-\pi 到\pi$之间。编程中有五个地方需要进行限定。滤波主程序中有2处，分别是在状态一步递推的前后。更新子函数中有3处，一处是最开始对一步预测的结果进行限定，另外两处是对真实测量值和一步预测对应的测量计算值进行限定。（文中已经标黄）
3. len(v)可以计算数组v的行数