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

2、不夠長， 這種積累誤差很有可能使估計誤差方差陣失去非負定性甚至失去對稱性，使濾波增益矩陣逐漸失去合適的加權(quán)作用而導(dǎo)致發(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 也不能確切建立。 如果所建立的模型與實際模

3、型不符可能回引起濾波發(fā)散。 自適應(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)濾波的基

4、本途徑就是根據(jù)量測數(shù)據(jù)估計出輸出函數(shù)序列Ck,再由Ck推算出最佳增益矩陣 K,使得增益矩陣 K 不斷地與實際量測數(shù)據(jù)CJ相適應(yīng)。.Sage-Husa 自適應(yīng)卡爾曼濾波是在利用量測數(shù)據(jù)進行遞推濾波時，通過時變噪聲估計估值器，實時估計和修正系統(tǒng)噪聲和量測噪聲的統(tǒng)計特性，從而達到降低系統(tǒng)模型誤差、抑制濾波發(fā)散提高哦濾波精度的目的。Xk=k,jXjWkZk=HkXkVkTE(Wk)=qk，E(WkWk)=Q*kjE(Vk)”E(VkVT)=Rk、*jE(wkVT)=0Sage-Husa 自適應(yīng)卡爾曼濾波算法可描述為?=?k,k1KkZkXk,k=.k,k尺，很k=Zk-Hk?k,k4-?Kk=Pk,k

5、#THkPk“HT昆廣R,k=：k,k4Pk,k,kQk4R=(I-KkHJPk”.其中，r?、是、？卜和由以下時變噪聲統(tǒng)計估值器獲得：傕1=(1-dk)?dk(Zk1-2Xkik)Rk由=(1dk)R+dk/ZHk+PkkHM)bki=(1-dk)&dk(Xkiki,k尺)1-b式中：dk=k4，0bk核心就在于近似，給出非線性估計方法的不同就在于其近似處理的思想和實現(xiàn)手段不同。近似的本質(zhì)就是對難以計算的非線性模型施加某種數(shù)學(xué)變換，變換成線性模型，然后用 Bayes 估計原理進行估計。進一步說，非線性變換到線性變換主要有兩種實現(xiàn)手段,一種是 Taylor 多項式展開,一種是插值多項式

6、展開。Bucy 和 Y.Sunahara 等人致力于研究將經(jīng)典卡爾曼濾波理論擴展到非線性隨機系統(tǒng)濾波估計中，提出了離散非線性隨機系統(tǒng)擴展卡爾曼濾波(Extendedkalmanfilter,以下簡稱 EKF)。EKF 是傳統(tǒng)非線性估計中的代表，其基本思想是將非線性狀態(tài)函數(shù)和量測函數(shù)進行局部線性化，即進行一階 Taylor 多項式展開，然后應(yīng)用線性系統(tǒng) Kalman 濾波公式。非線性離散系統(tǒng)狀態(tài)方程和觀測方程的一般形式如下所示Xk1=f(Xk,Uk)-kWk/、1-1Zk=g(Xk,Uk)Vk式中：UkWRr為輸入向量；WkwRp和 VkwRq均為高斯白噪聲，且互不相關(guān)，其統(tǒng)計特性為：,E(Wk

7、)=0,Cov(Wk,Wj)=Qgkj其中，,E(vJ=0,Cov(Vk,Vj)=RECov(wk,Vj)=0式中，Qk為過程激勵噪聲協(xié)方差矩陣，R 為觀測噪聲協(xié)方差矩陣。f(XkUk)是一個非線性狀態(tài)轉(zhuǎn)換函數(shù)，g(Xy,Uk)是一個非線性量測函數(shù)。每一個時刻點，根據(jù)一階泰勒展開將 f(Xy,Uk)，g(Xk,Uk)線性化，即將非線性狀態(tài)函數(shù)f(;)和非線性量測函數(shù)g(;)圍繞濾波值展開泰勒級數(shù)，并略去二階以上項，得到f(Xk,Uk)定f(Xk,Uk)名(Xk-Xk)1-2CXk八，勾(xk,uk)-/cg(Xk,Uk)為g(XLUk)+xkJ?k(XkXk)1-3%定義.=叼叼華,品=四2|

8、4 國，根據(jù)式(1-1)、式(1-2)OXkCXk和式(1-3)可以得到非線性系統(tǒng)線性化后只與狀態(tài)變量有關(guān)的表達式，如下Xk書*&kXk+f(Xk,Uk)dkXk+kWk.)1-4jk之Hxk+g(K,Uk)Rxk+vk式 1-4 中，注意到 f（浦山）一本kXk并非 Xk的函數(shù)，g（兄,u/用人并非 Xk的函數(shù),根據(jù) 1-4 近似結(jié)果,應(yīng)用上節(jié)的 Kalman 濾波器計算可以得到 EKF 迭代算法:定義格k=旦 8_!0,用=期人必）期人必）xk總，可得 cXkcXk濾波方程初始條件 X0=E（X0）,P0=var（X0）狀態(tài)先驗估計值 Xk,k=f（Xk,uk）誤差協(xié)方差先驗估計值

9、Pk,k1=k,kPkk,kk,kJQkJ-k,kJ增益矩陣 Kk=Pk,k,HTHkPk,kHTRk狀態(tài)后驗估計值 XkuQ,kKkZk-g（?k,Uk）誤差協(xié)方差后驗估計值 R=（I-KkHk）Pk,k無跡卡爾曼濾波（UKFEKF 是一種次優(yōu)非線性高斯濾波器，它采用對非線性函數(shù)進行線性化近似的方法，來計算狀態(tài)分布經(jīng)非線性函數(shù)傳遞之后的特性。盡管 EKF 得到了廣泛的應(yīng)用，但它依然存在自身無法克服的理論局限性： 要求非線性系統(tǒng)狀態(tài)函數(shù)和量測函數(shù)必須是連續(xù)可微的，這限制了 EKF 的應(yīng)用范圍；對非線性函數(shù)的一階線性化近似精度偏低，特別地，當(dāng)系統(tǒng)具有強非線性時，EKF 估計精度嚴重下降，甚至發(fā)散

10、；需要計算非線性函數(shù)的雅克比矩陣，容易造成 EKF 數(shù)值穩(wěn)定性差和出現(xiàn)計算發(fā)散。為了克服上述 EKF 的缺陷，能夠以較高的精度和較快的計算速度處理非線性高斯系統(tǒng)的濾波問題，Julier 等人根據(jù)確定性采樣的基本思路，基于 Unscented 變換（UT）提出了 Unscented 卡爾曼濾波（UKF。與 EKF 類似，UKF 仍繼承了卡爾曼濾波器的基本結(jié)構(gòu)，不同之處在于 UKF用 Unscented 變換取代了 EKF 中的局部線性化。UKF 仍假設(shè)隨機系統(tǒng)的狀態(tài)必須服從高斯分布，但取消了對系統(tǒng)模型的限制條件，也就是說，不要求系統(tǒng)是近似線性的，同時，UKF需要計算雅克比矩陣，因此不要求狀態(tài)函數(shù)

11、和量測函數(shù)必須是連續(xù)可微的，它甚至可以應(yīng)用于不連續(xù)系統(tǒng)。可以證明：不論系統(tǒng)非線性程度如何，UT 變換理論上至少能以三階泰勒精度逼近任何非線性高斯系統(tǒng)狀態(tài)的后驗均值和協(xié)方差，因此 UKF的理論估計精度優(yōu)于 EKEUK 祛首先要構(gòu)造 Sigma 散點集,設(shè)狀態(tài)向量為 n 維，念為時亥 Uk-1 的狀態(tài)向量估計值，Pj 為該時刻狀態(tài)向量的協(xié)方差矩陣,2n+1 維的 Sigma 點集可以表示為：x0,kxkAdx,k=/+(v(n+QP,k)i,i=1,2,.,nxin,k=xk(.(n)p,k)i對應(yīng)于與的一階二階權(quán)系數(shù)為“僅/(n+笛i=0W、1/2(n+K)i#0c九/(n+入)+1+B口2i=

12、0W=J/2(n+.)i#0其中，九=o(2(n+K)-n參數(shù) a 決定第 i 個 Sigma 點在狀態(tài)均值蹂,周圍的擴展空間，是取值區(qū)間為0.0001,1的常數(shù)；K為冗余量；P 為與狀態(tài)向量的先驗分布相關(guān)的參數(shù)，對高斯分布，0=2 為最優(yōu)。由時亥 ijk-1 的功和 Pk來計算 Sigma 點集由 Xi,k/y可得狀態(tài)向量預(yù)測值其/k及誤差協(xié)方差陣Pk/k4xi,k/k1=fk4(xi,k4)qkJLLXk/kl=WiXi,k/k4=i0i=0LcTR/ki=Wi(Xi,k/k1一？k/kj)(Xi,k/k.一尺/k.)-Qk4i=0同理，利用 XU 和 PU 按照前面的采樣策略來計算 Si

13、gma 點集(i=0,1.,L),通過非線性函數(shù) fy()+qk二傳播為Xi,k/k4?i=0,1,.,LWifk4(xi,k4)qk-1Xi,k_!(i=0,1.,L),通過非線性量測函數(shù) hk()十 rk傳播為？i,k/k，由,k/k可得輸出預(yù)測值Z/k及自協(xié)方差陣吟和互協(xié)方差陣 P 混i,k/kJ.=hk(xi,k/k1)rkLL?k/kJ.=Wii,k/kd=u，Wimhk(xi,k/k1)rki0iz0LP4=Wi(i,k/k一？k/k)(i,k/k一zk/k)Rkiz0LPx之=、Wj(Xi,k/k二-xk/k)(i,k/k-Zk/kJ)i=0在獲得新白量測后 Zk,進行濾波量測更

14、新泣=?k/k+Kk(Zk-Zk/k)1Kk=皿叱R=Pk/kKkPzkKT中心差分卡爾曼濾波器(CDKFIto 等人從數(shù)值積分的觀點出發(fā)提出了一種次優(yōu)高斯濾波器：中心差分濾波器(CentralDifferenceFilter,CDF)。 CDF用多項式插值方法來計算多維積分， 其計算簡單，易于實現(xiàn)。幾乎同時，M.Norgaard 等人也使用 Stirling 多項式插值公式來近似計算非線性函數(shù)的多維積分，得到了分開差分濾波器(DividedDifferenceFilter,DDF。武元新等人通過理論分析指出，DD 林口 CDF是基于函數(shù)擬合的思想來實現(xiàn)的，即都是使用一個函數(shù)序列近似被積函數(shù)，且函數(shù)序列中的每個函數(shù)積分都有解析解，此時近似函數(shù)的積分就可以看作是對積分的近似。由于 DDF 和CDFfc 本質(zhì)上是一致的，有異曲同工之妙，因此 R.V.Merwe 等人統(tǒng)一將它們稱為中心差分卡爾曼濾波器(CentralDifferenceKalmanFilter,CDKF),并給出了 CDKF 勺濾波遞推公式。平方根SPKF算法由于 CDK 所采用的多項式