最佳自適應(yīng)無跡卡爾曼濾波算法應(yīng)用研究

曹后龍

(上海核工程研究設(shè)計(jì)院有限公司，上海 200233)

1 引 言

在卡爾曼濾波應(yīng)用中，狀態(tài)方程或量測(cè)方程常為非線性函數(shù)，則濾波問題也表現(xiàn)為非線性，為了解決在非線性系統(tǒng)中卡爾曼濾波理論的應(yīng)用問題，S.J.Julier和J.K.Uhlmann提出了無跡卡爾曼濾波[1，2，3]，該算法采用線性卡爾曼濾波框架，以無跡變換為基礎(chǔ)，通過某種采樣策略生成采樣點(diǎn)集，然后將采樣點(diǎn)集通過無跡變換得到新的采樣點(diǎn)集，將變換后采樣點(diǎn)集的統(tǒng)計(jì)特性作為問題結(jié)果，避免了線性化誤差，而且需要很少的采樣點(diǎn)就可以得到優(yōu)于擴(kuò)展卡爾曼濾波的估計(jì)結(jié)果，不僅易于實(shí)現(xiàn)而且在保持與線性化方法相當(dāng)運(yùn)算量的同時(shí)具有較高的估計(jì)精度和較廣的適用范圍[4]。

在許多實(shí)際問題中，無跡卡爾曼濾波對(duì)前一時(shí)刻狀態(tài)參數(shù)估值比較敏感，其誤差將會(huì)直接影響濾波的估值；即使前一時(shí)刻狀態(tài)參數(shù)估值合理，由于動(dòng)力學(xué)模型異常誤差的存在，這也會(huì)直接影響無跡卡爾曼濾波濾波解的精度[5]。針對(duì)上述問題，恰當(dāng)?shù)淖赃m應(yīng)因子不僅能夠合理地平衡預(yù)測(cè)狀態(tài)信息與觀測(cè)信息之間對(duì)濾波估值的貢獻(xiàn)，而且能夠很好地控制前一時(shí)刻狀態(tài)參數(shù)估值誤差與動(dòng)力學(xué)模型異常誤差對(duì)濾波估值的影響[6，7，8]。

本文在自適應(yīng)無跡卡爾曼濾波算法的理論基礎(chǔ)上，給出了其改進(jìn)算法，引入最佳自適應(yīng)因子[9]，得到最佳自適應(yīng)無跡卡爾曼濾波。通過最佳自適應(yīng)因子來調(diào)節(jié)預(yù)測(cè)狀態(tài)向量的協(xié)方差矩陣，即調(diào)整預(yù)測(cè)狀態(tài)信息對(duì)濾波解更加合理的貢獻(xiàn)，從而進(jìn)一步提高濾波解的精度。

2 自適應(yīng)無跡卡爾曼濾波算法

已知tk時(shí)刻的狀態(tài)方程和觀測(cè)方程分別為：

Xk=Φk，k-1Xk-1+wk

(1)

Lk=h(Xk)+ek

(2)

式中，狀態(tài)方程為線性函數(shù)，觀測(cè)方程為非線性函數(shù)；Xk為tk時(shí)刻的n×1維狀態(tài)向量，Φk，k-1為狀態(tài)轉(zhuǎn)移矩陣，Lk為tk時(shí)刻m×1維觀測(cè)向量，h(Xk)為非線性觀測(cè)函數(shù)；wk和ek分別為n×1維狀態(tài)噪聲向量和m×1維觀測(cè)噪聲向量，均為加性高斯白噪聲，兩者的協(xié)方差矩陣分別為Qk和Rk，且互不相關(guān)。

(3)

(4)

(5)

其協(xié)方差矩陣和估值為：

(6)

(7)

(8)

(9)

(10)

(11)

3 最佳自適應(yīng)因子

在自適應(yīng)無跡卡爾曼濾波中，恰當(dāng)?shù)淖赃m應(yīng)因子不僅能夠合理地平衡預(yù)測(cè)狀態(tài)信息與觀測(cè)信息之間對(duì)濾波估值的貢獻(xiàn)，而且能夠很好地控制前一時(shí)刻狀態(tài)參數(shù)估值異常誤差與動(dòng)力學(xué)模型異常誤差對(duì)濾波估值的影響。

(12)

(13)

(14)

(15)

(16)

則：

(17)

對(duì)上式兩端同時(shí)取跡可得Ck為：

(18)

(19)

4 算例及分析

在教學(xué)樓前廣場(chǎng)進(jìn)行動(dòng)態(tài)導(dǎo)航定位實(shí)驗(yàn)，車輛載體安裝導(dǎo)航模塊，實(shí)驗(yàn)時(shí)長 20 min，接收機(jī)采樣間隔T=10s。利用接收機(jī)自帶的處理軟件從原始數(shù)據(jù)文件里導(dǎo)出觀測(cè)文件和廣播星歷文件，并對(duì)觀測(cè)數(shù)據(jù)和導(dǎo)航數(shù)據(jù)進(jìn)行簡單的處理，取觀測(cè)數(shù)據(jù)次數(shù)100次。動(dòng)力學(xué)模型采用常速(CV)模型[12，13]，觀測(cè)模型采用偽距觀測(cè)方程[14]。

初始狀態(tài)參數(shù)估計(jì)值X0及其協(xié)方差矩陣D0為：

D0=diag(100，100，100，100，0.0012，0.0012，0.0012，1)

狀態(tài)噪聲協(xié)方差矩陣Q和觀測(cè)噪聲協(xié)方差矩陣R為：

Q=diag(100，100，100，100，0.0012，0.0012，0.0012，1)

R=diag(100，100，100，100，100，100，100)

使用標(biāo)準(zhǔn)無跡卡爾曼濾波算法和最佳自適應(yīng)無跡卡爾曼濾波算法分別進(jìn)行濾波計(jì)算比較分析，采樣策略為對(duì)稱采樣。

(1)狀預(yù)測(cè)態(tài)信息無異常誤差時(shí)，即動(dòng)力學(xué)模型無異常誤差，前一時(shí)刻狀態(tài)參數(shù)估值無異常誤差，如圖1～圖3所示。

圖1 預(yù)測(cè)狀態(tài)信息無異常誤差時(shí)，X坐標(biāo)差值比較

圖2 預(yù)測(cè)狀態(tài)信息無異常誤差時(shí)，Y坐標(biāo)差值比較

圖3 預(yù)測(cè)狀態(tài)信息無異常誤差時(shí)，Z坐標(biāo)差值比較

(2)在給定初始狀態(tài)估值X0不恰當(dāng)時(shí)，即初始狀態(tài)參數(shù)估值存在較大誤差時(shí)，x、y、z坐標(biāo)差值如圖4～圖6所示。

X0=[-2412100.0 4687600.0 3564900.0 609729.1 0 0 0 100.0]T

圖4 初始狀態(tài)估值有誤差時(shí)，X坐標(biāo)差值比較

圖5 初始狀態(tài)估值有誤差時(shí)，Y坐標(biāo)差值比較

圖6 初始狀態(tài)估值有誤差時(shí)，Z坐標(biāo)差值比較

(3)為了檢驗(yàn)最佳自適應(yīng)因子的效果，每隔20次就在前一時(shí)刻狀態(tài)參數(shù)估值向量中的X坐標(biāo)，Y坐標(biāo)和Z坐標(biāo)上分別人為地加上 80 m異常誤差，x、y、z坐標(biāo)差值如圖7～圖9所示，即：

圖7 前一時(shí)刻狀態(tài)估值有異常誤差時(shí)，X坐標(biāo)差值比較

圖8 前一時(shí)刻狀態(tài)估值有異常誤差時(shí)，Y坐標(biāo)差值比較

圖9 前一時(shí)刻狀態(tài)估值有異常誤差時(shí)，Z坐標(biāo)差值比較

(1)從圖1、圖2、圖3可以看出，在觀測(cè)值可靠的情況下，在濾波初始段，標(biāo)準(zhǔn)無跡卡爾曼濾波估值精度稍微低于最佳自適應(yīng)無跡卡爾曼濾波估值精度；但在標(biāo)準(zhǔn)無跡卡爾曼濾波算法穩(wěn)定以后，兩者的濾波結(jié)果基本一致，精度相當(dāng)。

(2)從圖4、圖5、圖6可以看出，在觀測(cè)值可靠的情況下，由于狀態(tài)參數(shù)初始值存在較大誤差，導(dǎo)致了標(biāo)準(zhǔn)無跡卡爾曼濾波初期的濾波結(jié)果發(fā)散，精度很差，但隨著濾波的進(jìn)行，逐漸趨于穩(wěn)定；而最佳自適應(yīng)無跡卡爾曼濾波通過最佳自適應(yīng)因子調(diào)整初始狀態(tài)估值的協(xié)方差矩陣，減少初始狀態(tài)估值對(duì)濾波解的貢獻(xiàn)，得到了可靠的濾波結(jié)果，提高了濾波估值的精度。

(3)從圖7、圖8、圖9可以看出，在預(yù)測(cè)狀態(tài)向量存在較大誤差時(shí)，標(biāo)準(zhǔn)無跡卡爾曼濾波穩(wěn)定性很差，得到的濾波結(jié)果不可靠；而此時(shí)最佳自適應(yīng)無跡卡爾曼濾波能夠很好地控制預(yù)測(cè)狀態(tài)信息對(duì)濾波結(jié)果的影響，得到可靠的濾波結(jié)果。

5 結(jié) 論

本文先給出了自適應(yīng)無跡卡爾曼濾波算法，再利用預(yù)測(cè)殘差的理論協(xié)方差矩陣與實(shí)際協(xié)方差矩陣之間的關(guān)系，引入無跡卡爾曼濾波中最佳自適應(yīng)因子，得到了最佳自適應(yīng)因子無跡卡爾曼濾波算法。當(dāng)預(yù)測(cè)殘差實(shí)際協(xié)方差和理論協(xié)方差相等時(shí)，即自適應(yīng)因子Ck=1，此時(shí)的自適應(yīng)無跡卡爾曼濾波就是標(biāo)準(zhǔn)無跡卡爾曼濾波。在觀測(cè)值可靠的情況下，預(yù)測(cè)殘差反映了動(dòng)力學(xué)模型誤差與前一時(shí)刻狀態(tài)估值誤差總共的大小，最佳自適應(yīng)無跡卡爾曼濾波將它們作為一個(gè)整體，利用最佳自適應(yīng)因子合理地調(diào)整它們整體對(duì)濾波結(jié)果的貢獻(xiàn)。通過實(shí)例驗(yàn)證了最佳自適應(yīng)因子在無跡卡爾曼濾波中不僅能夠適用，而且能夠提高濾波結(jié)果的精度。