State Estimation and Localization for Self-Driving Cars 第二周作业 EKF练习
1.运动和测量模型
1.1汽车运动模型
x k = x k − 1 + T [ cos θ k − 1 0 sin θ k − 1 0 0 1 ] ( [ v k ω k ] + w k ) , w k = N ( 0 , Q ) \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), \quad \mathbf{w}_{k}=\mathcal{N}(\mathbf{0}, \mathbf{Q}) xk=xk−1+T⎣⎡cosθk−1sinθk−10001⎦⎤([vkωk]+wk),wk=N(0,Q)
其中， x k = [ x ， y ， θ ] T \mathbf{x}_{k}=[x ， y， \theta]^{T} xk=[x，y，θ]T是目前汽车的2D位姿。 v k v_{k} vk和 w k w_{k} wk是速度和角速度里程计的度数。
1.2 观测模型
观测模型按照如下公式将LIDAR测量的距离和方位 y k l = [ r ， ϕ ] T \mathbf{y}_{k}^{l}=[r， \phi]^{T} ykl=[r，ϕ]T与汽车的位姿结合起来。
y k l = [ ( x l − x k − d cos θ k ) 2 + ( y l − y k − d sin θ k ) 2 atan 2 ( y l − y k − d sin θ k , x l − x k − d cos θ k ) − θ k ] + n k l , n k l = N ( 0 , R ) \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}} \\ \operatorname{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}, \quad \mathbf{n}_{k}^{l}=\mathcal{N}(\mathbf{0}, \mathbf{R}) ykl=[(xl−xk−dcosθk)2+(yl−yk−dsinθk)2atan2(yl−yk−dsinθk,xl−xk−dcosθk)−θk]+nkl,nkl=N(0,R)
其中， x l x_l xl和 y l y_l yl是陆标 l l l的真实坐标。 x k x_k xk和 y k y_k yk和 θ k \theta_k θk是汽车的当前位姿。 d d d是汽车中心和LIDAR的距离。(已知)
2. 滤波方程
2.1 一步预测
x ˘ k = f ( x ^ k − 1 , u k − 1 , 0 ) P ˇ k = F k − 1 P ^ k − 1 F k − 1 T + L k − 1 Q k − 1 L k − 1 T F k − 1 = ∂ f ∂ x k − 1 ∣ x ^ k − 1 , u k , 0 , L k − 1 = ∂ f ∂ w k ∣ x ^ k − 1 , u k , 0 \begin{aligned} \breve{\mathbf{x}}_{k} &=\mathbf{f}\left(\hat{\mathbf{x}}_{k-1}, \mathbf{u}_{k-1}, \mathbf{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}=&\left.\frac{\partial \mathbf{f}}{\partial \mathbf{x}_{k-1}}\right|_{\hat{\mathbf{x}}_{k-1}, \mathbf{u}_{k}, 0}, \quad \mathbf{L}_{k-1}=\left.\frac{\partial \mathbf{f}}{\partial \mathbf{w}_{k}}\right|_{\hat{\mathbf{x}}_{k-1}, \mathbf{u}_{k}, 0} \end{aligned} x˘kPˇkFk−1==f(x^k−1,uk−1,0)=Fk−1P^k−1Fk−1T+Lk−1Qk−1Lk−1T∂xk−1∂f∣∣∣∣x^k−1,uk,0,Lk−1=∂wk∂f∣∣∣∣x^k−1,uk,0
2.2 状态更新
y k l = h ( x k , n k l ) H k = ∂ h ∂ x k ∣ x ˇ k , 0 , M k = ∂ h ∂ n k ∣ x ˇ k , 0 K k = P ˇ k H k T ( H k P ˇ k H k T + M k R k M k T ) − 1 y ˘ k l = h ( x ˇ k , 0 ) x ^ k = x ˘ k + K k ( y k l − y ˘ k l ) P ^ k = ( I − K k H k ) P ˇ k \begin{aligned} &\mathbf{y}_{k}^{l}=\mathbf{h}\left(\mathbf{x}_{k}, \mathbf{n}_{k}^{l}\right)\\ &\mathbf{H}_{k}=\left.\frac{\partial \mathbf{h}}{\partial \mathbf{x}_{k}}\right|_{\check{\mathbf{x}}_{k}, 0}, \quad \mathbf{M}_{k}=\left.\frac{\partial \mathbf{h}}{\partial \mathbf{n}_{k}}\right|_{\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}, \mathbf{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{aligned} ykl=h(xk,nkl)Hk=∂xk∂h∣∣∣∣xˇk,0,Mk=∂nk∂h∣∣∣∣xˇk,0Kk=PˇkHkT(HkPˇkHkT+MkRkMkT)−1y˘kl=h(xˇk,0)x^k=x˘k+Kk(ykl−y˘kl)P^k=(I−KkHk)Pˇk
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 仿真结果
个人心得，评注
这个EKF练习要求用python进行编程。我在编程的时候犯了两个错误，卡了很久。现在分享出来，希望读者在自己学习这门课的时候多加留意，避免这些坑。
- python做矩阵运算确实不如matlab方便，尤其是带向量的运算，一会儿是一维，一会儿是二维数组，傻傻分不清。要想要使能够按照矩阵来操作，必须把向量变成二维的。例如上面程序中有 x = x . r e s h a p e ( 1 , 3 ) x = x.reshape(1,3) x=x.reshape(1,3)。这是把 x x x变成一个行向量。(当然，也可以变成列向量）。
- 文中把姿态角限定在了 − π 到 π -\pi到\pi −π到π之间。编程中有五个地方需要进行限定。滤波主程序中有2处，分别是在状态一步递推的前后。更新子函数中有3处，一处是最开始对一步预测的结果进行限定，另外两处是对真实测量值和一步预测对应的测量计算值进行限定。（文中已经标黄）
- len(v)可以计算数组v的行数