自適應平方根無跡粒子濾波算法及其應用

李曉明，趙長勝，張立凱

(江蘇師范大學地理測繪與城鄉規劃學院，江蘇 徐州 221116)

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

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

1 自適應SRUKF算法

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

(1)

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

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

(1) 權值計算公式為

(2)

式中，m為均值；c為協方差；λ、α、β為UT變換的參數。

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

(3)

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

(4)

(5)

(4) 狀態預估為

(6)

狀態協方差平方根預估，采用矩陣QR分解，用狀態協方差的Cholesky因子代替進行遞推運算，cholupdate表示矩陣的上三角矩陣和其轉置的乘積

(7)

(8)

再次使用UT變換對噪聲方差的平方根進行預測為

(9)

(10)

(5) 觀測預測值為

(11)

(6) 計算預測殘差為

(12)

預測殘差可作為預估信息準確性的評判標準，基于預測殘差統計量的自適應因子可用于估計量測或狀態噪聲，指數函數表示的自適應因子可表示為[11]

(13)

(14)

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

(15)

其中

(16)

求得濾波增益、系統的狀態更新及其協方差更新為

(17)

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

(18)

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

(19)

(20)

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

(21)

狀態的均值計算可表示為

(22)

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

3 ASRUPF在GPS定位解算中的應用

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

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

(23)

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

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

xk=Φ·xk-1+wk-1

(24)

其中，接收機狀態

狀態噪聲協方差陣取為

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

R=I(size(sat,1))

式中，sat為觀測的衛星數。

初始狀態協方差陣取

Qx=diag([111500111500])

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

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

圖1 LS處理結果

圖2 ASRUKF濾波結果

表1 三維坐標位置偏差均方根誤差 m

圖3 ASRUPF濾波結果

表2 各算法耗時比較 s

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

4 結 語

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