曹后龍

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

1 引 言

在卡爾曼濾波應(yīng)用中，狀態(tài)方程或量測方程常為非線性函數(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)計特性作為問題結(jié)果，避免了線性化誤差，而且需要很少的采樣點(diǎn)就可以得到優(yōu)于擴(kuò)展卡爾曼濾波的估計結(jié)果，不僅易于實現(xiàn)而且在保持與線性化方法相當(dāng)運(yùn)算量的同時具有較高的估計精度和較廣的適用范圍[4]。

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

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

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

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

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

(1)

Lk=h(Xk)+ek

(2)

式中，狀態(tài)方程為線性函數(shù)，觀測方程為非線性函數(shù)；Xk為tk時刻的n×1維狀態(tài)向量，Φk，k-1為狀態(tài)轉(zhuǎn)移矩陣，Lk為tk時刻m×1維觀測向量，h(Xk)為非線性觀測函數(shù)；wk和ek分別為n×1維狀態(tài)噪聲向量和m×1維觀測噪聲向量，均為加性高斯白噪聲，兩者的協(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ù)測狀態(tài)信息與觀測信息之間對濾波估值的貢獻(xiàn)，而且能夠很好地控制前一時刻狀態(tài)參數(shù)估值異常誤差與動力學(xué)模型異常誤差對濾波估值的影響。

(12)

(13)

(14)

(15)

(16)

則：

(17)

對上式兩端同時取跡可得Ck為：

(18)

(19)

4 算例及分析

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

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

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

狀態(tài)噪聲協(xié)方差矩陣Q和觀測噪聲協(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)行濾波計算比較分析，采樣策略為對稱采樣。

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

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

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

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

(2)在給定初始狀態(tài)估值X0不恰當(dāng)時，即初始狀態(tài)參數(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)估值有誤差時，X坐標(biāo)差值比較

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

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

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

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

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

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

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

(2)從圖4、圖5、圖6可以看出，在觀測值可靠的情況下，由于狀態(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)估值對濾波解的貢獻(xiàn)，得到了可靠的濾波結(jié)果，提高了濾波估值的精度。

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

5 結(jié) 論

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