瞭解過導航、雷達數據處理的必然聽過卡爾曼濾波，因爲最近有項目需求，要驗證一下卡爾曼濾波對結果的優化程度，所以入門學習一下卡爾曼濾波器。畢竟是經典的濾波器，網上關於卡爾曼濾波的代碼和文章有很多，一維濾波的代碼也很簡單，就幾條代碼，拿來用很容易。但爲了致敬卡神，則花時間潛心拜讀了一些有關其背後思想的文章，看一輪文獻下來，總的感受就以下兩點：
難見把卡爾曼濾波講得特別通透的博客（參考文獻有比較好的外文文獻及其翻譯）；
同一個專業名詞各種翻譯並且混合使用，無故增加了閱讀障礙；
不同的文獻使用的不同數學表達式，各種各樣很久沒見過的統計學符號會嚇退你；
我數學思維一般，統計學的知識更是忘得差不多了。
基於以上幾點，我斷斷續續花了幾天時間，理解和消化了其中的部分基本思想，由於能力有限，不能夠把卡爾曼濾波器從核心思想到實際運用講的特別透徹，再加上寫本博客的初衷是梳理總結。因此，在看這篇博客之前，更建議你先閱讀博客後面列舉的參考文獻。
1. 簡介
以下是關於卡爾曼濾波的簡介，來自於英文文獻的直譯：
卡爾曼濾波器是一種基於估計算法的遞歸狀態空間模型。換句話說，它是一種最優的遞歸數據處理算法，故卡爾曼濾波器也稱爲Predictor-Corrector算法。
該濾波器以Rudolph E. Kalman的名字命名，是他於1960年發表的著名論文，描述了用一種遞歸方法來解決離散數據線性濾波器的問題。卡爾曼濾波器會處理所有可用的測量值，無論其精度如何，都可以使用來估計相關變量的當前值。
通過簡介，卡爾曼濾波器的特點可以用三個關鍵詞描述：線性、估計以及遞歸。
2. 應用簡述
一句話簡述卡爾曼濾波器就是：解決估計值和測量值之間的權重關係，從而得到一個更接近真實值的結果，這個結果我們叫最優估計。
實際運用中，我們往往是離散的獲取一個個的觀測量（要麼就是間歇性的測量，要麼就是AD採樣的離線數據），比如雷達觀測飛機的運動軌跡、傳感器捕獲被測物的某個物理量變化，那麼首先建立測量系統模型，運用初始值（當前值）來估計下一狀態/值，同時比較下一狀態的實際測量值，我們知道估計值可能不準確（被測物狀態不可能一成不變），測量值也未必有完美的精度（噪聲存在），因此究竟相信哪一個更多一些，是估計值還是測量值？那麼卡爾曼濾波就是解決兩者的權重問題（不確定性問題），計算一個結果，使其更接近於被測物真實的狀態，依次遞歸下去。
卡爾曼濾波器廣泛應用已經超過30年，包括機器人導航，控制，傳感器數據融合甚至在軍事方面的雷達系統以及導彈追蹤等等。近年來更被應用於計算機圖像處理，例如人臉識別，圖像分割，圖像邊緣檢測等等。
話說卡爾曼濾波器用到實際中，是要有效的把上面的概念“具體化”，這也就是很多時候我們知道一些知識的概念，但不知道如何更好的使用它來解決實際問題。
3. 前提條件
一個算法並不是能適用於任何場景，在使用線性卡爾曼濾波器前，它有兩個假設限定了它的應用場景，即：
系統是線性的
系統和測量噪聲是高斯白噪聲
什麼是高斯白噪聲？即噪聲滿足正態分佈，表述如下：
高斯白噪聲在時間尺度上是互不相關的，即上一時刻的噪聲狀態並不能決定下一時刻的噪聲狀態；
噪聲在所有頻率上具有相等的功率，即功率譜密度服從均勻分佈；
4. 分析過程
4.1 基本方程
先直接扔出卡爾曼濾波的經典5個方程（來自於參考文獻）：
- 預測（估計）狀態方程
- 更新方程
以上5個方程以矩陣運算形式代表了線性卡爾曼濾波算法的一般形式（不同文獻的數學表達方式略有不同）。第一眼看到這幾個方程裏面的F、K、H之類的變量以及符號肯定是蒙圈的，即使瞭解了符號代表的意義，在實際過程中怎麼使用可能也不是很清楚。
4.2 方程的解釋
【注：此節不會完整的再驗算一次推導過程，因爲篇幅有限，我只能根據我的理解，解釋捋清推導過程的一個基本脈絡，並解釋參考文獻中稍微有點繞的地方】
上述算法的一般方程來自於參考文獻3，文中以獲取機器人的位置和速度這兩個變量爲例，推導出了（二維）矩陣形式的一般方程，所以結合案例，總結一下方程中各變量符合所代表的意義。
4.2.1 預測（估計）狀態方程
【注：以下表述中“預測”和“估計”表示一個意思】
預測狀態方程是依據被測對象的數據模型建立的，如例子中所示，機器人的狀態x包括位置p和速度v兩個信息：
此後引入概率論和數理統計的概念，根據卡爾曼濾波的前提條件“系統噪聲爲高斯白噪聲”，即在外界干擾等因素的存在，使得機器人狀態中的位置和速度均符合高斯分佈，即有各自的期望以及方差，並且由於位置和速度兩者是相關的（速度決定單位時間內的位移），所以方差爲協方差，則k時刻的狀態期望及協方差可以表示爲：
【注：文獻中協方差用的是求和號，實在看不慣，換成教科書中的c】
所謂預測方程，即通過當前狀態預測下一狀態。由運動方程（系統線性）可知，k時刻的狀態期望由k-1時刻狀態期望決定：
【注：用兩種顏色的字體表示不同的狀態信息】其中delta_t代表兩次預測之間的時間差，並假設機器人處於勻速運動狀態。將上兩式用矩陣形式表示：
其中：
表示系統狀態係數矩陣。由推導過程可見系統狀態係數F_k是與系統模型緊密相關的，此處是運動方程，換個模型，F_k隨之發生改變。
那麼k-1時刻與k時刻的系統估計不確定度（協方差矩陣）又存在什麼樣的關係呢？已知系統狀態分佈關係：
那麼系統分佈的每一個點由k-1時刻變化到k時刻，由上述所知都需乘以一個系統狀態係數，那麼有：
這是由方差運算的基本性質所決定的。因此有：
【注：一開始在文獻中看到此式的解釋有點不明白，主要還是感覺符號的使用不規範以及自身概率論基礎不牢導致，後來這樣理解就稍微清晰一點，上面有關係統狀態係數F_k的推導用的是狀態分佈的期望，而此處又需要從分佈出發來考慮】
綜上所述，得到系統預測方程：
與本章開頭給的一般公式還存在一點細節差異。
在上述分析的過程都是基於假定機器人的運動是勻速運動的，如果運動過程中存在一個加速度a呢，那麼運動方程可以改寫爲：
同樣，改寫爲矩陣形式，得到：
其中：
B_k稱爲控制矩陣，u_k稱爲控制向量，我理解的這兩項乘積代表對系統施加的“穩定”的外部控制機制，比如就是加減速過程。由此可見，依然與系統的模型有關。【注：有的文獻把這項又叫外部噪聲，我感覺不是很合適，因爲本項更像是對系統施加的穩定的、持續的影響】
另外如果外部給系統施加了一個獨立的噪聲（依舊要求爲高斯白噪聲），即分佈滿足N(0,Q_k)，期望爲0，方差爲Q_k。同樣，由方差計算的基本性質（兩個獨立分佈和的期望和方差性質）可以得到：
忽略掉期望的0項，即得到本章開頭的預測方程。
4.2.2 更新方程
更新過程在有些地方又叫“修正過程”，實際上就是真正計算估計值與實測值之間權重的過程。
在上一節中，我們只是對系統在不同時刻的狀態進行了“估計”或者“預測”，說白了就是一種基於模型的猜測，實際真正的狀態是怎樣的，我們還不知道，所以此節的分析要引入“測量”，即某個時刻我們通過檢測手段，如GPS、加速度傳感器，來真實的獲知當前系統狀態。
系統的位置和速度僅僅是物理狀態的一種表徵，有的時候我們通過傳感器無法直接獲取這樣一種物理量，比如加速度傳感器輸出的並不是加速度，而是電壓值，因此需要引入一個空間轉換系數矩陣H，使得傳感器的測量空間與系統物理狀態空間可以轉換。
假設k時刻，系統狀態在系統物理空間狀態爲（X_k，P_k），轉換到傳感器的測量空間，即：
推導過程與系統狀態係數矩陣類似。值得注意的是，此時僅僅代表的是把當前狀態的空間轉換到傳感器測量空間，可以簡單理解爲一種量綱或者單位的轉換，並不是傳感器真正的測量值，所以也叫估計測量值（預測測量值）。
【注：文獻中看到這裏說是傳感器的讀數分佈，我就又蒙圈了，我理解的應該只是通過系統當前狀態分佈轉換到相對應的傳感器輸出，僅僅是一種狀態空間轉換的概念。舉個可能不太恰當的例子：水當前的狀態是沸騰的，那麼轉換到水壺溫度計的讀數應該是100℃】。
很多時候，傳感器的輸出值已經經過處理，與系統的狀態對應，所以往往H的值爲1。
與此同時，我們通過機器人身上的傳感器，如GPS和加速度計，獲取到系統當前真實的狀態，由於噪聲以及傳感器精度等問題，測量結果並不是一個穩定的結果，如某個時刻傳感器的實測值爲Z_k，在這個結果上，實際也疊加有噪聲，測量噪聲也符合高斯分佈（這也是卡爾曼濾波的前提條件），協方差爲R_k。
至此，我們已將估計值轉換到了傳感器空間，並且此時又獲得了傳感器真實的測量值，如下圖所示，那麼我們究竟應該“相信”哪一個結果呢？或者說信哪一方更多一些。我在想這就是卡神最NB的地方，他的博士論文論證了這樣一個結果：我們應該相信兩個分佈的乘積！
以上表述中，已經說明了估計測量值分佈以及傳感器測量值分佈均滿足高斯分佈，那麼他們的乘積依舊滿足高斯分佈，參考文獻中給出了推導證明過程。假定兩個高斯分佈分別爲N_0和N_1，那麼乘積的新分佈N（帶撇）的期望和方差如下：
經過一系列的分解，我們令：
那麼新分佈的期望和方差可以表示爲：
用矩陣形式表示（我們又不得不用求和號表示協方差）：
K就是非常重要的卡爾曼增益矩陣。
將估計測量分佈和傳感器實際測量分佈，分佈代入上式中，並化簡得到：
這就是我們最終得到的更新方程。
更新方程中的X和P（帶撇），就是我們此刻的最佳估計，也就是最接近於真實值的結果。我們可以看到，卡爾曼增益扮演的是權重的角色，K越小，我們越相信估計值；K越大，我們越相信測量值。
下圖給出的是完整的流程圖，每次計算得到的最佳估計結果，代入到下一次的預測中再計算，以實現遞歸。
以上。
5. 實際運用
上面第四章已經完成了線性卡爾曼濾波算法的推導，那麼如何在實際過程來使用呢？實際上上上一章斷斷續續也說了公式中一些符合的實際含義，那麼本章真正意義的把公式用到實際的數據處理當中。
再把卡爾曼濾波算法的5個經典公式扔出來：
再回顧一下式中一些參數的含義：
Bu——控制項，對系統施加的一些穩定的，導致系統狀態發生穩定變化的因素；
Q——系統狀態估計過程的噪聲分佈的方差，表示系統估計不確定度的偏差；
F——系統狀態係數矩陣，決定系統上一時刻到這一時刻的變化模型；
H——狀態空間轉換系數，目的是把系統的物理狀態轉換到跟測量設備（傳感器）一樣的空間，方便在同一維度下比較；
Z——測量設備（傳感器）的實際測量值；
R——測量的不確定度，與噪聲和精度有關；
第四章推導的卡爾曼濾波算法是基於運動學建立的，有位置（位移）和速度以及加速度控制等概念，那如何運用在其他領域呢？如加速度傳感器測量物體振動、溫度傳感器測量溫度值，以及其他一些一維測量領域，又怎麼來用卡爾曼濾波呢。尤其是方程中的Q R的取值等，有些文獻讓讀者毫無根據的瞎試各種Q R，雖然也許也能得到滿意的結果，但做法不敢苟同。
以文獻中（第四部分下）測溫案例類似，假定有一個具有恆溫控制系統（帶溫度計）的液灌，想要知道液體的最佳估計溫度，那麼如何運用公式：
- 由於是有溫控系統，估計液體在k-1時刻和k時刻的溫度狀態是一致的，那麼Bu項爲0，F爲1，則：
- 對於系統估計（協）方差P，由於我們不可能完全知道系統的模型是怎樣的，不妨先預設一個值10000（估計不確定度爲100，則方差爲100^2）【注：標準偏差是描述隨機變量圍繞均值的分散程度的，是可以用來表示不確定度的，而標準偏差的平方就是方差，那方差也是可以反映不確定度的】，沒關係P最終會收斂的。對於溫控系統，存在一定的溫控偏差，視爲系統過程噪聲Q，預設爲0.0001，則：
由於對液灌中液體的溫度估計是用溫度值（℃）表徵，溫度計測量出來的也是溫度值，所以估計和測量屬於同一個狀態空間，不存在狀態轉換，則H爲1。（如果模型有相應的變換，則H以實際爲準）
一般應用，我們是知道溫度計（傳感器）的測量誤差，則R需按照設備設置，此案例中是標準差0.1℃，則方差R=0.01【注：所以R值原則上是不能瞎設的】
那麼最終有：
由此，每一次計算完最佳估計之後，在迭代進預測方程，最終實現卡爾曼濾波算法。完整迭代過程，可以參見文獻。
6. 後續
6.1 結果偏差大
實際運用過程中，卡爾曼濾波器可能存在滯後誤差，即所謂的最佳估計最終與真實結果偏差較大。導致滯後誤差的原因有兩個：
系統動力模型與實際不符；
過程模型的可靠性。選擇了非常低的過程噪聲（案例中的Q=0.0001），而實際溫度波動要大得多。
6.2 非線性系統
理解卡爾曼濾波都是從線性系統出發，處理非線性系統可以採用擴展卡爾曼濾波，我還沒學習過……
7.參考文獻
- KalmanFilter.net
- 輕鬆理解卡爾曼濾波
- 《How a Kalman filter works, in pictures _ Bzarg》
- 《Products and Convolutions of Gaussian Probability Density Functions》
- 圖解卡爾曼濾波