1、自適應(yīng)卡爾曼濾波卡爾曼濾波發(fā)散的原因如果卡爾曼濾波是穩(wěn)定的,隨著濾波的推進,卡爾曼濾波估計的精度應(yīng)該越來越高,濾波誤差方差陣也應(yīng)趨于穩(wěn)定值或有界值.但在實際應(yīng)用中,隨著量測值數(shù)目的增加,由于估計誤差的均值和估計誤差協(xié)方差可能越來越大,使濾波逐漸失去準確估計的作用,這種現(xiàn)象稱為卡爾曼濾波發(fā)散.引起濾波器發(fā)散的主要原因有兩點：(1)描述系統(tǒng)動力學特性的數(shù)學模型和噪聲估計模型不準確,不能直接真實地反映物理過程,使得模型與獲得的量測值不匹配而導致濾波發(fā)散.這種由于模型建立過于粗糙或失真所引起的發(fā)散稱為濾波發(fā)散.(2)由于卡爾曼濾波是遞推過程,隨著濾波步數(shù)的增加,舍人誤差將逐漸積累.如果計算機字長不夠長

2、,這種積累誤差很有可能使估計誤差方差陣失去非負定性甚至失去對稱性,使濾波增益矩陣逐漸失去適宜的加權(quán)作用而導致發(fā)散.這種由于計算舍入誤差所引起的發(fā)散稱為計算發(fā)散.針對上述卡爾曼濾波發(fā)散的原因,目前已經(jīng)出現(xiàn)了幾種有效抑制濾波發(fā)散的方法,常用的有衰減記憶濾波、限定記憶濾波、擴充狀態(tài)濾波、有限下界濾波、平方根濾波、和自適應(yīng)濾波等.這些方法本質(zhì)上都是以犧牲濾波器的最優(yōu)性為代價來抑制濾波發(fā)散,也就是說,多數(shù)都是次優(yōu)濾波方法.自適應(yīng)濾波在很多實際系統(tǒng)中,系統(tǒng)過程噪聲方差矩陣Q和量測誤差方差陣R事先是不知道的,有時甚至連狀態(tài)轉(zhuǎn)移矩陣或量測矩陣H也不能確切建立.如果所建立的模型與實際模型不符可能回引起濾波發(fā)散.

3、自適應(yīng)濾波就是這樣一種具有抑制濾波發(fā)散作用的濾波方法.在濾波過程中,自適應(yīng)濾波一方面利用量測值修正預(yù)測值,同時也對未知的或不確切的系統(tǒng)模型參數(shù)和噪聲統(tǒng)計參數(shù)進行估計修正.自適應(yīng)濾波的方法很多,包括貝葉斯法、極大似然法、相關(guān)法與協(xié)方差匹配法,其中最根本也是最重要的是相關(guān)法,而相關(guān)法可分為輸出相關(guān)法和新息相關(guān)法.在這里只討論系統(tǒng)模型參數(shù),而噪聲統(tǒng)計參數(shù)Q和R未知情況下的自適應(yīng)濾波.由于Q和R等參數(shù)最終是通過增益矩陣K影響濾波值的,因此進行自適應(yīng)濾波時,也可以不去估計Q和R等參數(shù)而直接根據(jù)量測數(shù)據(jù)調(diào)整K就可以了.輸出相關(guān)法自適應(yīng)濾波的根本途徑就是根據(jù)量測數(shù)據(jù)估計出輸出函數(shù)序列C0,再由C訃推算出最正

4、確增益矩陣K,使得增益矩陣K不斷地與實際量測數(shù)據(jù)Ck相適應(yīng).Sage-Husa自適應(yīng)卡爾曼濾波是在利用量測數(shù)據(jù)進行遞推濾波時,通過時變噪聲估計估值器,實時估計和修正系統(tǒng)噪聲和量測噪聲的統(tǒng)計特性,從而到達降低系統(tǒng)模型誤差、抑制濾波發(fā)散提升哦濾波精度的目的.xkk,k1xk1wkZkHkXkVkE(wQqk,E(WkW：)Qk用E(vJ譏2皿)Rk©E(WkV【)0Sage-Husa自適應(yīng)卡爾曼濾波算法可描述為Xkxk,k1"kXk,k1k,k1?k1<ik1ZkZkHk&k1?KkPk.klH：HkPk,klHTRk1Pkkk,kR1k,k1Qk1pkKkHkJ

5、R,k1其中？、Rk、qk和Qk由以下時變噪聲統(tǒng)計估值器獲得：,?i(1dk)?dk(ZkiHki?kilk)Rk1(1dk)Rdk(ZklZk1Hk1Fk1,kHkl)(?k1dck)q>kdk(xk1k1,kxk)(1dQQkdk(KkiZkiZkriK：1pkiki,kpk內(nèi),W1b式中：dk百,0bl為遺忘因子.1bk1如果系統(tǒng)狀態(tài)變量的維數(shù)比擬高,而Sage-Husa自適應(yīng)濾波算法中又增加了對系統(tǒng)噪聲統(tǒng)計特性的計算,計算量將大大增加,實時性也將難以得到保證.除此之外,對于階次較高的系統(tǒng),Sage-Husa自適應(yīng)濾波算法中Rk和Qk的在線估計有時會由于計算發(fā)散失去半正定性和正定性

6、而出現(xiàn)濾波發(fā)散現(xiàn)象,此時Sage-Husa自適應(yīng)濾波算法的穩(wěn)定性和收斂性不能完全保證.基于極大似然準那么的自適應(yīng)卡爾曼濾波,通過系統(tǒng)狀態(tài)方差陣和量測噪聲方差陣實時估計系統(tǒng)噪聲統(tǒng)計特性的變化,以保證濾波器更好地適應(yīng)這種變化.極大似然估計從系統(tǒng)量測量出現(xiàn)概率最大的角度估計,其特點是不僅考慮新息的變化,而且考慮新息協(xié)方差矩陣Cvk的變化.它的量測噪聲協(xié)方差矩陣F?和系統(tǒng)噪聲協(xié)方差矩陣3為:RkCvkHkpk,k1Hkk3kXXik,klP1k,k1*IxkxkA,k1,Kkvk1kTCvkViViNikN1式中：VkZkZki,N為平滑窗口的寬度.擴展卡爾曼濾波最初提出的卡爾曼濾波根本理論只適用于狀

7、態(tài)方程和量測方程均為線性的隨機線性高斯系統(tǒng).但是大局部系統(tǒng)是非線性的,其中還有許多事強非線性的.非線性估計的核心就在于近似,給出非線性估計方法的不同就在于其近似處理的思想和實現(xiàn)手段不同.近似的本質(zhì)就是對難以計算的非線性模型施加某種數(shù)學變換,變換成線性模型,然后用Bayes估計原理進行估計.進一步說,非線性變換到線性變換主要有兩種實現(xiàn)手段,一種是Taylor多項式展開,一種是插值多項式展開.Bucy和等人致力于研究將經(jīng)典卡爾曼濾波理論擴展到非線性隨機系統(tǒng)濾波估計中,提出了離散非線性隨機系統(tǒng)擴展卡爾曼濾波Extendedkalmanfilter,以下簡稱EKF.EKF是傳統(tǒng)非線性估計中的代表,其根

8、本思想是將非線性狀態(tài)函數(shù)和量測函數(shù)進行局部線性化,即進行一階Taylor多項式展開,然后應(yīng)用線性系統(tǒng)Kalman濾波公式.非線性離散系統(tǒng)狀態(tài)方程和觀測方程的一般形式如下所示Xkif(Xk,Uk)kWkZkg(Xk5Uk)Vk式中：UkR為輸入向量；WkRP和VkRq均為高斯白噪聲,且互不相關(guān),其統(tǒng)計特性為：E(Wk)0,Cov(Wk,Wj)Qkkj其中,E(Vk)0,Cov(Vk,Vj)RkkjCov(wk,Vj)0式中,Qk為過程鼓勵噪聲協(xié)方差矩陣,Rk為觀測噪聲協(xié)方差矩陣.f(Xz,Uz)是一個非線性狀態(tài)轉(zhuǎn)換函數(shù),g(Xz,Liz)是一個非線性量測函數(shù).每一個時刻點,根據(jù)一階泰勒展開將f儀

9、小,11內(nèi)),g(Xki,uz)線性化,即將非線性狀態(tài)函數(shù)f(；)和非線性量測函數(shù)g(；)圍繞濾波值展開泰勒級數(shù),并略去二階以上項5得到f(Xk,Uk)g(Xk,Uk)定義4f(Xk,Uk)3WXk丘)Xkg(Xk,Uk)LU"Xk乂(XkXk)Xkf(Xk,Uk),HkXkXkXk1-39小'小)XkXk,根據(jù)式(11)、式(12)Xk和式(1-3)可以得到非線性系統(tǒng)線性化后只與狀態(tài)變量有關(guān)的表達式,如下Xk1?kXkf(?k,Uk)kxkkwk14ZkHkXkg(Xk,Uk)F?kXkVk式14中,注意到f(Xk,Uk)?kXk并非Xk的函數(shù),g(Xk,Uk)H?kXk并

10、非Xk的函數(shù),根據(jù)1-4近似結(jié)果,應(yīng)用上節(jié)的Kalman濾波器計算可以得到EKF迭代算法:定義？k曲MXk4g(Xk,Uk)XkXkXk,可得濾波方程初始條件X.E(Xo),P.var(Xo)狀態(tài)先驗估計值Xk?k1f(Xk誤差協(xié)方差先驗估計值pkfk1k,kR1k5k1k,k10k1k,k1增益矩陣KkPk,kiH：HkPk,kiHT狀態(tài)后驗估計值Xk?k,k1Kk1水g(xkiuk)誤差協(xié)方差后驗估計值R(IKkHk)Pk,ki無跡卡爾曼濾波(UKFEKF是一種次優(yōu)非線性高斯濾波器,它采用對非線性函數(shù)進行線性化近似的方法,來計算狀態(tài)分布經(jīng)非線性函數(shù)傳遞之后的特性.盡管EKF得到了廣泛的應(yīng)用

11、,但它依然存在自身無法克服的理論局限性：要求非線性系統(tǒng)狀態(tài)函數(shù)和量測函數(shù)必須是連續(xù)可微的,這限制了EKF的應(yīng)用范圍；對非線性函數(shù)的一階線性化近似精度偏低,特別地,當系統(tǒng)具有強非線性時,EKF估計精度嚴重下降,甚至發(fā)散；需要計算非線性函數(shù)的雅克比矩陣,容易造成EKF數(shù)值穩(wěn)定性差和出現(xiàn)計算發(fā)散.為了克服上述EKF的缺陷,能夠以較高的精度和較快的計算速度處理非線性高斯系統(tǒng)的濾波問題,Julier等人根據(jù)確定性采樣的根本思路,基于Unscented變換(UT提出了Unscented卡爾曼濾波(UKF.與EKF類似,UKF仍繼承了卡爾曼濾波器的根本結(jié)構(gòu),不同之處在于UKF用Unseented變換取代了E

12、KF中的局部線性化.UKF仍假設(shè)隨機系統(tǒng)的狀態(tài)必須服從高斯分布,但取消了對系統(tǒng)模型的限制條件,也就是說,不要求系統(tǒng)是近似線性的,同時,UKF不需要計算雅克比矩陣,因此不要求狀態(tài)函數(shù)和量測函數(shù)必須是連續(xù)可微的,它甚至可以應(yīng)用于不連續(xù)系統(tǒng).可以證實：不管系統(tǒng)非線性程度如何,UT變換理論上至少能以三階泰勒精度逼近任何非線性高斯系統(tǒng)狀態(tài)的后驗均值和協(xié)方差,因此UKF的理論估計精度優(yōu)于EKFUK法首先要構(gòu)造Sigma散點集,設(shè)狀態(tài)向量為n維,5?內(nèi)為時刻k1的狀態(tài)向量估計值,Pki為該時刻狀態(tài)向量的協(xié)方差矩陣,2n+1維的Sigma點集可以表示為：?1x0,k1,X)k17i(,(n)R,ki>,

13、i=1,25.,nxnk1Xki(.(n)R,ki>i111r對應(yīng)于i的一階二階權(quán)系數(shù)為m/(n)i0M.1/2(n)iO/(n)12-iow01/2(n)io其中,(n)n參數(shù)決定第i個Sigma點在狀態(tài)均值Xz周圍的擴展空間,是取值區(qū)間為,1的常數(shù)；為冗余量；為與狀態(tài)向量的先驗分布相關(guān)的參數(shù),對圖斯分布,=2為最優(yōu).由時刻k-1的Xki和Pz來計算Sigma點集xU0,1_L),通過非線性函數(shù)fk1()qk1傳播為xi,k/k1,由Xi,k/k1可得狀態(tài)向量預(yù)測值Xk/k1及誤差協(xié)方差陣xi,k/k1fk1(Rk!)qk1Lxk/k1wixi,k/k1iOWi«k1(/k1

14、)iOqk1Lcwi(xi,k/k1iO?k/k1)(xi,k/k1xk/kVQk1同理,利用XkI和RI根據(jù)前面的采樣策略來計算Sigma點集Xi,k1(i0,1,L),通過非線性量測函數(shù)hk()斤傳播為第你I,由Wki可得輸出預(yù)測值Z?0kl及自協(xié)方差陣i,k/k1hk(wk1)Lz?iz/iz1wm0KlWjj,0k1iOLPzkWic(i,k/k1iOLwiai,k/k1PxkzkiO在獲得新的量測后Zk,x?kx?k/k1Kk<2kz9IVkVKkPxkZkPzJPkPk/k1KkPzkKkPZk和互協(xié)方差陣PxkZkZ?k/k1)(i,k/k1Z?k/k1)Rk/k1Hi,k

15、/k1z?k/kl)進行濾波量測更新中央差分卡爾曼濾波器(CDKF)Ito等人從數(shù)值積分的觀點出發(fā)提出了一種次優(yōu)高斯濾波器：中央差分濾波器(CentralDiffereneeFilter,CDF).CDF使用多項式插值方法來計算多維積分,其計算簡單,易于實現(xiàn).幾乎同時,等人也使用Stirling多項式插值公式來近似計算非線性函數(shù)的多維積分,得到了分開差分濾波器(DividedDiffereneeFilter,DDF).武元新等人通過理論分析指出,DDF和CDF都是基于函數(shù)擬合的思想來實現(xiàn)的,即都是使用一個函數(shù)序列近似被積函數(shù),且函數(shù)序列中的每個函數(shù)積分都有解析解,此時近似函數(shù)的積分就可以看作是對積分的近似.由于DDF和CDF在本質(zhì)上是一致的,有異曲同工之妙,因此等人統(tǒng)一將它們稱為中央差分卡爾曼濾波器(CentralDiffereneeKalmanFi