|
1. 簡介 無損卡爾曼濾波又稱無跡卡爾曼濾波（Unscented Kalman Filter，UKF），是無損變換（Unscented Transform，UT）與標(biāo)準(zhǔn)卡爾曼濾波體系的結(jié)合，通過無損變換變換使非線性系統(tǒng)方程適用于線性假設(shè)下的標(biāo)準(zhǔn)卡爾曼體系。 UKF使用的是統(tǒng)計線性化技術(shù)，我們把這種線性化的方法叫做無損變換（unscented transformation）這一技術(shù)主要通過n個在先驗分布中采集的點（我們把它們叫sigma points）的線性回歸來線性化隨機變量的非線性函數(shù)，由于我們考慮的是隨機變量的擴展，所以這種線性化要比泰勒級數(shù)線性化（EKF所使用的策略）更準(zhǔn)確。 和EKF一樣，UKF也主要分為預(yù)測和更新。 UKF的基本思想是卡爾曼濾波與無損變換，它能有效地克服EKF估計精度低、穩(wěn)定性差的問題，因為不用忽略高階項，所以對于非線性分布統(tǒng)計量的計算精度高。 2. CTRV運動模型 恒定轉(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個步驟：
 3.1 Generate Sigma Points  通常，假定狀態(tài)的個數(shù)為 n ，我們會產(chǎn)生 2n+1 個sigma點，其中第一個就是我們當(dāng)前狀態(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 預(yù)測sigma point 3.3 預(yù)測均值和方差 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了，我們可以直接使用預(yù)測出來的sigma點集，并且可以忽略掉處理噪聲部分。那么對先驗的非線性映射就可以表示為如下的sigma point預(yù)測（即預(yù)測非線性變換以后的均值和協(xié)方差）： 這里的 R 也是測量噪聲，在這里我們直接將測量噪聲的協(xié)方差加到測量協(xié)方差上是因為該噪聲對系統(tǒng)沒有非線性影響。在本例中，以RADAR的測量為例，那么測量噪聲R為： 4.2 Update State 首先計算出sigma點集在狀態(tài)空間和測量空間的互相關(guān)函數(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)載請附上原文出處鏈接及本聲明。 |
|
|
