1、第二章第二章 幾種最優(yōu)估計和卡爾曼濾波原理幾種最優(yōu)估計和卡爾曼濾波原理 2.1 2.1 卡爾曼濾波與最優(yōu)估計卡爾曼濾波與最優(yōu)估計 2.2 2.2 卡爾曼濾波方程卡爾曼濾波方程 2.3 2.3 連續(xù)系統(tǒng)的卡爾曼濾波方程連續(xù)系統(tǒng)的卡爾曼濾波方程2.4 2.4 連續(xù)連續(xù)離散系統(tǒng)卡爾曼濾波方程離散系統(tǒng)卡爾曼濾波方程 2.5 2.5 卡爾曼濾波在組合導航中的應(yīng)用方式卡爾曼濾波在組合導航中的應(yīng)用方式 2.6 2.6 基于統(tǒng)計的非線性濾波方法基于統(tǒng)計的非線性濾波方法 卡爾曼濾波的本質(zhì)是什么？卡爾曼濾波的本質(zhì)是什么？濾波器濾波器從含有噪聲的量測信息中提取出有從含有噪聲的量測信息中提取出有 用的信息用的信息卡爾

2、曼濾波是一種數(shù)學濾波器（數(shù)據(jù)處理方法）卡爾曼濾波是一種數(shù)學濾波器（數(shù)據(jù)處理方法）濾波的基本步驟是什么？濾波的基本步驟是什么？第一步第一步，建立系統(tǒng)模型，建立系統(tǒng)模型（1）狀態(tài)量的選取及狀態(tài)模型建立）狀態(tài)量的選取及狀態(tài)模型建立系統(tǒng)要求系統(tǒng)要求 濾波要求濾波要求 實際運算量的要求實際運算量的要求（2）量測量的選取及狀態(tài)模型建立）量測量的選取及狀態(tài)模型建立第二步第二步，可控性和可觀性分析，可控性和可觀性分析第三步第三步，確定系統(tǒng)噪聲統(tǒng)計特性，確定系統(tǒng)噪聲統(tǒng)計特性第四第四步步，選擇適當?shù)臑V波方法，選擇適當?shù)臑V波方法卡爾曼濾波的效果依賴于系統(tǒng)模型和噪聲的統(tǒng)計特性?？柭鼮V波的效果依賴于系統(tǒng)模型和噪聲的統(tǒng)

3、計特性。o 例例1、一個小車在地面靜止不動，車上裝有一個精度為、一個小車在地面靜止不動，車上裝有一個精度為1m的的GPS接收機，請利用卡爾曼濾波技術(shù)估計小車的接收機，請利用卡爾曼濾波技術(shù)估計小車的位置。位置。o 例例3、小車做勻速直線運動，量測信息為某個地面站測、小車做勻速直線運動，量測信息為某個地面站測得的到小車的相對距離，請利用卡爾曼濾波技術(shù)估計小得的到小車的相對距離，請利用卡爾曼濾波技術(shù)估計小車的位置。車的位置。o 例例2、小車做勻速直線運動，車上裝有一個精度為、小車做勻速直線運動，車上裝有一個精度為1m的的GPS接收機，請利用卡爾曼濾波技術(shù)估計小車的位置。接收機，請利用卡爾曼濾波技術(shù)估

4、計小車的位置。o 例例4、小車做勻速圓周運動，量測信息為某個地面站測得、小車做勻速圓周運動，量測信息為某個地面站測得的到小車的相對距離，請利用卡爾曼濾波技術(shù)估計小車的到小車的相對距離，請利用卡爾曼濾波技術(shù)估計小車的位置。的位置。2.6 基于統(tǒng)計的非線性濾波方法基于統(tǒng)計的非線性濾波方法非線性系統(tǒng)濾波的難點是什么？非線性系統(tǒng)濾波的難點是什么？ o狀態(tài)一步預測方程狀態(tài)一步預測方程o狀態(tài)估值計算方程狀態(tài)估值計算方程o濾波增益方程濾波增益方程o一步預測均方差方程一步預測均方差方程o估計均方差方程估計均方差方程非線性系統(tǒng)濾波的難點在于預測，如何計算狀態(tài)變量經(jīng)過非線性系統(tǒng)濾波的難點在于預測，如何計算狀態(tài)變量

5、經(jīng)過非線性變換以后的均值和方差。非線性變換以后的均值和方差。TkXfXXXkknkkk1,111/1/1, k kk kkkkXXKZh Xk/1/11k kk kTTkkkkkKPHH PHR/11,1,11k kkTk kk kkPPQ/1()()k kTTkkkkkkkkPIK HPIK HK R K EKF是通過將非線性函數(shù)在濾波值附近進行泰勒級數(shù)展是通過將非線性函數(shù)在濾波值附近進行泰勒級數(shù)展開并忽略二階以上項進行線性化，因此其估計值的均值和開并忽略二階以上項進行線性化，因此其估計值的均值和方差只能精確到一階。方差只能精確到一階。 EKF的不足：的不足： 必須求非線性函數(shù)的必須求非線性

6、函數(shù)的Jacobi矩陣，對于模型復雜的系統(tǒng)，矩陣，對于模型復雜的系統(tǒng)，比較復雜且容易出錯比較復雜且容易出錯； 引入引入線性化誤差，對非線性強度高的系統(tǒng)，容易導致濾波線性化誤差，對非線性強度高的系統(tǒng)，容易導致濾波效果下降。效果下降。 20世紀世紀90年代，出現(xiàn)了不是利用線性化逼近非線性函數(shù)，年代，出現(xiàn)了不是利用線性化逼近非線性函數(shù)，而是而是用樣本加權(quán)求和來逼近隨機分布的方法，統(tǒng)稱為用樣本加權(quán)求和來逼近隨機分布的方法，統(tǒng)稱為sigma點卡爾曼濾波方法點卡爾曼濾波方法，其特點是：，其特點是： 1、不需對非線性函數(shù)進行求導、不需對非線性函數(shù)進行求導2、由于考慮了概率傳播問題，因此對均值和方差的估計精、

7、由于考慮了概率傳播問題，因此對均值和方差的估計精度提高到至少二階度提高到至少二階這類方法包括：這類方法包括：UKF，CKF，PF等等UKF濾波方法濾波方法假設(shè)一個離散非線性系統(tǒng)假設(shè)一個離散非線性系統(tǒng) kkkkk ,u,xFx1kkkkk ,u,xHzxk系統(tǒng)狀態(tài)向量系統(tǒng)狀態(tài)向量 uk輸入控制向量輸入控制向量 k觀測向量觀測向量 k量測噪聲向量量測噪聲向量 在在xk附近選取一系列采樣點附近選取一系列采樣點，這些采樣點通過該非線性這些采樣點通過該非線性系統(tǒng)，產(chǎn)生相應(yīng)的變換采樣點。對這些變換采樣點進行計系統(tǒng)，產(chǎn)生相應(yīng)的變換采樣點。對這些變換采樣點進行計算，便可得到預測的均值和協(xié)方差算，便可得到預測的

8、均值和協(xié)方差。主要思想：主要思想：UKF濾波方法濾波方法 Unscented變換Unscented Transformation1.初始化初始化 xEx 00 x xx xEP00T0002.計算采樣點計算采樣點Pnx Pnx x 1ki1k1ki1k1k1kni, 2 , 1 標準標準UKF濾波算法：濾波算法：UKF濾波方法濾波方法 3.時間更新時間更新1111k ,u,Fkkk | k120k | k , iniikWx Qx x WPkkk | k , iTkk | k , iik11k ,u,Hykk | kk | k11yWy 1k | k , in20iik標準標準UKF濾波算法：

9、濾波算法：UKF濾波方法濾波方法 4.4.量測更新量測更新Ry yy yWPkkk | k , iTkk | k , iniiykyk1120y yx WPk1k | k , iTk1k | k , in20iiykxkPPK1y ky kykxkky yKx x kkkkkKPKPPTky ky kkkk其中：其中：Qk和和Rk分別為系統(tǒng)和量測噪聲協(xié)方差分別為系統(tǒng)和量測噪聲協(xié)方差 標準標準UKF濾波算法：濾波算法：UKF濾波方法濾波方法 UKF濾波方法濾波方法 一種新的一種新的（UnscentedUnscented卡爾曼濾波，卡爾曼濾波，UKFUKF）。）。u對于對于來說，它的來說，它的；u

10、對于對于，它的，它的; ;u系統(tǒng)或量測噪聲可為系統(tǒng)或量測噪聲可為的情形的情形; ;u它不需要計算雅可比矩陣，不需要對狀態(tài)方程和量測方程線它不需要計算雅可比矩陣，不需要對狀態(tài)方程和量測方程線 性化，因此也就性化，因此也就。優(yōu)點：優(yōu)點：PF濾波方法濾波方法 wTime19wTime10wTime1SIS(Sequential Important Sampling) Particle Filter will not work well because of degeneracy 粒子退化現(xiàn)象示例PF濾波方法濾波方法 由于使用由于使用先驗先驗進行采樣（后驗是先驗與似然的乘積），似然很尖銳或者進行采樣（

11、后驗是先驗與似然的乘積），似然很尖銳或者位于先驗的某個尾部時，將很難有足夠的粒子產(chǎn)生于兩者的重疊區(qū)域，位于先驗的某個尾部時，將很難有足夠的粒子產(chǎn)生于兩者的重疊區(qū)域，這些粒子對后驗分布的表達誤差將因此增大。這些粒子對后驗分布的表達誤差將因此增大。改進方法，是使采樣分布包含當前的觀測值：例如，假設(shè)采樣分布為高改進方法，是使采樣分布包含當前的觀測值：例如，假設(shè)采樣分布為高斯，利用卡爾曼濾波器估計采樣分布。斯，利用卡爾曼濾波器估計采樣分布。PF濾波方法濾波方法 它是用它是用UKFUKF來得到粒子濾波的重要采樣密度函數(shù)，從而克服了標準的粒來得到粒子濾波的重要采樣密度函數(shù)，從而克服了標準的粒子濾波不考慮最新量測信息和子濾波不考慮最新量測信息和UKFUKF只能應(yīng)用到噪聲為高斯分布的不足。只能應(yīng)用到噪聲為高斯分布的不足。TiiiiiiixxxxEPxEx)()(0)(0)(0)(0)(0)(0)(0),.,1,()(0Nixi1t采樣步驟采樣步驟),(),()()(: 1)(1: 0)(ikikkikikPxNyxxq)(ikx下面給出下面給出UPFUPF算法：算法：初始化：初始化： t=0 PF濾波方法濾波方法 計算權(quán)重計算權(quán)重),()