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

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