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

1. 簡介

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

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

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

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

2. CTRV運動模型

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

2.1 CTRV的目標狀態量

2.2 CTRV的狀態轉移函數

2.3 CTRV Process Noise

3. Prediction

分為3個步驟：

• 產生Sigma點

  
  

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

  
  

• 預測系統狀態的均值和方差（類似于粒子濾波中的加權平均）

3.1 Generate Sigma Points

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

其中的 λ 是一個超參數，根據公式，λ 越大， sigma點就越遠離狀態的均值，λ 越小， sigma點就越靠近狀態的均值。

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

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

計算增廣的Sigma Points

3.2 預測sigma point

3.3 預測均值和方差

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

4. Update

4.1 Predict Measurement

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

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

Measurement model如圖所示：

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

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

4.2 Update State

首先計算出sigma點集在狀態空間和測量空間的互相關函數T k + 1 ∣ k T

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

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

更新狀態協方差矩陣，計算P k + 1 ∣ k + 1 

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

原文鏈接：

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