手撕自動駕駛算法—無跡卡爾曼濾波

1. 簡介

無損卡爾曼濾波又稱無跡卡爾曼濾波（Unscented Kalman Filter，UKF），是無損變換（Unscented Transform，UT）與標準卡爾曼濾波體系的結(jié)合，通過無損變換變換使非線性系統(tǒng)方程適用于線性假設下的標準卡爾曼體系。

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

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

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

2. CTRV運動模型

恒定轉(zhuǎn)率和速度模型（Constant Turn Rate and Velocity，CTRV）

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

2.2 CTRV的狀態(tài)轉(zhuǎn)移函數(shù)

2.3 CTRV Process Noise

3. Prediction

分為3個步驟：

• 產(chǎn)生Sigma點

  
  

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

  
  

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

3.1 Generate Sigma Points

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

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

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

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

計算增廣的Sigma Points

3.2 預測sigma point

3.3 預測均值和方差

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

4. Update

4.1 Predict Measurement

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

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

Measurement model如圖所示：

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

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

4.2 Update State

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

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

更新狀態(tài)，計算$x_{k+1|k+1}（其中 z k + 1 是新得到的測量，而 z k + 1 ∣ k 則是我們根據(jù)先驗計算出來的在測量空間的測量)。

更新狀態(tài)協(xié)方差矩陣，計算P k + 1 ∣ k + 1 

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

原文鏈接：

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