M估計的強跟蹤SVD-UKF算法在組合導航中的應用

池傳國，黃國勇，孫 磊

(1．昆明理工大學 信息工程與自動化學院，云南 昆明 650500；2．云南省礦物管道輸送工程技術研究中心，云南 昆明 650500)

組合導航系統中，觀測數據異常是導致濾波系統發散的原因之一。衛星導航系統中系統輸出數據不可避免會出現野值數據或粗差，與慣性導航系統數據融合濾波后仍不可避免異常數據對系統輸出的影響。衛星導航數據異常通常分為3類：單粗差、多粗差與緩慢增長誤差。衛星導航系統中，緩慢增長誤差常見于接收機鐘差問題，在松組合方式的組合導航系統中無法消除。衛星導航數據的單粗差和多粗差異常會導致組合導航濾波系統魯棒性降低[1]。

強跟蹤SVD-UKF算法中，強跟蹤濾波框架建立在新息正交的基礎上，利用正常新息數據實現對實際狀態的強跟蹤，其并不能判斷帶有單粗差或多粗差的新息序列的健康程度。一旦異常數據污染了新息序列，強跟蹤濾波框架強迫新息殘差序列正交后導致多重漸消因子變化，引起估計結果的偏差，降低估計精度并可能導致濾波系統的發散?？共罟烙嬍浅Ｓ玫慕鉀Q衛星信號野值數據問題的手段之一[2]。當濾波系統中粗差的影響不可避免時，通過適當的手段來避免粗差對狀態估計值的影響?？共罟烙嬍峭ㄟ^等價權來影響新息序列，以降低新息序列在出現野值時造成對狀態估計值的影響。將抗差估計引入強跟蹤SVD-UKF算法，在算法進行量測方程更新前對新息序列預處理，剔除新息序列中存在的衛星野值，保證濾波系統的可靠性和魯棒性[3]。

強跟蹤SVD-UKF算法是基于新息序列來判斷估計狀態是否偏離，故新息序列的信息完整性是強跟蹤濾波算法對系統建模及濾波精度保證的要求之一[4]。M估計是抗差估計的一種，其本質是通過等價權函數對新息序列進行加權，根據殘差序列來確定新息序列權重。當回歸殘差較大時，降低新息序列中異常數據對狀態估計值影響；當回歸殘差較小時，增大權重保證新息序列信息完整性。M估計的等價權函數方式有效提高新息數據的可利用率。因此，選擇M估計方法用于抑制衛星信號野值問題，能夠最大程度的提出衛星信號中的異常數據，并最低程度的影響新息數據的信息完整性，保證強跟蹤SVD-UKF算法在新息正交時不受影響[5]。

1 M估計理論

1.1 等價權原理

選取狀態向量X=[x1,x2,x3,…,xm]T，引入一組新息序列Z=[z1,z2,z3,…,zn]T，則相應的殘差方程為

(1)

在最小二乘估計中，最優線性無偏估計的準則是殘差平方和最小，即

(2)

最小二乘估計在新息異常時，新息值會直接影響估計結果的精度。M估計是基于最小二乘的穩健估計，通過加權新息序列來降低異常數據對估計過程的影響。如何選擇加權因子直接影響M估計的估計精度。

構造觀測加權矩陣B=diag[b1,b2,b3,…,bn]T，使得

(3)

則加權最小二乘估計為

(4)

針對M估計建立如式(5)所示的準則函數

(5)

式中f為適當選擇的函數，對準則函數以X(k)求偏導得

(6)

令φ[e(k)i]/e(k)i=ωi，則有

(7)

將結果帶入式中，可以得到基于M估計的最小二乘估計

(8)

當等價權矩陣為I時，基于M估計的最小二乘估計降階到標準最小二乘，故實際上最小二乘估計是M估計的一種特殊形式。等價權矩陣的選擇直接影響到M估計的精度，選擇不同的等價權函數代替等價權矩陣，能夠自適應處理新息數據，以更有效的利用新息數據。

1.2 等價權函數選擇

等價權函數的選擇是M估計理論的關鍵。等價權函數的選擇需要結合應用對象來進行選擇，以保證M估計的精度[6]。結合衛星導航數據的特點，等價權函數的選取目標是：

(1)新息數據在先驗模型的正常噪聲范圍內，經過M估計后仍然保證其有效性；

(2)新息數據超出先驗模型的正常噪聲范圍程度較小，經過M估計后，新息數據包含的有效信息不被受到破壞性影響；

(3)新息數據超出先驗模型的正常噪聲范圍程度較大，經過M估計后，新息數據不會較大程度影響濾波系統的狀態估計值。

根據等價權函數的選取目標，可以將等價權分為3類：(1)正常區，不影響新息序列；(2)調整區，通過權因子調整新息序列；(3)拒絕區，剔除新息數據。

根據等價權區劃分，選擇合適的等價權函數，保證新息數據在正常區能夠不影響后續狀態估計，在調整區提高新息數據的可利用率，在拒絕區實現對污染數據的抗差[7]。由此來選擇等價權函數，解決衛星信號野值問題。

常用的等價權函數有IGG法、丹麥法、Huber法等。根據上面等價權函數的選取原則以及衛星信號數據的特點，選擇IGG法作為本文M估計的等價權函數。IGG法相對等價權區將權函數分為3段，在正常區不對新息序列產生任何影響，即等價權矩陣為I，不影響后續估計；在調整區，對新息序列進行降權處理，保證新息數據信息的部分可用；在拒絕區，拋棄不可用新息數據避免估計受到粗差影響[8]。

(9)

其中，c0由系統新息數據統計確定。

1.3 基于M估計的強跟蹤SVD-UKF算法

強跟蹤SVD-UKF算法由于其新息正交性原理在新息數據誤差較大的情況易導致濾波發散。當衛星信號出現野值時，強跟蹤濾波框架不可避免的會受到野值的影響。將M估計應用到強跟蹤SVD-UKF算法中，在強跟蹤SVD-UKF算法迭代前對新息數據進行M估計，通過選擇恰當的等價權函數，判斷新息數據是否為野值并處理，有效解決了衛星信號野值對強跟蹤SVD-UKF算法干擾。等價權函數的選擇充分考慮衛星信號數據的基本模型以及強跟蹤SVD-UKF算法的內部結構，選擇IGG法等價權函數在數據異常時降低數據對強跟蹤SVD-UKF算法的干擾，在數據正常時，保證新息數據對于強跟蹤SVD-UKF算法的可利用率[9]。

基于M估計的強跟蹤SVD-UKF算法在強跟蹤SVD-UKF算法框架下，利用M估計對新息數據預處理，數據處理后送入強跟蹤SVD-UKF算法得到結果[10]。

將基于M估計的強跟蹤SVD-UKF算法應用到組合導航系統中，選擇系統狀態方程狀態向量的選擇如下

X(t)=[L,λ,h,VE,VN,VU,φE,φN,φU,εbx,εby,εbz]

(10)

式中,L,λ,h為位置；φE,φN,φU為INS輸出的姿態角；VE,VN,VU為速度；εbx,εby,εbz為陀螺零偏。系統的量測向量選擇如下

Z=[LB,λB,hB,VEB,VNB,VUB]

(11)

基于M估計的強跟蹤SVD-UKF算法的基本流程如下：

(1)初始化。狀態向量X以及協方差矩陣P的初值為

(12)

(2)構造Sigma點集

(13)

(3)時間更新

(14)

(15)

(16)

Yi=f(Xi);i=0,1,…,2n

(17)

(4)新息序列M估計處理

(18)

(19)

(20)

(5)多重漸消因子更新

(21)

Ck,i=tr[Nk]/tr[Mii,k]

(22)

(23)

(24)

(25)

(6)量測更新

(26)

(27)

(28)

由上可知，利用M估計理論，對異常新息數據進行“篩選”，保留有用新息，剔除有害新息，提供可靠有效的新息數據輸入強跟蹤SVD-UKF算法。經過M估計處理后的新息數據對于強跟蹤SVD-UKF算法的意義在于濾除衛星信號野值，保留新息有效信息，避免強跟蹤SVD-UKF算法因模型不精確導致估計精度的降低[11]。

2 仿真驗證

為了驗證本文提出算法的有效性，以下通過仿真實驗對基于M估計的強跟蹤SVD-UKF算法進行驗證，并與強跟蹤SVD-UKF算法進行對比驗證。

假設飛機做機動飛行，飛行軌跡中含有爬升、變速、平飛和轉彎等各種飛行狀態。飛機的初始位置為東經107.002°，北緯29.498°，高度300 m；初始速度為50 m/s，方向正北。其中，陀螺的常值漂移為0.1 deg/h，加速度計常值誤差1.0g～4g，陀螺一階馬爾可夫過程相關時間為3 600 s，加速度一階馬爾可夫過程相關時間為1 800 s。BDS水平位置誤差均方根10 m，高度誤差均方根20 m，速度誤差均方根0.1 m/s。INS初始水平位置誤差50 m，高度誤差為100 m；初始速度誤差0.5 m/s。BDS采樣周期1 s，INS采樣周期0.02 s，濾波周期1 s，仿真時間1 000 s。

兩組算法系統狀態向量與量測向量的選擇與強跟蹤SVD-UKF算法仿真實驗相同，在以上仿真模型中，衛星導航位置數據加入異常粗差后進行對比驗證算法效果。

(1)衛星單粗差數據異常對比。在仿真模型中加入5組水平位置異常數據，加入時間分別為第200 s、第400 s、第600 s、第800 s、第1 000 s。5組數據分別為：-1 000 m、-1 000 m、-1 000 m；-800 m、-800 m、-800 m；1 000 m、1 000 m、1 000 m；800 m、800 m、800 m；500 m、500 m、500 m。仿真結果如圖1和圖2所示。

圖1 兩種算法位置誤差比較

圖2 兩種算法速度誤差比較

對比兩組算法仿真結果可知，基于M估計的強跟蹤SVD-UKF濾波算法在衛星加入異常數據后仍能夠不受異常數據干擾，濾波系統魯棒性增強[12]。其中，強跟蹤SVD-UKF濾波算法在遇到衛星野值后無法克服野值干擾，在加入異常數據的時間點均出現較大誤差。在遇到異常新息數據后，強跟蹤SVD-UKF濾波算法無法判斷數據是否異常。由于強跟蹤濾波框架的新息正交性，多重漸消因子影響協方差矩陣使系統后驗狀態估計值傾向于新息數據，使得濾波系統輸出估計產生較大誤差。由于多重漸消因子利用歷史殘差數據統計，異常數據對多重漸消因子影響一直存在。故仿真結果中，異常數據對強跟蹤SVD-UKF濾波算法的影響隨著多重漸消因子慢慢遺忘歷史數據漸漸消失?；贛估計的強跟蹤SVD-UKF濾波算法在遇到異常數據時，通過M估計辨識出異常后，等價權因子降低了異常數據的影響。在本次仿真中，5組異常數據經過M估計后等價權因子均降為0，使得異常數據并未對濾波系統造成干擾，提高了濾波系統魯棒性[13]。

(2)衛星多粗差數據異常對比。在仿真模型中第45 s時加入持續100 s均值為0、強度為50的白噪聲干擾。仿真結果如圖3和圖4所示。

圖3 兩種算法位置誤差比較

圖4 兩種算法速度誤差比較

對比兩組算法仿真結果可知，基于M估計的強跟蹤SVD-UKF濾波算法在衛星加入持續異常數據仍

能保證濾波系統不受干擾。而強跟蹤SVD-UKF濾波算法在新息數據持續異常的時間段內亦產生較大誤差?；贛估計的強跟蹤SVD-UKF濾波算法異常數據殘差較大時，等價權因子權值為0，剔除異常干擾；在異常數據殘差在調整區時，等價權因子權值根據先驗統計降低新息權值，減少異常數據對系統產生的影響[14]?；贛估計的強跟蹤SVD-UKF濾波算法在多粗差情況下依然提高了系統魯棒性[15]。

3 結束語

本文提出一種基于M估計的強跟蹤SVD-UKF算法。該算法利用M估計理論，采用等價權因子對異常新息數據進行“篩選”，最大程度保留有效新息，剔除有害新息，避免由衛星信號野值引起的粗差對強跟蹤SVD-UKF算法的魯棒性影響。將該算法應用于組合導航系統仿真實驗，在仿真模型中加入單粗差、多粗差數據與強跟蹤SVD-UKF算法進行驗證，仿真結果表明，基于M估計的強跟蹤SVD-UKF算法有效提高了系統在新息數據異常時的魯棒性，證明了本文方法的可行性和有效性。