幾種卡爾曼濾波算法理論.doc

自適應(yīng)卡爾曼濾波卡爾曼濾波發(fā)散的原因如果卡爾曼濾波是穩(wěn)定的，隨著濾波的推進(jì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ù)的增加，舍入誤差將逐漸積累。如果計算機字長不夠長，這種積累誤差很有可能使估計誤差方差陣失去非負(fù)定性甚至失去對稱性，使濾波增益矩陣逐漸失去合適的加權(quán)作用而導(dǎo)致發(fā)散。這種由于計算舍入誤差所引起的發(fā)散稱為計算發(fā)散。 針對上述卡爾曼濾波發(fā)散的原因，目前已經(jīng)出現(xiàn)了幾種有效抑制濾波發(fā)散的方法，常用的有衰減記憶濾波、限定記憶濾波、擴(kuò)充狀態(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ā)散。自適應(yīng)濾波就是這樣一種具有抑制濾波發(fā)散作用的濾波方法。在濾波過程中，自適應(yīng)濾波一方面利用量測值修正預(yù)測值，同時也對未知的或不確切的系統(tǒng)模型參數(shù)和噪聲統(tǒng)計參數(shù)進(jìn)行估計修正。自適應(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影響濾波值的，因此進(jìn)行自適應(yīng)濾波時，也可以不去估計Q和R等參數(shù)而直接根據(jù)量測數(shù)據(jù)調(diào)整K就可以了。輸出相關(guān)法自適應(yīng)濾波的基本途徑就是根據(jù)量測數(shù)據(jù)估計出輸出函數(shù)序列，再由推算出最佳增益矩陣K，使得增益矩陣K不斷地與實際量測數(shù)據(jù)相適應(yīng)。 .Sage-Husa自適應(yīng)卡爾曼濾波是在利用量測數(shù)據(jù)進(jìn)行遞推濾波時，通過時變噪聲估計估值器，實時估計和修正系統(tǒng)噪聲和量測噪聲的統(tǒng)計特性，從而達(dá)到降低系統(tǒng)模型誤差、抑制濾波發(fā)散提高哦濾波精度的目的。Sage-Husa自適應(yīng)卡爾曼濾波算法可描述為.其中，、和由以下時變噪聲統(tǒng)計估值器獲得：.式中：，為遺忘因子。 如果系統(tǒng)狀態(tài)變量的維數(shù)比較高，而Sage-Husa自適應(yīng)濾波算法中又增加了對系統(tǒng)噪聲統(tǒng)計特性的計算，計算量將大大增加，實時性也將難以得到保證。除此之外，對于階次較高的系統(tǒng)，Sage-Husa自適應(yīng)濾波算法中和的在線估計有時會由于計算發(fā)散失去半正定性和正定性而出現(xiàn)濾波發(fā)散現(xiàn)象，此時Sage-Husa自適應(yīng)濾波算法的穩(wěn)定性和收斂性不能完全保證?；跇O大似然準(zhǔn)則的自適應(yīng)卡爾曼濾波，通過系統(tǒng)狀態(tài)方差陣和量測噪聲方差陣實時估計系統(tǒng)噪聲統(tǒng)計特性的變化，以保證濾波器更好地適應(yīng)這種變化。極大似然估計從系統(tǒng)量測量出現(xiàn)概率最大的角度估計，其特點是不僅考慮新息的變化，而且考慮新息協(xié)方差矩陣的變化。它的量測噪聲協(xié)方差矩陣和系統(tǒng)噪聲協(xié)方差矩陣為：式中：，N為平滑窗口的寬度。擴(kuò)展卡爾曼濾波最初提出的卡爾曼濾波基本理論只適用于狀態(tài)方程和量測方程均為線性的隨機線性高斯系統(tǒng)。但是大部分系統(tǒng)是非線性的，其中還有許多事強非線性的。非線性估計的核心就在于近似,給出非線性估計方法的不同就在于其近似處理的思想和實現(xiàn)手段不同。近似的本質(zhì)就是對難以計算的非線性模型施加某種數(shù)學(xué)變換,變換成線性模型,然后用Bayes估計原理進(jìn)行估計。進(jìn)一步說,非線性變換到線性變換主要有兩種實現(xiàn)手段,一種是Taylor多項式展開,一種是插值多項式展開。Bucy和Y.Sunahara等人致力于研究將經(jīng)典卡爾曼濾波理論擴(kuò)展到非線性隨機系統(tǒng)濾波估計中，提出了離散非線性隨機系統(tǒng)擴(kuò)展卡爾曼濾波(Extended kalman filter,以下簡稱EKF)。EKF是傳統(tǒng)非線性估計中的代表，其基本思想是將非線性狀態(tài)函數(shù)和量測函數(shù)進(jìn)行局部線性化，即進(jìn)行一階Taylor多項式展開，然后應(yīng)用線性系統(tǒng)Kalman濾波公式。非線性離散系統(tǒng)狀態(tài)方程和觀測方程的一般形式如下所示 1-1式中：為輸入向量；和均為高斯白噪聲，且互不相關(guān)，其統(tǒng)計特性為：其中，式中，為過程激勵噪聲協(xié)方差矩陣,為觀測噪聲協(xié)方差矩陣。是一個非線性狀態(tài)轉(zhuǎn)換函數(shù),是一個非線性量測函數(shù)。每一個時刻點,根據(jù)一階泰勒展開將,線性化，即將非線性狀態(tài)函數(shù)和非線性量測函數(shù)圍繞濾波值展開泰勒級數(shù)，并略去二階以上項，得到 1-2 1-3定義，根據(jù)式（1-1）、式（1-2）和式（1-3）可以得到非線性系統(tǒng)線性化后只與狀態(tài)變量有關(guān)的表達(dá)式，如下 1-4式1-4中,注意到并非的函數(shù),并非的函數(shù),根據(jù)1-4近似結(jié)果,應(yīng)用上節(jié)的Kalman濾波器計算可以得到EKF迭代算法:定義，可得濾波方程初始條件 狀態(tài)先驗估計值 誤差協(xié)方差先驗估計值 增益矩陣 狀態(tài)后驗估計值誤差協(xié)方差后驗估計值 無跡卡爾曼濾波（UKF） EKF是一種次優(yōu)非線性高斯濾波器，它采用對非線性函數(shù)進(jìn)行線性化近似的方法，來計算狀態(tài)分布經(jīng)非線性函數(shù)傳遞之后的特性。盡管EKF得到了廣泛的應(yīng)用，但它依然存在自身無法克服的理論局限性:要求非線性系統(tǒng)狀態(tài)函數(shù)和量測函數(shù)必須是連續(xù)可微的，這限制了EKF的應(yīng)用范圍；對非線性函數(shù)的一階線性化近似精度偏低，特別地，當(dāng)系統(tǒng)具有強非線性時，EKF估計精度嚴(yán)重下降，甚至發(fā)散；需要計算非線性函數(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ù)和量測函數(shù)必須是連續(xù)可微的，它甚至可以應(yīng)用于不連續(xù)系統(tǒng)。可以證明：不論系統(tǒng)非線性程度如何，UT變換理論上至少能以三階泰勒精度逼近任何非線性高斯系統(tǒng)狀態(tài)的后驗均值和協(xié)方差，因此UKF的理論估計精度優(yōu)于EKF。UKF法首先要構(gòu)造Sigma 散點集, 設(shè)狀態(tài)向量為n 維, 為時刻k-1 的狀態(tài)向量估計值, 為該時刻狀態(tài)向量的協(xié)方差矩陣, 2n+1維的Sigma 點集可以表示為:，i=1,2，.，n對應(yīng)于的一階二階權(quán)系數(shù)為 其中，參數(shù)決定第i 個Sigma 點在狀態(tài)均值周圍的擴(kuò)展空間,是取值區(qū)間為 0.0001, 1 的常數(shù)； 為冗余量；為與狀態(tài)向量的先驗分布相關(guān)的參數(shù),對高斯分布,=2為最優(yōu)。由時刻k-1的和來計算Sigma 點集，通過非線性函數(shù)傳播為，由可得狀態(tài)向量預(yù)測值及誤差協(xié)方差陣 同理，利用和按照前面的采樣策略來計算Sigma 點集，通過非線性量測函數(shù)傳播為，由可得輸出預(yù)測值及自協(xié)方差陣和互協(xié)方差陣 在獲得新的量測后，進(jìn)行濾波量測更新中心差分卡爾曼濾波器 （CDKF） Ito等人從數(shù)值積分的觀點出發(fā)提出了一種次優(yōu)高斯濾波器：中心差分濾波器（Central Difference Filter,CDF）。CDF使用多項式插值方法來計算多維積分，其計算簡單，易于實現(xiàn)。 幾乎同時，M.Norgaard等人也使用stirling多項式插值公式來近似計算非線性函數(shù)的多維積分，得到了分開差分濾波器（Divided Difference Filter，DDF）。武元新等人通過理論分析指出，DDF和CDF都是基于函數(shù)擬合的思想來實現(xiàn)的，即都是使用一個函數(shù)序列近似被積函數(shù)，且函數(shù)序列中的每個函數(shù)積分都有解析解，此時近似函數(shù)的積分就可以看作是對積分的近似。由于DDF和CDF在本質(zhì)上是一致的，有異曲同工之妙，因此R.V.Merwe等人統(tǒng)一將它們稱為中心差分卡爾曼濾波器（Central Difference Kalman Filter,CDKF），并給出了CDKF的濾波遞推公式。 平方根SPKF算法 由于CDKF所采用的多項式插值公式等價于UKF中