這段時間做軸承故障診斷和預測的時候，需要一個針對已經獲取了特徵向量的工具來對軸承故障狀態進行估計和預測。卡爾曼濾波器可以實現對過去、當前和未來目標位置的估計，所以想通過卡爾曼濾波器的設計思路找到一些靈感。雖然最後發現：卡爾曼濾波器中的狀態量是有具體的物理含義的物理量，而表徵軸承故障狀態的量只是一種表徵量。這兩者之間存在著本質的差別，因為軸承的退化過程目前為止還不能建模。雖然如此，我還是想將卡爾曼濾波器詳細的推導過程分享給大家。
學習過程中，參考了白巧克力亦唯心的文章：卡爾曼濾波–從推導到應用（一），卡爾曼濾波–從推導到應用（二），在此給出直達連線。他/她的文章有故事，有推導，有例子，有程式碼，很優秀，向大家推薦。我在這篇文章裡希望把很多文章中語焉不詳的推導，以及符號的定義闡釋清楚，並加入一些自己的理解，希望和大家分享。第一次寫部落格，markdown編輯器的用法還不熟悉，公式編輯會花去很多時間。所以直接用圖片代替了，希望大家能夠理解，以後我會慢慢按照標準的格式來編輯公式的。
一、基本卡爾曼濾波器（BKF）
卡爾曼在NASA埃姆斯研究中心訪問時，發現他的方法對於解決阿波羅計劃的軌道預測很有用，後來阿波羅飛船的導航電腦便使用了這種濾波器。關於這種濾波器的論文最早由Swerling(1958)發表了這種想法。
1、基本動態模型
假設1：k時刻的真實狀態是從k-1時刻的真實狀態演化而來；
假設2：演化與測量的過程由線性運算元來描述。
2、兩個基本的方程
四個狀態值的定義：

狀態轉移方程：

狀態測量方程：

基於k-1時刻狀態對k時刻狀態的估計值與真實值之間的差稱為估計誤差，該估計誤差的協方差矩陣的定義為後驗估計誤差協方差矩陣，用下式表示：

其模型拓撲結構用隱馬爾科夫鏈可以表示成圖1（維基百科:卡爾曼濾波）這樣：

圖1 卡爾曼濾波的隱馬爾可夫鏈式模型
3、兩個基本的過程
3.1 預測過程


從公式（3）*推導公式（2）：

3.2 更新過程
我們基於k-1時刻對k時刻狀態的估計是否正確，需要用與實際測量值之間的誤差來衡量，並且考慮用這個誤差來補償。所以在更新之前，我們應該計 算實際測量值與估計輸出值之間的差值及其協方差矩陣。

（3）式的協方差矩陣表示為：

下面推導公式（4）：

然後我們再進行更新的步驟。更新是指：由基於k-1時刻對k時刻狀態的估計值應當如何得到k時刻的估計值。卡爾曼的思想就是：用基於k-1時刻對k時刻狀態的估計值與預測輸出值和實際輸出值之間的差進行線性組合得到k時刻的估計值，連線這兩者的就是卡爾曼增益。這裡體現的就是反饋的思想，更新過程的第一步用下式表示：

那麼現在問題來了，如何求取這個卡爾曼增益呢？
這時候我們應該回到我們的出發點，我們希望的是濾除干擾真實狀態的噪聲，是濾波器的估計狀態與真實狀態最為接近。最為接近可以理解為k時刻的真實狀態與k時刻的估計狀態之間的誤差二範平方和最小，也就等價於協方差矩陣的跡最小。可以表示為：

在這種情況下求取的卡爾曼增益稱為最優卡爾曼增益。求取的過程就是直接上式對卡爾曼增益求一階導數。
更新過程的第二步，就是計算卡爾曼增益：

下面給出推導過程：


最優卡爾曼增益計算出來之後，我們發現在最優卡爾曼增益情況下可以對後驗誤差協方差矩陣進行簡化。第三步就是計算在最優卡爾曼增益下的後驗誤差協方差矩陣：

推導過程：

4、總結
卡爾曼在濾波器的推導過程就已經完成了，下面我們再將它們整合到一起，有個更清晰的認識：

將這些方程做成框圖：

從中可以看到，只要對初始狀態進行設定，卡爾曼濾波器就可以完成迭代了。
下面的圖是我用白巧克力亦唯心提到的勻加速的例子做了三張GIF來動態展示這個過程，以示其效果。

圖2 理論值

圖3 測量值

圖4 卡爾曼濾波結果
該卡爾曼濾波器是從第10個時間步才開始測量的，之前保持為0。
二、擴充套件卡爾曼濾波器（EKF）
EKF只是在KF的基礎之上改變了狀態轉移函式和測量函式，從而將卡爾曼濾波器的線性運算元變為非線性運算元。下面只給出與KF不同的地方的公式推導。其他部分參考KF的推導。


可以看出，EKF依然是在KF的框架內進行的改進，所以思路與KF是完全一致的。只是與的計算方法不同。
下面給出推導過程。


到這一步，近似最優卡爾曼增益的計算就與KF的推導過程完全一致了，在此不再贅述。
只是需要注意一下的是：此處為什麼是近似最優卡爾曼增益而不是最優卡爾曼增益。這是因為計算與的時候理論上應該計算函式f和h的雅可比矩陣。但是實際操作起來非常困難，特別是對於一些複雜的非線性系統。因此往往採用泰勒展開去一階線性的部分。由於近似，得到的卡爾曼在增益也就不是最優卡爾曼增益，而是近似最優卡爾曼增益。這就直接導致了EKF在高度非線性系統下效能銳減的必然結果。而且系統初始狀態估計錯誤或者說建模不正確，EKF也會迅速發散。所以在第三部分介紹的UKF則避免了求取函式的雅可比矩陣，從而提高了濾波器的效能和魯棒性。
三、無向卡爾曼濾波器（UKF）
UKF依然沒有脫離KF的框架。只不過對下一時刻狀態的預測方法變成了sigma點集的擴充與非線性對映。這樣做有兩個優點：1、避免了複雜非線性函式雅可比矩陣的複雜運算；2、保證了非線性系統的普遍適應性。此外，由於高斯分佈sigma點集的擴充套件，使高斯分佈的噪聲得到抑制。
預測過程：


更新過程：


在準確建模的前提下，KF，EKF和UKF都有不錯的表現。但是對於很多複雜的系統而言，建模就是一個複雜的問題。如果模型引數沒辦法準確估計，那麼卡爾曼濾波器的應用就會受到限制。在不知道模型引數的情況下，可以通過蒙特卡洛取樣，特別是粒子濾波的方法來對引數進行估計。這也是筆者繼續研究的方向。以上內容，僅供參考。限於水平，難免紕漏。如有不妥之處，還請告知。