1. 簡介

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

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

和EKF一樣，UKF也主要分為預(yù)測(cè)和更新。

UKF的基本思想是卡爾曼濾波與無損變換，它能有效地克服EKF估計(jì)精度低、穩(wěn)定性差的問題，因?yàn)椴挥煤雎愿唠A項(xiàng)，所以對(duì)于非線性分布統(tǒng)計(jì)量的計(jì)算精度高。

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

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

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

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

2.3 CTRV Process Noise

3. Prediction

分為3個(gè)步驟：

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

  
  

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

  
  

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

3.1 Generate Sigma Points

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

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

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

其中，P′ 就是我們?cè)瓉淼牟淮_定性矩陣（在CTRV模型中就是一個(gè) 5×5 的矩陣），Q是處理噪聲的協(xié)方差矩陣，在CTRV模型中考慮到直線加速度核Q的形式為：

計(jì)算增廣的Sigma Points

3.2 預(yù)測(cè)sigma point

3.3 預(yù)測(cè)均值和方差

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

4. Update

4.1 Predict Measurement

將先驗(yàn)映射到測(cè)量空間然后算出均值和方差:

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

Measurement model如圖所示：

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

這里的 R 也是測(cè)量噪聲，在這里我們直接將測(cè)量噪聲的協(xié)方差加到測(cè)量協(xié)方差上是因?yàn)樵撛肼晫?duì)系統(tǒng)沒有非線性影響。在本例中，以RADAR的測(cè)量為例，那么測(cè)量噪聲R為：

4.2 Update State

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

計(jì)算卡爾曼增益K k + 1 ∣

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

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

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

原文鏈接：

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