擴展卡爾曼濾波EKF

1、第三章 擴展卡爾曼濾波EKF3.1 擴展Kalman濾波原理Kalman濾波能夠在線性高斯模型的條件下，可以對目標(biāo)的狀態(tài)做出最優(yōu)的估計，得到較好的跟蹤效果。對非線性濾波問題常用的處理方法是利用線性化技巧將其轉(zhuǎn)化為一個近似的線性濾波問題。因此，可以利用非線性函數(shù)的局部性特性，將非線性模型局部化，再利用Kalman濾波算法完成濾波跟蹤。擴展Kalman濾波就是基于這樣的思想，將系統(tǒng)的非線性函數(shù)做一階Taylor展開，得到線性化的系統(tǒng)方程從而完成對目標(biāo)的濾波估計等處理。非線性系統(tǒng)離散動態(tài)方程可以表示為 （3-1-1） （3-1-2）這里為了便于數(shù)學(xué)處理，假定沒有控制量的輸入，并假定過程噪聲是均值為零

2、的高斯白噪聲，且噪聲分布矩陣是已知的。其中，觀測噪聲也是加性均值為零的高斯白噪聲。假定過程噪聲和觀測噪聲序列是彼此獨立的，并且有初始狀態(tài)估計和協(xié)方差矩陣。和線性系統(tǒng)的情況一樣，我們可以得到擴展Kalman濾波算法如下 （3-1-3） （3-1-4） （3-1-5） （3-1-6） （3-1-7）這里需要重要說明的是，狀態(tài)轉(zhuǎn)移和量測矩陣 是由和的雅克比矩陣代替的。其雅克比矩陣的求法如下： 假如狀態(tài)變量有n維，即，則對狀態(tài)方程對各維求偏導(dǎo)， （3-1-8） （3-1-9）3.2 擴展卡爾曼在一維非線性系統(tǒng)中的應(yīng)用所謂的非線性方程，就是因變量和自變量的關(guān)系不是線性的，這類方程很多，例如平方關(guān)系，對數(shù)

3、關(guān)系，指數(shù)關(guān)系，三角函數(shù)關(guān)系等等。這類方程可分為兩類，一類是多項式方程，一種是非多項式方程。為了便于說明非線性卡爾曼濾波擴展Kalman濾波的原理，我們選用一下系統(tǒng)，系統(tǒng)狀態(tài)為，它僅包含一維變量，即，系統(tǒng)狀態(tài)方程為 （3-2-1）觀測方程為 （3-2-2）其中，式（3-1-1）是包含分式，平方，三角函數(shù)在內(nèi)的嚴(yán)重非線性的方程，為過程噪聲，其均值為0，方差為，觀測方程中，觀測信號與狀態(tài)的關(guān)系也是非線性的，也是均值為0，方差為的高斯白噪聲。因此關(guān)于（3-1-1）和（3-2-2）是一個狀態(tài)和觀測都為非線性的一維系統(tǒng)。以此為通用的非線性方程的代表，接下來講述如何用擴展Kalman濾波來處理噪聲問題。第

4、一步：初始化狀態(tài)，協(xié)方差矩陣。第二步：狀態(tài)預(yù)測 （3-2-3）第三步：觀測預(yù)測 （3-2-4）第四步：一階線性化狀態(tài)方程，求解狀態(tài)轉(zhuǎn)移矩陣 （3-2-5）第五步：一階線性化觀測方程，求解觀測矩陣 （3-2-6）第六步：求協(xié)方差矩陣預(yù)測 （3-2-7）這里需要說明的是，當(dāng)噪聲驅(qū)動矩陣不存在的時候，或系統(tǒng)狀態(tài)方程中，在前沒有任何驅(qū)動矩陣，這時候，必然和狀態(tài)的維數(shù)一樣的方陣，可將式（3-2-7）直接寫為。第七步：求Kalman增益 （3-2-8）第八步：求狀態(tài)更新 （3-2-9）第九步：協(xié)方差更新 （3-2-10）以上九步為擴展卡爾曼年濾波的一個計算周期，如此循環(huán)下去就是各個時刻EKF對非線性系統(tǒng)的處理過程。