卡爾曼濾波的變種有哪些？1

1從基礎卡爾曼濾波到互補卡爾曼濾波

卡爾曼濾波自從1960被Kalman發明并應用到阿波羅登月計劃之后一直經久不衰，直到現在也被機器人、自動駕駛、飛行控制等領域應用。基礎卡爾曼濾波只能對線性系統建模；擴展卡爾曼濾波對非線性方程做線性近似以便將卡爾曼濾波應用到非線性系統。

后來研究者發現將系統狀態分成主要成分和誤差，并將卡爾曼濾波用來預測誤差，會使得系統的近似程度更高，效果更好。在姿態解算任務中，研究者們用輔助傳感器如加速度計和磁力計來修正角速度計的積分結果，得到互補卡爾曼濾波的形式。

卡爾曼濾波是一種工具，對實際問題的不同建模方式會得到不同形式的卡爾曼濾波器。這導致了對于初學者不知道從何看起是好。另外也似乎很少有文章對基礎卡爾曼濾波到各種形式的濾波形式做總結說明，于是便有了這篇文章。本文會從以下幾個方面分析和講解多種卡爾曼濾波器形式：

基礎卡爾曼濾波 ——對線性系統的預測
擴展卡爾曼濾波 ——基礎卡爾曼濾波在非線性系統的擴展
基于四元素的卡爾曼濾波器 ——基于實際問題的講解
狀態誤差卡爾曼濾波 ——將狀態誤差引入狀態向量
互補卡爾曼濾波 ——一種只使用角度誤差和角速度誤差作為狀態向量和測量向量的濾波器形式

2符號定義

小寫字母為變量；加粗小寫字母為向量；大寫和加粗大寫為矩陣

3基礎卡爾曼濾波器

宏觀認識

卡爾曼濾波包含兩個步驟

預測（prediction）—— Dynamic model
更新（correction/measurment update）—— Observation model

所謂預測，就是用一個數學模型，根據當前的傳感器輸入，直接計算此時系統的狀態。可以理解為一個方程的計算就行。

所謂的更新，就是在某些時刻或者每一時刻，獲取一些系統的其他狀態輸入（我們將這個值叫做 測量值 ），比較此刻預測的系統狀態和測量的系統狀態，對預測出的系統狀態進行修正，因此也叫 測量更新 （measurment update）。

整體框架如下圖所示

狀態方程及測量方程

其中是系統狀態向量，是測量向量。分別是過程噪聲和觀測噪聲，且滿足高斯分布

預測過程

其中，是先驗狀態的誤差協方差矩陣

更新過程

詳細公式推導

本文作為一篇概述性文章，為了不使篇幅過于冗長，不進行基礎卡爾曼濾波器公式的推導。想完全理解基礎卡爾曼濾波器可以參考下面這幾篇資料：

卡爾曼濾波基礎知識及公式推導——較為形象化地講解預測和更新這兩個過程之間地概率分布關系
wiki Kalman Filter——準確的公式化推導

如何理解卡爾曼濾波器？

從概率分布的角度

卡爾曼濾波器將系統狀態的變化中的過程噪聲假設為均值為0的高斯噪聲，使得狀態向量也變為一個符合高斯分布的隨機向量。同時對觀測過程的噪聲也假設為均值為0的高斯噪聲。通過測量方程，即公式（1-2）得到將狀態向量映射到測量向量的函數。于是，當得到測量值的時候，可以利用測量值與狀態向量之間的關系得出另外一個對狀態向量的估計。利用測量值得出的狀態估計和狀態方程計算的狀態均符合高斯分布，兩個高斯分布的聯合概率分布依舊保持高斯特性。進一步推導可以得到公式（1-5）到公式（1-7）。關于這個角度的理解可以閱讀上面推薦的第一篇文章。

從最小化誤差的角度

卡爾曼濾波的最終輸出是，真實的狀態為，令

對誤差的平方求最小值，同樣可以推導出公式（1-5）到公式（1-7）。因此卡爾曼濾波器也是系統狀態的最優估計。關于這個角度的理解和推導可以參考上面推薦的第二篇文章。

4擴展卡爾曼濾波

非線性方程的線性近似

卡爾曼濾波器建立在線性的狀態方程和測量方程也就是公式（1-1）和公式（1-2）。但是在實際應用中，更多的關系是非線形關系，比如如何從連續的角速度計算出車輛當前的姿態角等。為了能夠利用基本卡爾曼濾波器的預測和更新過程，對于非線性的狀態方程和觀測方程，我們利用一階的泰勒展開，將非線性公式近似為線性公式。