李曉明，趙長勝，張立凱

(江蘇師范大學(xué)地理測繪與城鄉(xiāng)規(guī)劃學(xué)院，江蘇 徐州 221116)

目前應(yīng)用于GPS單點定位的非線性濾波方法有基于貝葉斯估計的各種卡爾曼濾波改進(jìn)算法和貫序的蒙特卡羅方法-粒子濾波(particle filter，PF)及其結(jié)合抗差、自適應(yīng)、平方根因子等的改進(jìn)算法。自適應(yīng)卡爾曼濾波的提出有效抑制了由于狀態(tài)噪聲方差和觀測噪聲方差的不確定而引起的濾波發(fā)散問題。基于有色噪聲一階AR模型的自適應(yīng)擴展卡爾曼濾波算法確定了自適應(yīng)因子，有效提升了解算過程中的收斂速度和幅度[1]，缺陷在于省略了高階項信息，而且只能在噪聲較小時使用。UKF摒棄了EKF對非線性函數(shù)線性化的傳統(tǒng)處理，對于非線性的預(yù)測方程，基于先驗均值和先驗方差矩陣的平方根選擇采樣點，對于非線性分布的統(tǒng)計量有較高的計算精度[2]，非線性程度高時，協(xié)方差極容易出現(xiàn)負(fù)定，平方根無跡卡爾曼濾波具有數(shù)值穩(wěn)定性，因此保證了狀態(tài)協(xié)方差的正定性[3]?；赨T變換和自適應(yīng)調(diào)節(jié)因子綜合選取重要密度函數(shù)進(jìn)行粒子的重要性重采樣，提高了機器人的定位精度[4]。改進(jìn)的無跡粒子濾波算法避免在狀態(tài)空間進(jìn)行無跡變換，從而降低了時間開銷[5]。改進(jìn)的非線性容積卡爾曼濾波算法在單點定位中的應(yīng)用，有效地抑制了預(yù)報信息誤差[6]；通過將SRUKF算法和自適應(yīng)因子結(jié)合，迭代計算更新系統(tǒng)狀態(tài)方差陣和噪聲方差陣的平方根，以保證系統(tǒng)的非負(fù)定性[7]。粒子濾波(也稱為粒子云濾波)的作用主要是用于參數(shù)估計，不受高斯模型的約束，不同于UKF的是，PF借用蒙特卡洛的思想，調(diào)整離散的樣本集的權(quán)值來近似后驗分布，從而獲得狀態(tài)的最小方法估計。粒子濾波算法的定位精度適用于非線性[8]、非高斯、多維狀態(tài)問題[9]，能有效減弱噪聲擾動，但同時增大了計算時間[10]，粒子濾波過程中粒子退化問題影響了粒子濾波的應(yīng)用。

本文結(jié)合無跡卡爾曼濾波、粒子濾波各自的優(yōu)點，采用自適應(yīng)濾波理論，提出了自適應(yīng)平方根無跡粒子濾波(ASRUPF)的新算法。ASRUPF選擇合適的重采樣技術(shù)，分別對系統(tǒng)狀態(tài)和觀測進(jìn)行更新，在保證過程噪聲方差陣正定的同時，有效延緩粒子退化現(xiàn)象，改善了粒子濾波算法性能，提高非線性系統(tǒng)的狀態(tài)估值精度，且具有抗差性質(zhì)。應(yīng)用于GPS單點定位解算中取得了較好的濾波效果。

1 自適應(yīng)SRUKF算法

平方根無跡卡爾曼濾波使UT變換后的驗后方差精度具有三階精度，但濾波效果仍然受到濾波初值的影響，假設(shè)由隨機變量x和具有高斯白噪聲的觀測變量Z構(gòu)成的非線性系統(tǒng)描述如下

(1)

式中，xk為n維隨機變量；Zk為觀測值向量；f( )為系統(tǒng)狀態(tài)函數(shù)；h( )為量測函數(shù)；狀態(tài)噪聲wk-1與觀測噪聲vk互不相關(guān)且服從高斯分布，其協(xié)方差矩陣分別為Q、R。

假設(shè)已知k-1時刻的狀態(tài)均值和初始方差為Xk和P，通過UT變換得到2n+1個Sigma點集及其相應(yīng)的權(quán)值，得到狀態(tài)的非線性映射，過程如下。

(1) 權(quán)值計算公式為

(2)

式中，m為均值；c為協(xié)方差；λ、α、β為UT變換的參數(shù)。

(2) 采取平方根法，對權(quán)陣進(jìn)行Cholesky分解，使用chol函數(shù)計算Sigma點集為

(3)

(3) Sigma點集計算及一步預(yù)測為

(4)

(5)

(4) 狀態(tài)預(yù)估為

(6)

狀態(tài)協(xié)方差平方根預(yù)估，采用矩陣QR分解，用狀態(tài)協(xié)方差的Cholesky因子代替進(jìn)行遞推運算，cholupdate表示矩陣的上三角矩陣和其轉(zhuǎn)置的乘積

(7)

(8)

再次使用UT變換對噪聲方差的平方根進(jìn)行預(yù)測為

(9)

(10)

(5) 觀測預(yù)測值為

(11)

(6) 計算預(yù)測殘差為

(12)

預(yù)測殘差可作為預(yù)估信息準(zhǔn)確性的評判標(biāo)準(zhǔn)，基于預(yù)測殘差統(tǒng)計量的自適應(yīng)因子可用于估計量測或狀態(tài)噪聲，指數(shù)函數(shù)表示的自適應(yīng)因子可表示為[11]

(13)

(14)

式(7) 求得觀測值協(xié)方差及其與狀態(tài)值的互協(xié)方差為

(15)

其中

(16)

求得濾波增益、系統(tǒng)的狀態(tài)更新及其協(xié)方差更新為

(17)

2 自適應(yīng)無跡粒子濾波算法(ASRUPF)

(18)

利用粒子濾波計算采樣更新粒子的核心為在一次迭代周期中UKF算法可為每個粒子計算其均值及協(xié)方差，然后用于更新采樣該粒子，其實質(zhì)是從建議密度函數(shù)角度來優(yōu)化粒子濾波算法，從k=0時刻起，流程如下

(19)

(20)

粒子濾波算法輸出的是一組樣本點，當(dāng)采樣數(shù)量無窮大時，這些樣本點可近似地表示為后驗分布

(21)

狀態(tài)的均值計算可表示為

(22)

式中，gk為利益函數(shù)，若選用X0:k為邊緣條件均值，此時gk(X0:k)=Xk，邊緣條件均值反映了利益函數(shù)的質(zhì)量。

3 ASRUPF在GPS定位解算中的應(yīng)用

GPS偽距觀測方程表示為[14]

ρ=R+cI+cT+cδtu-cδts

(23)

式中，ρ為偽距觀測值；R為衛(wèi)星與接收機的幾何距離；c為真空中光速；I為電離層延遲；T為對流層延遲；δtu為接收機鐘差；δts為衛(wèi)星鐘差。

選取的載體運動模型選擇CV(常速度)模型，狀態(tài)函數(shù)表示為

xk=Φ·xk-1+wk-1

(24)

其中，接收機狀態(tài)

狀態(tài)噪聲協(xié)方差陣取為

q為位置和速度系統(tǒng)噪聲的譜密度，取q=0.01 m2·s-3。本文算法驗證所用數(shù)據(jù)為廣播星歷，共1364個歷元，以GPS雙頻偽距觀測消除電離層誤差，采用Hop-filed模型改正對流層延遲誤差[15-16]，使用自適應(yīng)平方根無跡粒子濾波器進(jìn)行濾波處理，采用的重采樣算法為殘差重采樣，量測噪聲為

R=I(size(sat,1))

式中，sat為觀測的衛(wèi)星數(shù)。

初始狀態(tài)協(xié)方差陣取

Qx=diag([111500111500])

以精密單點定位數(shù)據(jù)后處理值為濾波估計參考，三維位置各個方向濾波誤差如圖1—圖3所示。

各方向位置均方誤差結(jié)果見表1，一個迭代周期中各算法平均耗時見表2。

圖1 LS處理結(jié)果

圖2 ASRUKF濾波結(jié)果

表1 三維坐標(biāo)位置偏差均方根誤差 m

圖3 ASRUPF濾波結(jié)果

表2 各算法耗時比較 s

圖1—圖3的濾波結(jié)果表明，在GPS定位解算中，因UKF采用的無跡變換算法能使后驗方差的精度達(dá)到三階，UKF較LS有較高的濾波精度優(yōu)勢，基于預(yù)測殘差統(tǒng)計量的自適應(yīng)無跡卡爾曼濾波與粒子濾波的結(jié)合使得濾波算法得到優(yōu)化，ASRUPF處理的結(jié)果相較于ASRUKF有較大的提升，自適應(yīng)因子減小了誤差和不確定性因素的影響，PF與UKF相比增加了粒子的多樣性，延緩了退化，但是算法耗費的時間增加了，3種算法在一個迭代周期中的耗時長度LS最短，ASRUKF次之，ASRUPF耗時最長。

4 結(jié) 語

本文利用無跡卡爾曼在濾波精度上的優(yōu)勢，結(jié)合基于預(yù)測殘差統(tǒng)計量的自適應(yīng)因子，構(gòu)建出合理的建議密度函數(shù)，從而改進(jìn)了粒子濾波算法，提升了算法性能，應(yīng)用到GPS定位解算領(lǐng)域，具有很好的定位精度，進(jìn)一步提高了GPS導(dǎo)航定位的可靠性。目前仍然存在的問題是隨著歷元的增加及粒子數(shù)的增多，ASRUPF在實時性要求較高的領(lǐng)域相比一般的濾波算法優(yōu)勢不明顯， 如何選擇更合理的建議密度函數(shù)或更好地改進(jìn)濾波算法則有待更進(jìn)一步的研究。