卡爾曼濾波（KF）與擴展卡爾曼濾波（EKF）講解與推導

卡爾曼濾波

• 系統方程：

                                                                       

       爲狀態轉移矩陣，爲控制矩陣，爲控制向量，爲系統白噪聲，對應狀態的每一個分量，均值爲0，方差爲。爲觀測白噪聲，同樣方差爲，服從高斯分佈。

• 基本假設

      下一個狀態是前一個狀態的線性函數，觀測量是狀態量的線性函數。

• 預測

                                                                           

• 更新

                                                                   

• 符號意義

        是卡爾曼增益，是狀態預測值，是預測狀態協方差值，是狀態更新值（結果），是更新狀態協方差值。

KF推導過程：

       卡爾曼濾波通過迭代遞推實現狀態值與狀態協方差值的更新與計算，每次輸入爲上一時刻計算得到的狀態值與協方差值。

       首先根據系統方程可以計算得到預測方程：

                                                                           

       根據協方差公式得到：

                                                                           

       根據定理：如果，則統一量崗後，，均值與方差滿足如下公式：

                                                                          

        預測狀態量統一量綱到觀測變量，加上原觀測量得到兩組高斯分佈：,。

        融合兩組高斯變量，爲了表示方便令，將得到的結果視爲真值得到：

                                                                

        去掉觀測矩陣變量H，令得到：

                                                                       

        推導完畢，簡單的理解就是用狀態方程進行預測，用觀測變量進行後驗概率校正，將兩種高斯分佈融合增加狀態可信度。從卡爾曼增益可以看出，如果狀態遞推的方差較大，即使用狀態遞推不準確度大，則狀態更新值中觀測量的比重較大；反之如果觀測量的方差較大，觀測量不準確，狀態更新值中狀態量的比重較大。狀態遞推與觀測值使用更準的，即兩種傳感器使用更準的完成狀態更新。

卡爾曼濾波假設狀態方程與觀測方程的線性，當狀態與觀測方程不滿足線性條件時需要將其線性化，即EKF。 

KF的三大假設：

應用KF具有侷限性，需要滿足三大假設：

1 測量噪聲、系統噪聲以及初始值服從高斯分佈且互不相關

2 系統是線性的，觀測方程與狀態方程均要求爲線性方程

3 滿足貝葉斯濾波的馬爾可夫假設，假設過去以及未來的數據都是獨立的。也就是t時刻的狀態可以只由t-1時刻的狀態推出

然而實際應用中很難滿足系統是線性的，需要用到擴展卡爾曼濾波，將非線性系統轉化爲線性系統

EKF推導過程：

• 非線性系統方程：

                                                                       

對狀態轉移函數進行一階泰勒展開得到：

                                                     

完成線性化後，系統爲線性，狀態方程與觀測方程同KF中的方程，線性化後EKF5個方程推導過程上述KF所示。