概率機器人中的高斯濾波

---

高斯濾波
常用的高斯濾波有卡爾曼濾波、擴展卡爾曼濾波、無跡卡爾曼濾波、信息濾波。
連續空間。
置信度用多元正態分佈表示。
高斯是單峯的，具有單一的極大值。
高斯濾波的缺點：單峯高斯分佈方面的侷限。

矩參數：高斯濾波中參數均值和方差稱爲矩參數，均值是概率分佈的一階矩陣，方差是概率分佈的二階矩陣，正態分佈其他矩都是0。
正則參數：也叫本質參數，可通過矩參數轉化。

---

卡爾曼濾波：（KF）

適用於：線性高斯系統
作用：對連續狀態的置信度計算。不適用於離散或混合狀態空間。
使用KF的四個條件：
（1）滿足貝葉斯濾波的馬爾可夫假設
（2）狀態轉移概率，必須是帶有隨機高斯噪聲的參數的線性函數
（3）測量概率，也與帶有高斯噪聲的自變量呈線性關係。
（4）初始置信度，必須是正態分佈
滿足上面的四個條件，在任何時刻t總是高斯分佈。

卡爾曼濾波算法：
KF表示均值爲μ_t、方差爲Σ_t在時刻t的置信度bel(x_t)。
KF算法僞代碼如下圖所示：

KF算法的輸入：
（1）t-1時刻的置信度，其均值和方差分別用μ_t-1、Σ_t-1表示。
（2）t時刻的控制向量u_t和測量向量z_t

KF算法的輸出：
輸出t時刻的置信度，用用μ_t、Σ_t表示。然後用多元正態分佈公式輕鬆的就計算出來了。

KF算法的流程：
（1） 通過u_t和上一時刻的置信度，求預測置信度，第2-3行
（2） 計算卡爾曼增益矩陣，反映了測量數據在新的狀態估計的程度。第4行
（3） 通過卡爾曼增益矩陣、真實的測量向量z_t、預測置信度來更新該時刻的置信度。稱爲更新，第5-6行

KF算法的計算效率：
矩陣求逆的計算複雜度爲O(d^2.4),這裏指維數爲dxd的矩陣
KF算法的每一次迭代下界近似爲O(k^2.4)，這裏k是測量向量zt的維數。源於第4行矩陣求逆。
第6行乘法，複雜度至少爲O(n²),n爲狀態空間的維數。

KF算法的侷限性：
只適用於高斯線性系統。

---

擴展卡爾曼濾波：(EKF)

卡爾曼濾波雖然很好，但是不幸的是，實際狀態轉移和測量很少是線性的。所以擴展卡爾曼濾波就出現了。
擴展卡爾曼濾波放寬了使用條件，假設狀態轉移概率和測量概率分別由非線性函數g和h控制。
對於任意函數g和h，置信度不再是一個高斯分佈。
實際上EKF計算的置信度是真實置信度的高斯近似值。EKF繼承KF表示t時刻置信度的表示，但是有不同。

EKF的目標：從EK精確的計算後驗概率，轉變爲有效地估計均值和方差。

EKF近似的思想：通過一階泰勒展開的線性化

線性化通過一個在高斯函數的均值處與非線性函數g相切的線性函數去近似g。
蒙特卡洛對高斯的估計是將500000點進行g變換，然後計算出均值和方差。與EKF的存在誤差。

EKF的線性化：
（1） 泰勒展開根據g的值和斜率構造一個函數g的線性近似函數。

對於高斯函數，最有可能的狀態就是後驗的均值μ_t-1。

寫成高斯形式，狀態轉移概率可由下式近似：

G_t爲n x n矩陣，n代表狀態的維數。該矩陣稱爲雅克比矩陣，不同時刻是不同的。
（2） 將函數h線性化

寫成高斯函數，測量概率爲：

擴展卡爾曼濾波算法：
算法僞代碼如下：

算法流程：與KF一致，只是其中內容被替換了，核心就是將非線性的狀態轉移和測量線性化後進行與KF一樣的預測、更新流程。
雅克比矩陣G_t對應A_t和B_t，H_t對應C_t。

EKF的時間複雜度：
O(k^2.4+n²),其中，k是測量向量z_t的維數，n爲狀態向量x_t的維數。

EKF的侷限性：
如果非線性函數在估計均值處近似爲線性，那麼EKF能夠以足夠高的精度近似後驗置信度。
若是較高的不確定性通常會導致結果隨機變量的均值和方差估計更不精確。

---

無跡卡爾曼濾波：（UKF）

UKF通過使用加權統計線性迴歸過程實現隨機線性化。

線性化方式：
不是採用泰勒級數展開的方式，通過取的σ點直接經過函數g進行變換。若是n維高斯分佈，結果有2n+1個σ點。σ點位於均值處及對稱分佈於主軸的協方差處（每維兩個）。
每個σ點都有兩個與之相關的權值，一個權值計算均值時使用，另一個權值計算高斯的協方差時使用。

UKF的精度高於EKF

無跡卡爾曼濾波算法：
UKF算法的漸近複雜性與EKF的相同，UKF的一個優點是不需要計算雅克比矩陣，因此UKF也被稱爲免求導濾波器。

UKF的侷限性：
如果置信度是高度非高斯的，UKF表示就具有很大侷限性，濾波實現就會很差。

---

信息濾波：（IF）

KF的對偶濾波算法就是信息濾波IF。IF用高斯分佈表示置信度，標準的IF和KF有相同的假設。
KF算法的高斯分佈由它們的矩（均值和方差）表示。
IF算法的高斯分佈由正則參數表示。該正則參數由一個信息矩陣和信息向量組成。

正則參數：

信息濾波算法：
信息濾波的僞代碼如下：

IF算法和KF的算法流程一樣，式子可以也通過矩和本質參數的轉化，化爲一致。
需要滿足KF滿足的所有條件，也只適用於線性高斯系統。
輸入和輸出變成了本質參數，也是預測和更新過程。
IF算法的複雜度正好和KF的順序相反。