曹后龍
(上海核工程研究設(shè)計(jì)院有限公司,上海 200233)
在卡爾曼濾波應(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)一步提高濾波解的精度。
已知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)
在自適應(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)
在教學(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é)果。
本文先給出了自適應(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é)果的精度。