卡爾曼濾波總結——KF、EFK、UKF

1.用途

現實是咱們的處理和測量模型都是非線性的,結果就是一個不規則分佈,KF可以使用的前提就是所處理的狀態是知足高斯分佈的,爲了解決這個問題,EKF是尋找一個線性函數來近似這個非線性函數,而UKF就是去找一個與真實分佈近似的高斯分佈。算法

  • KF處理線性模型:

  • EKF 經過雅克比和偏導數近似非線性模型,可是忽略了高階導數:(強非線性系統下偏差大,另外一方面Jacobian矩陣的計算複雜)

  • UKF 經過去點的方式近似非線性模型,由於沒有用雅克比和偏導數,讓計算變得更加簡單,同時也沒有忽略高階導數項。
  • PF不必定限於高斯分佈

 

2.KF

卡爾曼濾波是一種高效率的遞歸濾波器(自迴歸濾波器), 它可以從一系列的不徹底包含噪聲的測量(英文:measurement)中,估計動態系統的狀態,然而簡單的卡爾曼濾波必須應用在符合高斯分佈的系統中。數組

百度百科是這樣說的,也就是說卡爾曼濾波第一是遞歸濾波,其次KF用於線性系統。函數

但通過研究和改進,出現了不少卡爾曼,如EKF(extended kalman filter)擴展卡爾曼,UKF(Unscented Kalman Filter)無跡卡爾曼等等。優化

而咱們就來研究EKF,而EKF的中心思想就是將非線性系統線形化後再作KF處理。spa

 

狀態方程  3d

 當咱們從最簡單的系統開始,咱們假定k時刻的系統狀態與k-1時刻有關,因而咱們能夠獲得方程:blog

 其中a爲常量,可是當系統內部有噪聲,咱們稱爲過程噪聲 ,計爲w。因此方程能夠寫成:教程

 

而咱們實際觀測的時候會出現觀測噪聲,因而咱們將觀測值計爲Z,觀測噪聲爲v,那咱們能夠把k時刻的觀測值與系統狀態值寫成方程:遞歸

因而最簡單的系統狀態方程就有啦。rem

卡爾曼濾波算法核心思想在於預測+測量反饋,它由兩部分組成,第一部分是 線性系統狀態預測方程,第二部分是 線性系統觀測方程。

這裏系統的預測時候,咱們要將經過系統狀態方程計算的預測值做爲先驗信息,以後在觀測部分在從新更新這個信息。

 

回頭說狀態的預測,咱們引入一個符號 ^在相應的變量上表示該變量是預計值。再說一個新概念:新息。一個時間序列{X(t)}裏,根據歷史數據的預測值,新息是真實值減去預測值。

注:上有波浪線表示新息,也有用字母e表示的;上面尖的是預測值;無標註爲真實值

咱們給出一個優化過的預測值,新預測值 = 上一輪預測值(先驗) + 權重 × 新息,即:

那下一個問題就是權重g是怎麼計算得出的呢?答案是間接的從噪聲裏求得的。當咱們將估計協方差叫作p,而傳感器的協方差爲r,咱們經過這兩個值計算權重。

既然得到了g,咱們下一步將先驗信息輸入到上面的兩個方程中得到後驗預測值,也就是咱們的輸出值。

那整個計算步驟和過程就應該是這樣的:

先預測後更新。具體計算過程和曲線生成能夠參考levys教程的Part7。

 

以後咱們再說更復雜一點的系統,系統地狀態方程引入了輸入量U。換句話說有一個U會時刻影響系統狀態量。

而觀測到的觀測值爲

 

 那咱們新的計算方法就須要引入這兩個新的變量,

 

 一樣是預測,更新,預測下一時刻,更新的計算方式。

 而當系統用矩陣的方式寫的狀態方程,就須要以矩陣的方式計算。相同的數學公式及計算方法。如P的計算,

又如G,

還有P的更新,

這樣的話,咱們的系統方程及計算步驟方式以下

須要注意的是咱們使用的R是對測量偏差v的協方差,當咱們在引入一個Q表明過程噪聲引發的偏差時,能夠使系統表現更好,即便Q很小,

 

 

 2.1 非線性系統之EKF

 

那若是系統是非線性的呢?咱們若是將觀測值Z計作X的非線性函數組成的,咱們的模型能夠改爲。

 

而咱們的中心思想是將其線性化,若是咱們使用函數一階導爲C來計算G和P。

 最後系統複雜到這樣的程度,

 

那咱們就引入雅科比矩陣,雅各比矩陣就是方程矩陣對每個變量的偏導數。

則在向量分析中,雅可比矩陣Jacobian matrix是該函數的全部份量( m 個)對向量 x 的全部份量( n 個)的一階偏導數組成的矩陣。

函數有 f_1,...,f_m 個份量,因而有 m 行。向量 x 有 x_1,...,x_n 個份量,因而有 n 列。

而最終,F爲f方程的雅可比,H爲h方程的雅可比。最終計算以下:

 

到這裏簡單的EKF就已經完成基本介紹啦。

 

3.UKF

 3.1 模型

z(k+1)=h(x(k+1))

 

3.2 預測

1.預測sigma point(k+1步的分佈點)

2.預測均值(Xpred)和方差(Ppred)

 

其中:

3.預測測量值

z(k+1)i=h(x(k+1)i)

從而zpred:

 

3.3更新

 1.更新卡爾曼係數

其中:

2.更新最終值:

相關文章
相關標籤/搜索