任務介紹

---

在本任務中，您將使用可用的測量值和運動模型來遞迴估計車輛沿軌跡的位置。車輛有了非常簡單的LIDAR感測器，可以返回與環境中各個地標相對應的範圍（range）和方位測量值（bearing）。假定地標的所有位置是已知的並假設他們已知的資料關聯，即哪個度量屬於哪個界標。.

運動和測量模型

---

運動模型

車輛運動模型將線速度和角速度里程計讀數作為輸入，並輸出車輛的狀態（即2D狀態）：

$$\begin{array}{rl}{\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}\left(0,\mathbf{Q}\right)\end{array}$$

• ${\mathbf{x}}_k={\left[xy\theta \right]}^T$ 是當前車輛的狀態
• $v_k$ 和 ${\omega }_k$ 是線速度和角速度的里程錶讀數，並作為模型的輸入值

過程噪聲 ${\mathbf{w}}_k$ 服從正太分佈（均值為0）和常數協方差 $\mathbf{Q}$.

測量模型

測量模型將車輛的當前姿態與鐳射雷達範圍和方位測量值相關聯 ${\mathbf{y}}_k^l={\left[r\varphi \right]}^T$.

$$\begin{array}{r}{\mathbf{y}}_k^l=\left[\begin{array}{c}\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{array}\right]+{\mathbf{n}}_k^l,{\mathbf{n}}_k^l=\mathcal{N}\left(0,\mathbf{R}\right)\end{array}$$

• $x_l$ 和 $y_l$ are 標記值 $l$ 的真實值
• $x_k$ 、$y_k$ 和 ${\theta }_k$ 表示當前車輛的狀態
• $d$ 機器人中心和前輪鐳射測距儀（laser rangefinde)的距離 (LIDAR)

標記值的測量噪聲 ${\mathbf{n}}_k^l$ 服從正太分佈（均值為0）和常數協方差 $\mathbf{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

---

• 在${\check{\mathbf{x}}}_k$計算測量模型的雅可比矩陣

$$\begin{array}{rl}{\mathbf{y}}_k^l=& \mathbf{h}({\mathbf{x}}_k,{\mathbf{n}}_k^l)\\ & \\ {\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}.\end{array}$$

• 計算卡爾曼增益

$$\begin{array}{rl}{\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}\end{array}$$

• 修正預測狀態
  $$\begin{array}{rl}{\check{\mathbf{y}}}_k^l& =\mathbf{h}\left({\check{\mathbf{x}}}_k,0\right)\\ {\hat{\mathbf{x}}}_k& ={\check{\mathbf{x}}}_k+{\mathbf{K}}_k\left({\mathbf{y}}_k^l-{\check{\mathbf{y}}}_k^l\right)\end{array}$$
• 修正協方差

$$\begin{array}{rl}{\hat{\mathbf{P}}}_k& =\left(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k\right){\check{\mathbf{P}}}_k\end{array}$$

因此，這裡詳細推導一下$H_k$和$M_k$，$H_k$是h對$x_k$求偏導，而h就是$y_k^l$,$y_k^l$是(2,1)的矩陣，分別對裡面的$x_k$、$y_k$、${\theta }_k$求偏導那麼可以得到(2,3)的矩陣，即$H_k$的維數是(2,3)
$$\begin{array}{r}{\mathbf{y}}_k^l=\left[\begin{array}{c}\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{array}\right]+{\mathbf{n}}_k^l,{\mathbf{n}}_k^l=\mathcal{N}\left(0,\mathbf{R}\right)\end{array}$$
為了表示方便，採用三個中間變數A、B、C來簡化過程。
$A=x_l-x_k-d\cos {\theta }_k$
$B=y_l-y_k-d\sin {\theta }_k$
$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}$
因此求導後的$H_k$為

$$\begin{array}{r}{\mathbf{H}}^k=\left[\begin{array}{ccc}-A/C& -B/C& (d/C)\ast (A\sin \theta -B\cos \theta )\\ B/C^2& -A/C^2& (-d/C^2)\ast \cos \theta (A+B)-1\end{array}\right]\end{array}$$

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的預測步驟：
$$\begin{array}{rl}{\check{\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.\end{array}$$
其中
$$\begin{array}{r}{\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}$$
那麼同樣求一下$F_{k-1}$,由於f’就是$x_k$,類似地，f是(3,1)的矩陣，對裡面的$x_k$、$y_k$、${\theta }_k$求偏導那麼可以得到(3,3)的矩陣，即$F_{k-1}$的維數是(3,3)
$$\begin{array}{rl}{\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}\left(0,\mathbf{Q}\right)\end{array}$$
由於$x_{k-1}$是$(x_{k-1},y_{k-1},{\theta }_{k-1}{)}^T$,那麼帶入後
$$\begin{array}{rl}{\mathbf{x}}_k& =\left[\begin{array}{c}{x}_{k-1}+T\cos {\theta }_{k-1}\ast (v_k+{\mathbf{w}}_k)\\ y_{k-1}+T\sin {\theta }_{k-1}\ast (v_k+{\mathbf{w}}_k)\\ T\ast (v_k+{\mathbf{w}}_k)\end{array}\right]\end{array}$$
那麼每一列分別對$x_{k-1},y_{k-1},{\theta }_{k-1}$求導可以得到$F_{k-1}$
$$\begin{array}{rl}{\mathbf{F}}_{k-1}& =\left[\begin{array}{ccc}1& 0& -T\ast v_k\ast \sin {\theta }_{k-1}\\ 0& 1& T\ast v_k\ast \cos {\theta }_{k-1}\\ 0& 0& 1\end{array}\right]\end{array}$$
類似地，對 ${\mathbf{w}}_k$求導顯而易見
$$\begin{array}{rl}{\mathbf{L}}_{k-1}& =T\left[\begin{array}{cc}\cos {\theta }_{k-1}& 0\\ \sin {\theta }_{k-1}& 0\\ 0& 1\end{array}\right]\end{array}$$

#### 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