基于牛頓插值的抗差卡爾曼濾波算法

蔡保杰，邵 雷，李佳偉，李正杰

（1.空軍工程大學 防空反導學院，西安 710051；2.西安電子科技大學 電子工程學院，西安 710071）

0 引言

導航系統(tǒng)在航空航天和日常生活中應用廣泛。導航的方法有很多種，慣性導航和衛(wèi)星導航的組合導航的方法是最為常用的1 種[1]。在運載體運動過程中，由于所處環(huán)境錯綜復雜，衛(wèi)星導航的觀測值會出現損壞的情況，嚴重影響導航的精度。

對于觀測值故障問題[2-3]，常用的是傳統(tǒng)抗差卡爾曼（Kalman）濾波方法[4]，文獻[5]中構造了1 個故障檢驗公式，與設定的卡方檢驗閾值相比較，對檢驗值進行分段處理。文獻[6]先用假設檢驗的方法檢測出故障觀測值的具體位置，用設定的門限值代替觀測噪聲值，可以較大程度地減小粗差的影響。文獻[7]在一步預測均方誤差陣上乘1 個漸消因子，進行自適應濾波。文獻[8]通過降低濾波增益值來達到修正預測值的效果。文獻[9]提出錯誤預警率和故障探測率的概念，對故障的定位更加精確。文獻[10]通過等價權函數法對觀測噪 聲方差陣進行替換，可以實時有效地濾除粗差。

上述方法都能一定程度地減小粗差帶來的影響，但也存在不足之處?？ǚ綑z驗并不能保證百分之百的正確率，適當地選取閾值尤為重要。當檢測系統(tǒng)出現誤檢情況時，用門限值代替正確的觀測值也會造成觀測值誤差相對增大。針對這種情況，本文提出1 種基于卡方檢驗和牛頓插值的抗差濾波方法，利用前幾個時刻的觀測值組成的牛頓插值多項式進行外延，對當前時刻的故障觀測值進行替換，以使組合導航系統(tǒng)穩(wěn)定性更高，抗差能力更強。

1 組合導航系統(tǒng)建模與常用的抗差濾波方法概述

1.1 組合導航系統(tǒng)建模

導航系統(tǒng)能夠測量并解算出運載體的瞬時運動狀態(tài)和位置，提供給駕駛員或自動駕駛儀，以實現對運載體的正確操縱或控制。隨著科學技術的發(fā)展，可供導航系統(tǒng)利用的信息源越來越多，導航系統(tǒng)種類也越來越多。捷聯慣導(strapdown inertial navigation system， SINS)和全球衛(wèi)星導航系統(tǒng)(global navigation satellite system， GNSS)所構成的組合導航是近年來導航領域最常使用的1 種方法，SINS 作為主導航系統(tǒng)，利用慣性器件實時解算出運載體的位置、速度等信息，而GNSS 也可以對SINS 不斷地進行修正，避免了捷聯慣導系統(tǒng)誤差隨時間增大的問題。

SINS/GNSS 的組合方式有松組合[11-13]、緊組合、深組合等。松組合的狀態(tài)量為

式中： δL 為經度誤差； δλ 為緯度誤差； δ h為高度誤 差；δ v = [ δ vx， δ vy， δvz]為 速度誤差；φ =[φx，φy，φz]為 姿 態(tài) 角 誤 差； ε=[εx， εy，εz]為 陀 螺 儀 漂 移 誤 差?=[?x，?y，?z]為 加 速 度 計 零 偏 誤 差。其 狀 態(tài) 方 程離散化后的形式為

式中： Ak,k-1為狀態(tài)轉移矩陣；W 為系統(tǒng)噪聲。當狀態(tài)向量為15 維時， Ak,k-1是1 個15 維的方陣，其非零項為： 其中：ωie為地球自轉角速度；fx、fy、fz為加速度計測得的比力信息；R 為地球半徑。SINS/GNSS 的觀測模型取GNSS 和SINS 輸出的位置和速度之差作為 觀 測 值，觀 測 向 量 為 Zk= [rGNSS- rSINS，vGNSS-vSINS]T。其中： r =[ L， λ， h]； v =[ vx， vy， vz]。

量測方程為

式中： Hk為量測矩陣；Vk為量測噪聲。 Hk為1 個6 行15 列的矩陣，其非零元素為： H1，2=R cos L ;H2，1=R; H4，4=1; H5，5=1; H6，6=1。

1.2 2 種常用的抗差濾波方法

1.2.1 等價權函數法

等價權函數法[9,14]為調整觀測噪聲協方差矩陣 kR 的大小，抗差卡爾曼濾波在第k 時刻的估計值可以表示為

而式（4）中濾波增益為

式中： Pk，k-1為一步預測均方誤差陣；Rk為 Rk的等價權矩陣，而Rk可以通過丹麥法、胡貝爾（Huber）函數或通過中國科學院測量與地球物理研究所(Institute of Geodesy & Geophysics，Chinese Academy of Sciences， IGG)方案獲得，即

式中：M、N 為常值； ，1kkZ-? 為標準化殘差。此方法可以直接利用觀測值作為評判標準，計算量小，易于實現，但若系統(tǒng)未能檢測到小周跳，仍會有較大誤差出現。

1.2.2 卡方檢驗法

卡爾曼濾波是1 種線性最小方差估計，是1 個遞推的過程，而在每1 步遞推的過程中，殘差是1 個很重要的信息來源，它包含有1 步預測誤差信息。當導航系統(tǒng)發(fā)生故障時，殘差序列會有異常變化。利用殘差序列的這個特性，可以對故障觀測值進行檢測。此方法分2 步進行，即先對故障進行檢測，再對有故障的觀測值進行處理?？ǚ綑z驗的方法在計算上較為復雜，但其檢測精度較高，故本文 采用卡方檢驗的方法。

2 基于牛頓插值的抗差卡爾曼濾波算法

2.1 采用卡方檢驗的故障檢測方法

2.1.1 卡方檢驗方法概述

由1 步預測值 Xk，k-1代替真實值 Xk引起的誤差為： X?k，k-1= Xk-X?k，k-1。1 步預測與觀測值相比較的差值為

式中kS 為殘差序列的協方差矩陣，即

設 H0為原假設， H1為備選假設， H0、 H1分別定義為

式中：n 為殘差序列的維數；γ 為非中心化參數。取顯著性水平為α，α 的大小直接決定了誤差的判定條件的寬松程度。設檢測量容許的最大值為T ( n )， T ( n) =( n)，一般將 T ( n )稱為閾值，當檢驗量的值超過閾值 T ( n )時，認為發(fā)生故障[15-16]。

載體運動的過程中，引起觀測值損壞的因素有很多，當出現鏈路中斷等情況時，會造成當前時刻的觀測值全部損壞；但有時可能只是因為個別儀器儀表的故障，并不會使觀測值全部失效，這時若將當前時刻全部觀測值當作故障值顯然不妥，對導航精度也會造成一定影響。所以本文采用單個觀測值的故障檢測系統(tǒng)。設k 時刻第i 個觀測值為 Dk( i )，這時的原假設和備選假設可以定義為

令 T (1)= χ2(1 ) ，當統(tǒng)計量 Dk( i ) ＜χ2(1 ) 時，認為系統(tǒng)無故障；當 Dk( i ) ＞ χ2(1 ) 時，認為導航系統(tǒng)觀測值存在故障。

2.1.2 閾值的選取

在實際的運動過程中，載體所處的環(huán)境是比較復雜的，對觀測值故障的容忍程度也會變大，所以選取的閾值也相對較大。當閾值選取過小時，會把一些正確的觀測值當作故障觀測值來處理，隨著時間的推移會出現累積誤差；當閾值選取過大時，被認定為出現故障的觀測值就會變得很少，系統(tǒng)幾乎檢測不到故障，使抗差濾波之前和之后的曲線接近重合。

表1 為顯著性水平α 和 ( )χ21 的分布表。

表1 顯著性水平和閾值的關系

由表1 可知，顯著性水平α 越小， χ2(1 ) 的值越大，故障的判定條件就越寬松。本文通過大量的實驗分析，發(fā)現當顯著性水平α 為0.25～0.01、閾值T (1)為5.024～6.635 時可以取得較好的效果，如圖1所示?？ǚ綑z驗統(tǒng)計量分布在6.635 以下的概率為 96.8%，以閾值為6.635 作為判定條件可以較為精確地檢測到故障，誤檢的概率較小，所以本文選取6.635 作為閾值進行仿真驗證。

圖1 卡方檢驗統(tǒng)計量分布

2.2 利用牛頓插值修正觀測值

插值法[17]是利用已知函數 f ( x )某個區(qū)間的若干點，構造1 個近似函數 P ( x )來近似代替 f ( x )，牛頓插值可以根據計算精度靈活增加插值節(jié)點進行遞推運算，公式形式對稱、結構緊湊，容易編寫程序。本文采用的牛頓插值方法是將近似函數P ( x )進行1 個單位的外延。

經典組合導航系統(tǒng)中，每次濾波過程的間隔時間一般是固定的，可以使用等距的插值公式，其表達式[18]為

據式（13）便可用前n 個時刻的函數值對當前時刻函數值進行插值。

一般來說，插值節(jié)點取得越多，利用牛頓插值構造的曲線與原函數曲線近似程度越高，誤差越小，適當增加節(jié)點數對計算結果的準確度是有幫助的，但并非插值多項式的次數越高越好。當插值節(jié)點增多時，不能保證非節(jié)點處的插值精度得到改善，有時反而誤差更大。龍格現象表明，大范圍內使用高次插值，逼近的效果往往是不理想的。根據大量實驗驗證，牛頓插值在4 階時精度較高，出現龍格現象的概率也比較小，所以本文將以4 階牛頓插值多項式來進行實驗驗證?；诳ǚ綑z驗和牛頓插值的抗差濾波算法[13]為：

1）計算狀態(tài)量的1 步預測值

2）計算1 步預測的均方誤差值

3）計算濾波增益值

4）計算估計均方誤差待下1 時刻使用

仿真步長為1 s，共用時500 s，為營造與飛機飛行相近的環(huán)境，人為地給觀測值加入噪聲，加入噪聲的數值如表3 所示。

表3 隨機噪聲值

5）根據殘差向量計算當前時刻 kD 值

6）確定觀測值

式 中 βk=Diag[β1， β2,… ,βn](i =1， 2， … , n)。

7）得到估計值

3 仿真分析及驗證

3.1 仿真軌跡生成

本文模擬1 條飛機飛行軌跡來驗證抗差濾波算法的有效性，飛行軌跡如圖2 所示，將模擬的軌跡數據送入軌跡發(fā)生器，分別得到慣性導航系統(tǒng)(inertial navigation system， INS）和全球定位系統(tǒng)(global positioning system， GPS）的數據，各誤差值如表2 所示。

為驗證本抗差算法的有效性，制造量測不準確的外部環(huán)境，仿真在每隔10 s 的時刻加入1 個噪聲放大系數m，使此時刻的噪聲含有m 倍粗差。

3.2 分析與對比

本文對比使用上述抗差方法和不使用抗差方法的位置和速度誤差，如圖3、圖4 所示。

圖3 抗差濾波前后位置誤差

圖4 抗差濾波前后速度誤差

考慮到當GNSS 信號受到影響時，INS 仍然處于工作狀態(tài)，分析對比了使用本文提出的抗差方法和僅使用INS 的誤差，如圖5、圖6 所示。

圖5 只使用慣導和使用抗差濾波后位置誤差

從圖3、圖4 可以看出，當不使用任何抗差方法時，由于GNSS 信號誤差非常大，造成觀測值發(fā)生急劇變化，如表4 所示。在使用本文提出的基于牛頓插值的抗差濾波方法后，對GNSS 所收到的觀測值進行改良，使觀測值恢復到正常范圍，使位置和速度誤差大大減小，如表5 所示。

慣性導航誤差是隨著時間逐漸增大的，沒有衛(wèi)星導航的實時修正，慣性導航方法無論是位置誤差還是速度誤差都會慢慢增大，與此相比，本文提出的基于牛頓插值的抗差濾波方法能較好地使誤差維持在1 個較小的范圍內，使導航精度得到提高，如圖5、圖6 所示。但不可否認的是，慣性導航在獨立工作時間較短時，導航精度仍有一定的準確性。與此相比，由于在導航數據已有誤差的前提下又人為加入噪聲，致使使用本文提出的抗差方法在短時間內導航精度比只使用慣性導航精度略低，北向速度誤差和天向速度誤差在個別點處出現較大波動，如圖6(b)、圖6(c)所示。但北向速度誤差值仍在-1.78～0.57 m/s，天向速度誤差仍在-0.62～0.59 m/s 這樣1 個較小的范圍內，且隨著時間的推移，慣性導航誤差增大的現象慢慢顯現出來。綜合對比來看，基于牛頓插值的抗差卡爾曼濾波新算法仍不失為1 種有效的抗差濾波方法。

表4 抗差濾波前誤差值

表5 抗差濾波后誤差值

5 結束語

本文對 INS/GNSS 導航系統(tǒng)在運動過程中GNSS 信號會出現故障問題，提出1 種基于牛頓插值的抗差卡爾曼濾波算法。該算法通過當前時刻單個觀測值的卡方檢驗方法，通過大量實驗選取合適的閾值，對故障觀測值進行更精確的定位。用過去幾個時刻的觀測值組成牛頓插值多項式進行外延，得到當前時刻觀測值，給出了基于牛頓插值的抗差卡爾曼濾波方法的具體過程。最后，通過大量的仿真分析進行驗證，該算法使誤差控制在1 個較小的范圍內，具有較好的抗差效果。該算法提高了抗差濾波系統(tǒng)的穩(wěn)定性，計算簡單，易于工程實現。