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), \quad \mathbf{w}_{k}=\mathcal{N}(\mathbf{0}, \mathbf{Q})$xk​=xk−1​+T⎣⎡​cosθk−1​sinθk−1​0​001​⎦⎤​([vk​ωk​​]+wk​),wk​=N(0,Q)

其中，$\mathbf{x}_{k}=[x ， y， \theta]^{T}$xk​=[x，y，θ]T是目前汽车的2D位姿。$v_{k}$vk​和$w_{k}$wk​是速度和角速度里程计的度数。

1.2 观测模型

  观测模型按照如下公式将LIDAR测量的距离和方位$\mathbf{y}_{k}^{l}=[r， \phi]^{T}$ykl​=[r，ϕ]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}} \\ \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​)2​atan2(yl​−yk​−dsinθk​,xl​−xk​−dcosθk​)−θk​​]+nkl​,nkl​=N(0,R)

其中，$x_l$xl​和$y_l$yl​是陆标$l$l的真实坐标。$x_k$xk​和$y_k$yk​和$\theta_k$θk​是汽车的当前位姿。$d$d是汽车中心和LIDAR的距离。(已知)

2. 滤波方程

2.1 一步预测

$\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˘k​Pˇk​Fk−1​=​=f(x^k−1​,uk−1​,0)=Fk−1​P^k−1​Fk−1T​+Lk−1​Qk−1​Lk−1T​∂xk−1​∂f​∣∣∣∣​x^k−1​,uk​,0​,Lk−1​=∂wk​∂f​∣∣∣∣​x^k−1​,uk​,0​​

2.2 状态更新

$\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​,0​Kk​=Pˇk​HkT​(Hk​Pˇk​HkT​+Mk​Rk​MkT​)−1y˘​kl​=h(xˇk​,0)x^k​=x˘k​+Kk​(ykl​−y˘​kl​)P^k​=(I−Kk​Hk​)Pˇk​​

3.程序代码

3.1 获取数据

import pickle
import numpy as np
import math
import matplotlib.pyplot as plt

with open(‘data/data.pickle’, ‘rb’) as f:
data = pickle.load(f)

t = data[‘t’] # timestamps [s]

x_init = data[‘x_init’] # initial x position [m]
y_init = data[‘y_init’] # initial y position [m]
th_init = data[‘th_init’] # initial theta position [rad]
v = data[‘v’] # translational velocity input [m/s]
om = data[‘om’] # rotational velocity input [rad/s]

b = data[‘b’] # bearing to each landmarks center in the frame attached to the laser [rad]
r = data[‘r’] # range measurements [m]
l = data[‘l’] # x,y positions of landmarks [m]
d = data[‘d’] # distance between robot center and laser rangefinder [m]

3.2 参数初始化

v_var = 0.01 # translation velocity variance
om_var =1 # rotational velocity variance 待调参数
r_var = 0.001# range measurements variance待调参数
b_var = 0.001 # bearing measurement variance待调参数

Q_km = np.diag([v_var, om_var]) # input noise covariance
cov_y = np.diag([r_var, b_var]) # measurement noise covariance

x_est = np.zeros([len(v), 3]) # estimated states, x, y, and theta
P_est = np.zeros([len(v), 3, 3]) # state covariance matrices

x_est[0] = np.array([x_init, y_init, th_init]) # initial state
P_est[0] = np.diag([1, 1, 0.1]) # initial state covariance

x_check = x_est[0]
x_check = x_check.reshape(1,3)
P_check = P_est[0]

3.3 限定角度的函数

def wraptopi(x):
if x > np.pi:
x = x - (np.floor(x / (2 * np.pi)) + 1) * 2 * np.pi
elif x < -np.pi:
x = x + (np.floor(x / (-2 * np.pi)) + 1) * 2 * np.pi
return x

3.4 测量更新函数

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.5 滤波主程序

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.6 作图

e_fig = plt.figure()
ax = e_fig.add_subplot(111)
ax.plot(x_est[:, 0], x_est[:, 1])
ax.set_xlabel(‘x [m]’)
ax.set_ylabel(‘y [m]’)
ax.set_title(‘Estimated trajectory’)
plt.show()

e_fig = plt.figure()
ax = e_fig.add_subplot(111)
ax.plot(t[:], x_est[:, 2])
ax.set_xlabel(‘Time [s]’)
ax.set_ylabel(‘theta [rad]’)
ax.set_title(‘Estimated trajectory’)
plt.show()

4.仿真结果

图1 地面真值

图2 估计值

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

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