幾種卡爾曼濾波算法理論

1、word自適應(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、果計算機字長不夠長，這種積累誤差很有可能使估計誤差方差陣失去非 負(fù)定性甚至失去對稱性，使濾波增益矩陣逐漸失去適宜的加權(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)濾波的根本途徑就是根據(jù)

4、量測數(shù)據(jù)估計出輸出函數(shù)序列CJ ,再由C J推算出最優(yōu)增益矩陣K,使得增益矩陣K不斷地與實際量測數(shù)據(jù)CJ相適應(yīng)。wordSage-Husa自適應(yīng)卡爾曼濾波是在利用量測數(shù)據(jù)進展遞推濾波時，通過時變 噪聲估計估值器，實時估計和修正系統(tǒng)噪聲和量測噪聲的統(tǒng)計特性，從而達到降 低系統(tǒng)模型誤差、抑制濾波發(fā)散提高哦濾波精度的目的。Xkk,k iXk 1 WkZk HkXk VkE(wQ qk，E(WkWT) Qk 內(nèi)E(Vk) rk, E(VkVT) Rk kjE(WkVT) 0Sage-Husa自適應(yīng)卡爾曼濾波算法可描述為XkXk,k 1 K kzkXk,k1 k,k 1?k 1qk 1ZkZkHkXk,

5、k1?KkPk,k1HTHkPk,k1HTRk 1Pk,kk,k 1 Pk 1 k,k 1Qk 1Pk(IKM)k 1.其中,r?、Rk、qk和Qk由以下時變噪聲統(tǒng)計估值器獲得:?k 1(1dk)? dk(Zk 1 Hk 1?k 1,k)冬1式中:(1 dk)昆dk(k1：1 也用1,田：1)(1dk)qkdk(xk 1 k 1,k>?k)(1dk)£dk(Kk 1k 1kr1KT1Pk 1 k1,kR T1,k)1 b、，一、一一dk k-y , 0 b 1為返忘因子。1 bk 1如果系統(tǒng)狀態(tài)變量的維數(shù)比擬高,而 Sage-Husa自適應(yīng)濾波算法中又增加 了對系統(tǒng)噪聲統(tǒng)計特性

6、的計算，計算量將大大增加，實時性也將難以得到保證。word除此之外，對于階次較高的系統(tǒng)，Sage-Husa自適應(yīng)濾波算法中Rk和Qk的在線估計有時會由于計算發(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é)方差矩陣Cvk的變化。它的量測噪聲協(xié)方差矩陣 E和系統(tǒng)噪聲協(xié)方差矩陣Q為:RkCvkHkPk,kiH

7、TQk1 kT DXi XiPkN i k N 1k,k 1 Pk 1 k,k 1XkXkxk,k 1Kkvkc 1k TCvkViViN i k N 1式中：VkZkzk,k 1 , N為平滑窗口的寬度。擴展卡爾曼濾波最初提出的卡爾曼濾波根本理論只適用于狀態(tài)方程和量測方程均為線性的 隨機線性高斯系統(tǒng)。但是大局部系統(tǒng)是非線性的，其中還有許多事強非線性的。 非線性估計的核心就在于近似，給出非線性估計方法的不同就在于其近似處理的 思想和實現(xiàn)手段不同。近似的本質(zhì)就是對難以計算的非線性模型施加某種數(shù)學(xué)變 換，變換成線性模型，然后用Bayes估計原理進展估計。進一步說，非線性變換到 線性變換主要有兩種實

8、現(xiàn)手段，一種是Taylor多項式展開，一種是插值多項式展 開。Bucy和Y.Sunahara等人致力于研究將經(jīng)典卡爾曼濾波理論擴展到非線性隨 機系統(tǒng)濾波估計中，提出了離散非線性隨機系統(tǒng)擴展卡爾曼濾波（Extendedkalman filter,以下簡稱EKF）。EKF是傳統(tǒng)非線性估計中的代表，其根本思想是word將非線性狀態(tài)函數(shù)和量測函數(shù)進展局部線性化，即進展一階Taylor多項式展開,然后應(yīng)用線性系統(tǒng)Kalman濾波公式。非線性離散系統(tǒng)狀態(tài)方程和觀測方程的一般形式如下所示Xk 1f (Xk,Uk)kWk/、1-1Zk g(Xk,Uk) Vk式中：Uk Rr為輸入向量；Wk Rp和Vk Rq均

9、為高斯白噪聲，且互不相關(guān),其統(tǒng)計特性為：E(Wk) 0,Cov(Wk,Wj) Qk kj其中，E(Vk) 0,Cov(Vk,Vj) Rk kjCov(wk,Vj) 0式中，Q為過程激勵噪聲協(xié)方差矩陣，Rk為觀測噪聲協(xié)方差矩陣。f (Xk 1,Uk 1)是一個非線性狀態(tài)轉(zhuǎn)換函數(shù)，g(Xk 1,Uk 1)是一個非線性量測函數(shù)。每一個時刻點,根據(jù)一階泰勒展開將f (Xk 1,Uk i) , g(Xk 1,Uk 1)線性化,即將非線性狀態(tài)函數(shù)f(;)和非線性量測函數(shù)g(;)圍繞濾波值展開泰勒級數(shù)，并略去二階以上 項，得到f(Xk,Uk)f(Xk,Uk)f(Xk,Uk)XkXk(XkXk)1-2Xkg(

10、Xk,Uk)g(Xk,Uk)g(Xk,Uk)xk5(XkXk)1-3Xk定義？kf(Xk,Uk)|Xk Xk , Hk 前迎Xk Xk ,根據(jù)式1-1、式1-2XkXk和式1-3可以得到非線性系統(tǒng)線性化后只與狀態(tài)變量有關(guān)的表達式，如下Xk 1?kXk f(?k,Uk) ?kXkkWk一1-4Zk H?kXk g(猿，Uk) F?kXk Vk式1-4中，注意到f(Xk,Uk) ?kXk并非Xk的函數(shù)，g(Xk,Uk) HkXk并非Xk的函數(shù),根據(jù)1-4近似結(jié)果，應(yīng)用上節(jié)的Kalman濾波器計算可以得到EKF迭代算法:word定義？卜 士山)？ r .gUk,Uk) Xk?，可得 kxk xkkX

11、kXk，pi7XkXk濾波方程初始條件Xo E(x0),P0 var(x0)狀態(tài)先驗估計值>?k,k i f (K i，Uk i)誤差協(xié)方差先驗估計值Pk,k ik,k 1 Pk 1 k,k 1k,k iQk i k,k i增益矩陣KkPk,kiH：HkPk,kiH：Rk i狀態(tài)后驗估計值）?k?k,k iLZ gCkiUH誤差協(xié)方差后驗估計值Pk(I KkHJR,ki無跡卡爾曼濾波UKHEKF 是一種次優(yōu)非線性高斯濾波器，它采用對非線性函數(shù)進展線性化 近似的方法，來計算狀態(tài)分布經(jīng)非線性函數(shù)傳遞之后的特性。盡管EKF得到了廣 泛的應(yīng)用，但它依然存在自身無法克制的理論局限性：要求非線性系統(tǒng)

12、狀態(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è)隨

13、機系統(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)于EKEUK祛首先要構(gòu)造Sigma散點集，設(shè)狀態(tài)向量為n維，猿1為時刻k-1的word狀態(tài)向量估計值，Pki為該時刻狀態(tài)向量的協(xié)方差矩陣,2n+1維的Sigma點集可 以表示為：x0,k 1 雙 1X,ki 兄 1 (忑一)P,k i)i ,

14、i=1,2 , . , nXi n,k1 尺 1(. (n -)P,k1)i對應(yīng)于i的一階二階權(quán)系數(shù)為'A,m/(n) i 01/2(n) i 02W /(n) 1i 01/2(n)i 0其中，2(n) n參數(shù) 決定第i個Sigma點在狀態(tài)均值稱1周圍的擴展空間，是取值區(qū)間為0.0001, 1的常數(shù)；為冗余量；為與狀態(tài)向量的先驗分布相關(guān)的參數(shù)，對高斯分布，=2為最優(yōu)。由時刻k-1的Xk 1和Pk 1來計算Sigma點集為到0,1,L),通過非線性函數(shù)fk i() qk i傳播為 xi,k/k 1 ,由 xi,k/k 1 可得狀態(tài)向量預(yù)測值 Xk/k 1 與誤差協(xié)方差陣-1Xi,k/k

15、1 fk 1 (xi,k 1 ) qk 1 i 。1,LLLk/ k 1Pk/k 1Wi xi ,k/k 1Wi fk 1 (xi,k 1) qk 1i 0i 0LWi (xi,k/k 1?k/k 1)(xi,k/k 15?k/ k 1 ) Qk 1i 0同理，利用濕1和R 1按照前面的采樣策略來計算Sigma點集xi,k 1 (i 0,1 .,L),通過非線性量測函數(shù)hk()4傳播為 鵬1 ,由 小可得輸word出預(yù)測值？k/k 1與自協(xié)方差陣PZk和互協(xié)方差陣Pkki,k/k 1hk(xi,k/k 1)2k/k 1LWmWi i ,k/k 1 i 0LWmhk(Xi,k/k 1)rk i

16、0PZkc/Wi ( i,k/k 1 0?k/k 1)( i,k/k 1Zk/k 1)RkLk/k 1 )( i ,k/k 1Zk/k 1 )c ，Wi (xi,k/k 1i 0在獲得新白量測后Zk ,進展濾波量測更新?k/k 1K k(zkzk/k 1)_ 1Kk %PkRPk/k 1 Kk比 KT中心差分卡爾曼濾波器CDKFIto等人從數(shù)值積分的觀點出發(fā)提出了一種次優(yōu)高斯濾波器：中心差分濾波器Central Difference Filter,CDF 。CDF®用多項式插值方法來計算 多維積分，其計算簡單，易于實現(xiàn)。幾乎同時，M.Norgaard等人也使用Stirling多項式插值公式來近似計算非線性函數(shù)的多維積分，得到了分開差分濾波器 Divided Difference Filter , DDF。武元新等人通過理論分析指出，DD林口 CDFtB是基于函數(shù)擬合的 思想來實現(xiàn)的，即都是使用一個函數(shù)序列近似被積函數(shù)，且函數(shù)序列中的每個函數(shù)積分都有解析解，此時近似函數(shù)的積分就可以看作是對積分的近似。由于 DDF 和CDFfc本質(zhì)上是一致的，有異曲同工之妙，因此R.V.Merwe等人統(tǒng)一將它們稱 為中心差分卡爾曼濾波器Central Differ