[詳細推導]基於EKF的小車運動模型的python程式設計實現
任務介紹
在本任務中，您將使用可用的測量值和運動模型來遞迴估計車輛沿軌跡的位置。車輛有了非常簡單的LIDAR感測器，可以返回與環境中各個地標相對應的範圍（range）和方位測量值（bearing）。假定地標的所有位置是已知的並假設他們已知的資料關聯，即哪個度量屬於哪個界標。.
運動和測量模型
運動模型
車輛運動模型將線速度和角速度里程計讀數作為輸入，並輸出車輛的狀態（即2D狀態）：
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 ) \begin{aligned}\mathbf{x}_{k} &= \mathbf{x}_{k-1} + T\begin{bmatrix}\cos\theta_{k-1} &0 \\\sin\theta_{k-1} &0 \\0 &1 \end{bmatrix}\left(\begin{bmatrix}v_k \\\omega_k\end{bmatrix} + \mathbf{w}_k\right)\, , \, \, \, \, \, \mathbf{w}_k = \mathcal{N}\left(\mathbf{0}, \mathbf{Q}\right)\end{aligned} xk=xk−1+T⎣⎡cosθk−1sinθk−10001⎦⎤([vkωk]+wk),wk=N(0,Q)
- x k = [ x y θ ] T \mathbf{x}_k = \left[ x \, y \, \theta \right]^T xk=[xyθ]T 是當前車輛的狀態
- v k v_k vk 和 ω k \omega_k ωk 是線速度和角速度的里程錶讀數，並作為模型的輸入值
過程噪聲 w k \mathbf{w}_k wk 服從正太分佈（均值為0）和常數協方差 Q \mathbf{Q} Q.
測量模型
測量模型將車輛的當前姿態與鐳射雷達範圍和方位測量值相關聯 y k l = [ r ϕ ] T \mathbf{y}^l_k = \left[r \, \phi \right]^T ykl=[rϕ]T.
y k l = [ ( x l − x k − d cos θ k ) 2 + ( y l − y k − d sin θ k ) 2 a r c t a n ( x l − x k − d cos θ k , y l − y k − d sin θ k ) − θ k ] + n k l , n k l = N ( 0 , R ) \begin{aligned} \mathbf{y}^l_k = \begin{bmatrix} \sqrt{(x_l - x_k - d\cos\theta_{k})^2 + (y_l - y_k - d\sin\theta_{k})^2} \\ arctan\left(x_l - x_k - d\cos\theta_{k},y_l - y_k - d\sin\theta_{k}\right) - \theta_k \end{bmatrix} + \mathbf{n}^l_k \, , \, \, \, \, \, \mathbf{n}^l_k = \mathcal{N}\left(\mathbf{0}, \mathbf{R}\right) \end{aligned} ykl=[(xl−xk−dcosθk)2+(yl−yk−dsinθk)2arctan(xl−xk−dcosθk,yl−yk−dsinθk)−θk]+nkl,nkl=N(0,R)
- x l x_l xl 和 y l y_l yl are 標記值 l l l 的真實值
- x k x_k xk 、 y k y_k yk 和 θ k \theta_{k} θk 表示當前車輛的狀態
- d d d 機器人中心和前輪鐳射測距儀（laser rangefinde)的距離 (LIDAR)
標記值的測量噪聲 n k l \mathbf{n}^l_k nkl 服從正太分佈（均值為0）和常數協方差 R \mathbf{R} R.
準備
由於上述模型是非線性的，因此我們建議使用擴充套件卡爾曼濾波器（EKF）作為狀態估計器。具體來說，需要實現兩個環節：
- 預測環節，使用測距法測量和運動模型在給定的時間步長產生狀態和協方差估計值
- 修正環節，使用範圍和方位角鐳射雷達提供的測量值以糾正狀態和他們的協方差估計
載入資料
import pickle
import numpy as np
import matplotlib.pyplot as plt
import math
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]
# input signal
v = data['v'] # translational velocity input [m/s]
om = data['om'] # rotational velocity input [rad/s]
# bearing and range measurements, LIDAR constants
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]
注意這裡的引數d，即機器人中心到LIDAR的距離放到了d變數中。
Ground Truth
如果實現了該任務，相應的軌跡和角度為：
注意方向值以弧度變成了 [ − π , π ] \left[-\pi,\pi\right] [−π,π]的範圍.
初始化引數
現在已經載入了資料，我們可以開始為求解程式進行設定了。設計濾波器的最重要方面之一是確定輸入和測量噪聲協方差矩陣，以及初始狀態和協方差值，設定值如下：
v_var = 0.01 # translation velocity variance
om_var = 0.1 # rotational velocity variance
r_var = 0.1 # range measurements variance
b_var = 0.1 # 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
注意: 有必要調整測量噪聲的方差 r_var, b_var 使得濾波器表現更好
為了使方向的估計值和方位測量值一致, 有必要把所有測量的 θ \theta θ values 換到 ( − π , π ] (-\pi , \pi] (−π,π] 範圍.
# Wraps angle to (-pi,pi] range
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
這個函式可以將一個單獨的值換算到範圍內，還考慮過這種寫法
def wraptopi1(x):
x[x>np.pi]=x[x>np.pi] - (np.floor(x[x>np.pi] / (2 * np.pi)) + 1) * 2 * np.pi
x[x<-np.pi]= x[x<-np.pi] + (np.floor(x[x<-np.pi] / (-2 * np.pi)) + 1) * 2 * np.pi
return x
這樣定義函式，就可以把陣列內的所有值換到該範圍內。以下的函式都是基於第一個定義方法寫的。
## Correction Step
- 在 x ˇ k \mathbf{\check{x}}_{k} xˇk計算測量模型的雅可比矩陣
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 . \begin{aligned} \mathbf{y}^l_k = &\mathbf{h}(\mathbf{x}_{k}, \mathbf{n}^l_k) \\\\ \mathbf{H}_{k} = \frac{\partial \mathbf{h}}{\partial \mathbf{x}_{k}}\bigg|_{\mathbf{\check{x}}_{k},0}& \, , \, \, \, \, \mathbf{M}_{k} = \frac{\partial \mathbf{h}}{\partial \mathbf{n}_{k}}\bigg|_{\mathbf{\check{x}}_{k},0} \, . \end{aligned} ykl=Hk=∂xk∂h∣∣∣∣xˇk,0h(xk,nkl),Mk=∂nk∂h∣∣∣∣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 \begin{aligned} \mathbf{K}_k &= \mathbf{\check{P}}_k \mathbf{H}_k^T \left(\mathbf{H}_k \mathbf{\check{P}}_k \mathbf{H}_k^T + \mathbf{M}_k \mathbf{R}_k \mathbf{M}_k^T \right)^{-1} \end{aligned} Kk=PˇkHkT(HkPˇkHkT+MkRkMkT)−1
- 修正預測狀態
y ˇ k l = h ( x ˇ k , 0 ) x ^ k = x ˇ k + K k ( y k l − y ˇ k l ) \begin{aligned} \mathbf{\check{y}}^l_k &= \mathbf{h}\left(\mathbf{\check{x}}_k, \mathbf{0}\right) \\ \mathbf{\hat{x}}_k &= \mathbf{\check{x}}_k + \mathbf{K}_k \left(\mathbf{y}^l_k - \mathbf{\check{y}}^l_k\right) \end{aligned} yˇklx^k=h(xˇk,0)=xˇk+Kk(ykl−yˇkl) - 修正協方差
P
^
k
=
(
I
−
K
k
H
k
)
P
ˇ
k
\begin{aligned} \mathbf{\hat{P}}_k &= \left(\mathbf{I} - \mathbf{K}_k \mathbf{H}_k \right)\mathbf{\check{P}}_k \end{aligned}
P^k=(I−KkHk)Pˇk
因此，這裡詳細推導一下
H
k
H_{k}
Hk和
M
k
M_{k}
Mk，
H
k
H_{k}
Hk是h對
x
k
x_{k}
xk求偏導，而h就是
y
k
l
{y}^l_k
ykl,
y
k
l
{y}^l_k
ykl是(2,1)的矩陣，分別對裡面的
x
k
x_{k}
xk、
y
k
y_{k}
yk、
θ
k
\theta_{k}
θk求偏導那麼可以得到(2,3)的矩陣，即
H
k
H_{k}
Hk的維數是(2,3)
y
k
l
=
[
(
x
l
−
x
k
−
d
cos
θ
k
)
2
+
(
y
l
−
y
k
−
d
sin
θ
k
)
2
a
r
c
t
a
n
(
x
l
−
x
k
−
d
cos
θ
k
,
y
l
−
y
k
−
d
sin
θ
k
)
−
θ
k
]
+
n
k
l
,
n
k
l
=
N
(
0
,
R
)
\begin{aligned} \mathbf{y}^l_k = \begin{bmatrix} \sqrt{(x_l - x_k - d\cos\theta_{k})^2 + (y_l - y_k - d\sin\theta_{k})^2} \\ arctan\left(x_l - x_k - d\cos\theta_{k},y_l - y_k - d\sin\theta_{k}\right) - \theta_k \end{bmatrix} + \mathbf{n}^l_k \, , \, \, \, \, \, \mathbf{n}^l_k = \mathcal{N}\left(\mathbf{0}, \mathbf{R}\right) \end{aligned}
ykl=[(xl−xk−dcosθk)2+(yl−yk−dsinθk)2arctan(xl−xk−dcosθk,yl−yk−dsinθk)−θk]+nkl,nkl=N(0,R)
為了表示方便，採用三個中間變數A、B、C來簡化過程。
A
=
x
l
−
x
k
−
d
cos
θ
k
A=x_l - x_k - d\cos\theta_{k}
A=xl−xk−dcosθk
B
=
y
l
−
y
k
−
d
sin
θ
k
B=y_l - y_k - d\sin\theta_{k}
B=yl−yk−dsinθk
C
=
(
x
l
−
x
k
−
d
cos
θ
k
)
2
+
(
y
l
−
y
k
−
d
sin
θ
k
)
2
=
A
2
+
B
2
C=\sqrt{(x_l - x_k - d\cos\theta_{k})^2 + (y_l - y_k - d\sin\theta_{k})^2}=\sqrt{A^2 + B^2}
C=(xl−xk−dcosθk)2+(yl−yk−dsinθk)2=A2+B2
因此求導後的
H
k
H_{k}
Hk為
H k = [ − A / C − B / C ( d / C ) ∗ ( A sin θ − B cos θ ) B / C 2 − A / C 2 ( − d / C 2 ) ∗ cos θ ( A + B ) − 1 ] \begin{aligned} \mathbf{H}^k = \begin{bmatrix} -A/C & -B/C & (d/C) *(A\sin\theta-B\cos\theta)\\ B/C^2 & -A/C^2 & (-d/C^2)*\cos\theta(A+B)-1 \end{bmatrix} \end{aligned} Hk=[−A/CB/C2−B/C−A/C2(d/C)∗(Asinθ−Bcosθ)(−d/C2)∗cosθ(A+B)−1]
def measurement_update(lk, rk, bk, P_check, x_check):
theta = x_check[:,2]
bk = wraptopi(bk)
theta= wraptopi(theta)
A = lk[0] - x_check[:,0] - d * math.cos(theta)
B = lk[1] - x_check[:,1] - d * math.sin(theta)
C = (A ** 2 + B ** 2) ** (0.5)
# 1. Compute measurement Jacobian
M = np.identity(2)
H = np.array([[-A/C,-B/C,(d/C)*(A*math.sin(theta)- B*math.cos(theta))],
[B/(C**2),-A/(C**2),((-d/(C**2))*math.cos(theta)*(A+B))-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) - theta
h = wraptopi(h)
ss = K @ np.array([rk-C,bk-h])
x_check = x_check + ss.reshape(1,3)
theta = x_check[:,2]
theta= wraptopi(theta)
# 4. Correct covariance
P_check = (np.identity(3)-K @ H) @ P_check
return x_check, P_check
預測環節
現在，實現主濾波器迴圈，使用提供的運動模型定義EKF的預測步驟：
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
.
\begin{aligned} \mathbf{\check{x}}_k &= \mathbf{f}\left(\mathbf{\hat{x}}_{k-1}, \mathbf{u}_{k-1}, \mathbf{0} \right) \\ \mathbf{\check{P}}_k &= \mathbf{F}_{k-1}\mathbf{\hat{P}}_{k-1}\mathbf{F}_{k-1}^T + \mathbf{L}_{k-1}\mathbf{Q}_{k-1}\mathbf{L}_{k-1}^T \, . \end{aligned}
xˇkPˇk=f(x^k−1,uk−1,0)=Fk−1P^k−1Fk−1T+Lk−1Qk−1Lk−1T.
其中
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} \mathbf{F}_{k-1} = \frac{\partial \mathbf{f}}{\partial \mathbf{x}_{k-1}}\bigg|_{\mathbf{\hat{x}}_{k-1},\mathbf{u}_{k},0} \, , \, \, \, \, \mathbf{L}_{k-1} = \frac{\partial \mathbf{f}}{\partial \mathbf{w}_{k}}\bigg|_{\mathbf{\hat{x}}_{k-1},\mathbf{u}_{k},0} \, . \end{aligned}
Fk−1=∂xk−1∂f∣∣∣∣x^k−1,uk,0,Lk−1=∂wk∂f∣∣∣∣x^k−1,uk,0.
那麼同樣求一下
F
k
−
1
{F}_{k-1}
Fk−1,由於f’就是
x
k
x_{k}
xk,類似地，f是(3,1)的矩陣，對裡面的
x
k
x_{k}
xk、
y
k
y_{k}
yk、
θ
k
\theta_{k}
θk求偏導那麼可以得到(3,3)的矩陣，即
F
k
−
1
F_{k-1}
Fk−1的維數是(3,3)
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
)
\begin{aligned}\mathbf{x}_{k} &= \mathbf{x}_{k-1} + T\begin{bmatrix}\cos\theta_{k-1} &0 \\\sin\theta_{k-1} &0 \\0 &1 \end{bmatrix}\left(\begin{bmatrix}v_k \\\omega_k\end{bmatrix} + \mathbf{w}_k\right)\, , \, \, \, \, \, \mathbf{w}_k = \mathcal{N}\left(\mathbf{0}, \mathbf{Q}\right)\end{aligned}
xk=xk−1+T⎣⎡cosθk−1sinθk−10001⎦⎤([vkωk]+wk),wk=N(0,Q)
由於
x
k
−
1
{x}_{k-1}
xk−1是
(
x
k
−
1
,
y
k
−
1
,
θ
k
−
1
)
T
({x}_{k-1},{y}_{k-1},{\theta}_{k-1})^T
(xk−1,yk−1,θk−1)T,那麼帶入後
x
k
=
[
x
k
−
1
+
T
cos
θ
k
−
1
∗
(
v
k
+
w
k
)
y
k
−
1
+
T
sin
θ
k
−
1
∗
(
v
k
+
w
k
)
T
∗
(
v
k
+
w
k
)
]
\begin{aligned}\mathbf{x}_{k} &= \begin{bmatrix}{x}_{k-1}+T\cos\theta_{k-1} *(v_k+ \mathbf{w}_k) \\ {y}_{k-1}+T\sin\theta_{k-1} *(v_k+ \mathbf{w}_k) \\T*(v_k+ \mathbf{w}_k)\end{bmatrix}\end{aligned}
xk=⎣⎡xk−1+Tcosθk−1∗(vk+wk)yk−1+Tsinθk−1∗(vk+wk)T∗(vk+wk)⎦⎤
那麼每一列分別對
x
k
−
1
,
y
k
−
1
,
θ
k
−
1
{x}_{k-1},{y}_{k-1},{\theta}_{k-1}
xk−1,yk−1,θk−1求導可以得到
F
k
−
1
F_{k-1}
Fk−1
F
k
−
1
=
[
1
0
−
T
∗
v
k
∗
sin
θ
k
−
1
0
1
T
∗
v
k
∗
cos
θ
k
−
1
0
0
1
]
\begin{aligned}\mathbf{F}_{k-1} &= \begin{bmatrix}1&0&-T*v_{k}*\sin\theta_{k-1}\\ 0&1&T*v_{k}*\cos\theta_{k-1} \\0&0&1\end{bmatrix}\end{aligned}
Fk−1=⎣⎡100010−T∗vk∗sinθk−1T∗vk∗cosθk−11⎦⎤
類似地，對
w
k
\mathbf{w}_k
wk求導顯而易見
L
k
−
1
=
T
[
cos
θ
k
−
1
0
sin
θ
k
−
1
0
0
1
]
\begin{aligned}\mathbf{L}_{k-1} &=T\begin{bmatrix}\cos\theta_{k-1} &0 \\\sin\theta_{k-1} &0 \\0 &1 \end{bmatrix}\end{aligned}
Lk−1=T⎣⎡cosθk−1sinθk−10001⎦⎤
#### 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 = x_est[k - 1,:]
P_check = P_est[k - 1,:,:]
theta = x_check[2]
theta = wraptopi(theta)
# 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
theta = x_check[0,2]
theta = wraptopi(theta)
# 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,0]
x_est[k, 1] = x_check[0,1]
x_est[k, 2] = x_check[0,2]
P_est[k, :, :] = P_check
最後畫圖顯示下結果
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()
事實上，有時候結果並非很滿意，那麼按照一下思路更改方差
v_var不做更改（v_var=0.01）- 15>
om_var>5 - 1>
r_var>0.001 - 1>
b_var>0.001
相關文章
- 基於Python的Xgboost模型實現Python模型
- 基於css3動畫實現的旅行的小車CSSS3動畫
- 基於android的智慧導診的設計與實現Android
- 基於AliOS的車載小程式iOS
- 基於ReactiveCocoa和MVVM設計的購物車基本操作實現程式碼解析ReactMVVM
- 基於AlexNet和Inception模型思想的TFCNet模型設計與實現模型
- 基於java的企業車輛管理系統的設計與實現Java
- javascript: 基於原型的物件導向程式設計JavaScript原型物件程式設計
- 基於 LLM 的智慧運維 Agent 系統設計與實現運維
- asp.net 實現購物車詳細程式碼ASP.NET
- 基於XML的購物車的實現(轉)XML
- 關於詳細設計/程式碼的同步問題--請教
- Go物件導向程式設計OOP的實現Go物件程式設計OOP
- 基於任務的導航設計
- Socket程式設計入門（基於Java實現）程式設計Java
- javascript實現的運動框架詳解JavaScript框架
- 基於使用者偏好的新聞推薦系統的設計與實現
- [.net 物件導向程式設計基礎] (2) 關於物件導向程式設計物件程式設計
- 看不見的設計——關於“分享”的小細節
- 基於Python的自動化程式碼審計Python
- 實現基於內容的電影推薦系統—程式碼實現
- 詳細講解DirectDraw程式設計基礎(轉)程式設計
- python物件導向程式設計基礎Python物件程式設計
- python基礎（物件導向程式設計）Python物件程式設計
- 非同步程式設計：.NET 4.5 基於任務的非同步程式設計模型(TAP)非同步程式設計模型
- 基於Python與Qt的快速GUI程式設計PythonQTGUI程式設計
- 基於Python的Akka實現Python
- 基於 Agent 的模型入門：Python 實現隔離模擬模型Python
- 關於直播賣貨小程式原始碼推流功能的實現原始碼
- 基於RBAC的許可權設計模型模型
- 基於jsp運動會賽前管理子系統的設計與實現3(含原始檔)JS
- 基於python爬蟲技術對於淘寶的資料分析的設計與實現Python爬蟲
- 移動推廣運營的AARRR模型分享模型
- 軟體工程結隊專案：基於C++實現的自動生成小學四則運算的命令列程式軟體工程C++命令列
- 基於 Angular Material 的 Data Grid 設計實現Angular
- 基於Python的滲透測試資訊收集系統的設計和實現Python
- 12 Python物件導向程式設計：運算子過載Python物件程式設計
- Python實現快遞分揀小程式(附原始碼和超詳細註釋)Python原始碼