手撕自動(dòng)駕駛算法—無(wú)跡卡爾曼濾波

1. 簡(jiǎn)介

無(wú)損卡爾曼濾波又稱(chēng)無(wú)跡卡爾曼濾波（Unscented Kalman Filter，UKF），是無(wú)損變換（Unscented Transform，UT）與標準卡爾曼濾波體系的結合，通過(guò)無(wú)損變換變換使非線(xiàn)性系統方程適用于線(xiàn)性假設下的標準卡爾曼體系。

UKF使用的是統計線(xiàn)性化技術(shù)，我們把這種線(xiàn)性化的方法叫做無(wú)損變換（unscented transformation）這一技術(shù)主要通過(guò)n個(gè)在先驗分布中采集的點(diǎn)（我們把它們叫sigma points）的線(xiàn)性回歸來(lái)線(xiàn)性化隨機變量的非線(xiàn)性函數，由于我們考慮的是隨機變量的擴展，所以這種線(xiàn)性化要比泰勒級數線(xiàn)性化（EKF所使用的策略）更準確。

和EKF一樣，UKF也主要分為預測和更新。

UKF的基本思想是卡爾曼濾波與無(wú)損變換，它能有效地克服EKF估計精度低、穩定性差的問(wèn)題，因為不用忽略高階項，所以對于非線(xiàn)性分布統計量的計算精度高。

2. CTRV運動(dòng)模型

恒定轉率和速度模型（Constant Turn Rate and Velocity，CTRV）

2.1 CTRV的目標狀態(tài)量

2.2 CTRV的狀態(tài)轉移函數

2.3 CTRV Process Noise

3. Prediction

分為3個(gè)步驟：

• 產(chǎn)生Sigma點(diǎn)

  
  

• 預測Sigma點(diǎn)的下一幀狀態(tài) （類(lèi)似于粒子濾波中的預測，更新粒子狀態(tài)）

  
  

• 預測系統狀態(tài)的均值和方差（類(lèi)似于粒子濾波中的加權平均）

3.1 Generate Sigma Points

通常，假定狀態(tài)的個(gè)數為 n ，我們會(huì )產(chǎn)生 2n+1 個(gè)sigma點(diǎn)，其中第一個(gè)就是我們當前狀態(tài)的均值 μ ，sigma點(diǎn)集的均值的計算公式為：

其中的 λ 是一個(gè)超參數，根據公式，λ 越大， sigma點(diǎn)就越遠離狀態(tài)的均值，λ 越小， sigma點(diǎn)就越靠近狀態(tài)的均值。

在我們的CTRV模型中，狀態(tài)數量 n 除了要包含5個(gè)狀態(tài)以外，還要包含處理噪聲 μa 和 μω˙,因為這些處理噪聲對模型也有著(zhù)非線(xiàn)性的影響。在增加了處理噪聲的影響以后，我們的不確定性矩陣 P 就變成了：

其中，P′ 就是我們原來(lái)的不確定性矩陣（在CTRV模型中就是一個(gè) 5×5 的矩陣），Q是處理噪聲的協(xié)方差矩陣，在CTRV模型中考慮到直線(xiàn)加速度核Q的形式為：

計算增廣的Sigma Points

3.2 預測sigma point

3.3 預測均值和方差

x k+1∣k是sigma點(diǎn)集中每個(gè)點(diǎn)各個(gè)狀態(tài)量的加權和， P′ 即為先驗分布的協(xié)方差（不確定性） P k + 1 ∣ k 由每個(gè)sigma點(diǎn)的方差的加權和求得。

4. Update

4.1 Predict Measurement

將先驗映射到測量空間然后算出均值和方差:

測量分為兩個(gè)部分，LIDAR測量和RADAR測量，其中LIDAR測量模型本身就是線(xiàn)性的，所以我們重點(diǎn)還是放在RADAR測量模型的處理上面，RADAR的測量f非線(xiàn)性映射函數為：

Measurement model如圖所示：

再一次，我們使用無(wú)損轉換來(lái)解決，但是這里，我們可以不用再產(chǎn)生sigma points了，我們可以直接使用預測出來(lái)的sigma點(diǎn)集，并且可以忽略掉處理噪聲部分。那么對先驗的非線(xiàn)性映射就可以表示為如下的sigma point預測（即預測非線(xiàn)性變換以后的均值和協(xié)方差）：

這里的 R 也是測量噪聲，在這里我們直接將測量噪聲的協(xié)方差加到測量協(xié)方差上是因為該噪聲對系統沒(méi)有非線(xiàn)性影響。在本例中，以RADAR的測量為例，那么測量噪聲R為：

4.2 Update State

首先計算出sigma點(diǎn)集在狀態(tài)空間和測量空間的互相關(guān)函數T k + 1 ∣ k T

計算卡爾曼增益K k + 1 ∣

更新?tīng)顟B(tài)，計算$x_{k+1|k+1}（其中 z k + 1 是新得到的測量，而 z k + 1 ∣ k 則是我們根據先驗計算出來(lái)的在測量空間的測量)。

更新?tīng)顟B(tài)協(xié)方差矩陣，計算P k + 1 ∣ k + 1 

版權聲明：本文為CSDN博主「令狐少俠、」的原創(chuàng )文章，遵循CC 4.0 BY-SA版權協(xié)議，轉載請附上原文出處鏈接及本聲明。

原文鏈接：

https://blog.csdn.net/weixin_42905141/article/details/99710297