蘇天祥，文援蘭，朱 俊

1.61892部隊，廣東 汕頭515071；2.國防科技大學航天科學與工程學院，湖南 長沙410072；3.宇航動力學國家重點實驗室，陜西 西安710043

1 引 言

針對誤差類別不同而衍生出的自適應濾波、抗差濾波和抗差自適應濾波等不同濾波算法，國內(nèi)外很多學者進行了廣泛研究，并取得了顯著成果［1－5］。對動力學模型信息不精確和觀測模型出現(xiàn)誤差時，直接利用Kalman算法濾波效果不佳，通常利用自適應濾波對其狀態(tài)進行估計。自適應濾波是引入一個調(diào)節(jié)因子來均衡調(diào)節(jié)動力學模型信息不精確和觀測模型誤差對濾波估計值的影響［6］。針對狀態(tài)估計量類型的不同又衍生出多因子和分類因子自適應濾波，其實質(zhì)都是單因子自適應濾波的推廣，因為這些濾波均采用單個因子來均衡調(diào)節(jié)動力學模型和觀測模型誤差對估值的影響。分類因子算法主要在處理不同類型（類型不同、數(shù)量級也可能不同）的狀態(tài)量上有優(yōu)勢［7－9］，多因子其實是將每個因子作為分類因子處理的特例［10］?？共钭赃m應濾波算法較為均衡地解決了觀測模型誤差和動力學模型誤差對狀態(tài)參數(shù)估計的影響，其主要步驟為先求解狀態(tài)參數(shù)抗差解，然后根據(jù)狀態(tài)參數(shù)抗差解求出自適應因子，最后根據(jù)自適應因子解算狀態(tài)參數(shù)［5－6］。

本文基于抗差自適應Kalman濾波算法，引入兩個自適應因子分別調(diào)節(jié)模型誤差和觀測誤差對狀態(tài)估計的影響，首先分別解算兩個自適應因子，然后由兩個自適應因子共同解算最終狀態(tài)參數(shù)解［11］。

2 濾波模型

2.1 狀態(tài)方程推導

對于離散系統(tǒng)如下［12－16］

狀態(tài)方程

觀測方程

式中，Xk為k時刻的狀態(tài)量向量；Φ（k，k－1）為狀態(tài)轉(zhuǎn)移矩陣；Γ（k，k－1）為系統(tǒng)噪聲矩陣；wk－1為系統(tǒng)噪聲向量；Lk為k時刻的觀測向量；Ak為觀測矩陣；ek為觀測噪聲向量。

狀態(tài)向量預報誤差方程

觀測向量誤差方程

式中，Lk觀測向量信息；Vk反映觀測值的誤差。

按最小二乘原理構造如下?lián)p失函數(shù)［11，18］

式（5）對求導，并令其導數(shù)為0，得

由矩陣恒等變換可得

2.2 自適應因子選取

動力學模型信息自適應因子βk和觀測模型自適應因子αk可參照抗差估計方法中指數(shù)型兩段法對其進行計算［5］，其中

2.2.1αki自適應因子選?。?9－22］

式（4）反映了觀測殘差，現(xiàn)對其第i項分量Vki進行標準化處理，即是Vki的均方差［6］，取λα為閾值，其范圍是λα∈［1.0，3.0］

2.2.2βki自適應因子選取

式（3）反映了模型信息的精確性，將式（7）代入式（3），則

第1種方案：取式（9）為狀態(tài)的預測殘差。

對狀態(tài)預測殘差第i項分量~VˉXki進行標準化處理，即的均方差［6］，取λβ為閾值，其范圍是λβ∈ ［1.0，3.0］

3 仿真場景

設一輛汽車在公路上行駛，在車上安裝兩個路程傳感器A和B，傳感器A的路程測量精度為5m（1σ）；傳感器B的路程測量精度1m（1σ）；t表示時間，單位為秒；S表示位移，單位為m；v表示速度，單位為 m／s；a表示加速度，單位為 m／s2。汽車兩種工作模式如下：

（1）在t∈ ［0，1500］期間，汽車以v＝20m／s勻速直線運動。

（2）在t∈ ［0 ，5 00）期間，汽車以v＝20m／s的速度勻速直線運動，汽車中途在t∈［50 0 ，550）以a＝1m／s2的加速度作勻加速運動，在t∈ ［55 0 ，1500］期間，汽車以v＝20m／s的速度勻速直線運動。其中工作模式（2）中汽車在t∈ ［50 0 ，550）作勻加速運動是模擬動力學模型不準對估計的影響。

圖1 汽車工作模式圖Fig.1 The figure of the work mode of the car

方案1：用標準Kalman濾波算法估計汽車位移S，簡稱SKF。

方案3：用抗差自適應Kalman濾波算法［1］估計位移S，簡稱RAF。

方案4：采用非簡化預測殘差的雙自適應子濾波算法估計位移S，簡稱DAF。

對上述汽車工作模式進行建模，設汽車狀態(tài)位置矢量S，速度矢量v，t0為初始時刻，S0為t0時刻的位移，則汽車的狀態(tài)量設為X＝Sv［］T，CV模型如下

則狀態(tài)轉(zhuǎn)移矩陣為

狀態(tài)方程為Xk＝Φ（k，k－1）Xk－1＋Wk，其中Wk為模型誤差。

觀測方程Yk＝Axk＋Vk，其中A＝［1 0］，Vk為測量誤差。

時間更新如下

測量更新如下

4 結(jié)果分析

方案2、方案3和方案4中自適應因子的比較門限值采用相同設置，即觀測自適應因子λα＝2.5，動力學模型信息自適應因子λβ＝1.5。

算例1：汽車整個過程處在工作模式1下。此算例主要用于驗證動力學模型信息和觀測模型誤差均正常時，雙自適應因子濾波算法在通常的情況下是否與標準Kalman濾波一致，4種算法的性能如圖2—圖5所示。

算例2：汽車工作在模式1下，每300s給傳感器A加入300m粗差。此算例主要是驗證觀測模型發(fā)生異常、動力學模型信息正確時，雙因子自適應濾波算法是否能夠抵御觀測粗差對狀態(tài)估計的影響，4種算法的性能如圖6—圖9所示。

圖2 標準Kalman估計誤差Fig.2 The estimation error of the standard Kalman

圖3 SDF估計誤差Fig.3 The estimation error of SDF

圖4 RAF估計誤差Fig.4 The estimation error of RAF

圖5 DAF估計誤差Fig.5 The estimation error of DAF

圖6 標準Kalman估計誤差Fig.6 The estimation error of standard Kalman

算例3：汽車在模式2下工作，中間有50s的加速運動，并且每300s對傳感器A加入300m的粗差。此算例用于驗證模型信息和觀測均存在異常時，雙因子自適應濾波算法是否能夠抵御兩種粗差對狀態(tài)估計的影響，能否達到抗差自適應算法的性能，4種算法的性能如圖10—圖13所示。

圖7 SDF估計誤差Fig.7 The estimation error of SDF

圖8 RAF估計誤差Fig.8 The estimation error of RAF

圖9 DAF估計誤差Fig.9 The estimation error of DAF

表1 4種算法在不同算例下的RMSTab.1 The RMS of the four algorithms in different work modes m

圖10 標準Kalman估計誤差Fig.10 The estimation error of standard Kalman

圖11 SDF估計誤差Fig.11 The estimation error of SDF

圖12 RAF估計誤差Fig.12 The estimation error of RAF

由上面的仿真結(jié)果可以得出以下結(jié)論：

（1）標準Kalman濾波算法易受到誤差的擾動出現(xiàn)較大波動。尤其是當動力學模型信息不精確時，標準Kalman濾波則會出現(xiàn)較大波動，見圖2、圖6和圖10。

（2）簡化殘差的雙自適應因子濾波算法性能比較穩(wěn)定，但是由于簡化殘差較大，對誤差的變化較為敏感，因此簡化殘差的雙自適應因子濾波算法表現(xiàn)出較為頻繁的波動特性，見圖3、圖7和圖11。

圖13 DAF估計誤差Fig.13 The estimation error of DAF

（3）抗差自適應算法和雙自適應因子算法性能較為穩(wěn)定，不僅能抑制觀測粗差對估值的影響，而且能夠較好地削弱動力學模型不精確對估值的影響，見圖4、圖5、圖8、圖9、圖12和圖13。由表1可以看出，單就RMS來說，雙自適應因子濾波算法的RMS要略低于抗差自適應濾波算法。

5 結(jié) 論

本文提出了雙自適應因子濾波算法，通過兩個自適應因子分別調(diào)節(jié)動力學模型誤差和觀測模型誤差對濾波估值的影響，理論上推導了雙自適應因子算法的濾波模型，并進行了建模和仿真驗證，結(jié)果表明該算法具有同時抵制觀測異常和動力學模型異常影響的能力。但該算法適用于動力學模型短時間出現(xiàn)小范圍異常誤差，若動力學模型長時間不準確或者動力學模型異常誤差過大，則可能導致濾波發(fā)散。

［1］ ZHOU Jiangwen.Classical Theory of Errors and Robust Estimation［J］.Acta Geodaetica et Cartographica Sinica，1989，18（2）：115－120.（周江文.經(jīng)典誤差理論和抗差估計［J］.測繪學報，1989，18（2）：115－120.）

［2］ YANG Yuanxi.Adaptively Robust Least Squares Estimation［J］.Acta Geodaetica et Cartographica Sinica，1996，25（3）：205－211.（楊元喜.自適應抗差最小二乘估計［J］.測繪學報，1996，25（3）：205－211.）

［3］ YANG Yuanxi.Robust Estimation for Dependent Observations［J］.Manuscripta Geodaetica，1994，19：10－17.

［4］ YANG Yuanxi，HE Haibo，XU Tianhe.Adaptive Robust Filtering for Kinematic GPS Positioning ［J］. Acta Geodaetica et Cartographica Sinica，2001，30（4）：293－298.（楊元喜，何海波，徐天河.論動態(tài)自適應濾波［J］.測繪學報，2001，30（4）：293－298.）

［5］ YANG Yuanxi，REN Xia，XU Yan.Major Progress in Adaptive Robust Filtering Theory and Application［J］.Navigation and Positioning，2013，1：9－15.（楊元喜，任夏，許艷.自適應抗差濾波理論及應用的主要進展［J］.導航定位學報，2013，1：9－15.）

［6］ YANG Yuanxi.Adaptive Navigation and Kinematic Positioning ［M］.Beijing：Surveying and Mapping Press，2006.（楊元喜.自適應動態(tài)導航定位［M］.北京：測繪出版社，2006.）

［7］ CUI Xianqiang，YANG Yuanxi.Adaptively Robust Filtering with Classified Adaptive Factors［J］.Progress in Nature Science，2006，16（8）：846－851.

［8］ HUANG Guanwen，YANG Yuanxi，ZHANG Qin.Estimate and Predict Satellite Clock Error Using Adaptively Robust Sequential Adjustment with Classified Adaptive Factors Based on Opening Windows［J］.Acta Geodaetica et Cartographica Sinica，2011，40（1）：15－21.（黃觀文，楊元喜，張勤.開窗分類因子抗差自適應序貫平差用于衛(wèi)星鐘差參數(shù)估計與預報［J］.測繪學報，2011，40（1）：15－21.）

［9］ ZHANG Liping，ZENG Anmin.Determination of Coordinate Transformation Parameters by Adaptively Adjustment Method with Classified Adaptive Factors ［J］.Geodesy and Geodynamics，2008，28（2）：110－114.（張麗萍，曾安敏.用分類因子自適應序貫平差法確定坐標轉(zhuǎn)換參數(shù)［J］.大地測量與地球動力學，2008，28（2）：110－114.）

［10］ YANG Yuanxi，CUI Xianqiang.Adaptively Robust Filter with Multi Adaptive Factors［J］.Survey Review，2008，40（309）：260－270.

［11］ SU Tianxiang.The Rresearch of Autonomous Orbit Determination of Navigation Constellation with Distribution Algorithm Based on Ground－based Anchor［D］.Changsha：National University of Defense Technology，2012.（蘇天祥.基于錨固站的導航星座自主定軌技術研究［D］.長沙：國防科技大學，2012.）

［12］ LIU Jun，XIA Zhixun.Dynamics System Identification and Modeling［M］.Changsha：Press of National University of Defense Technology，2007.（劉君，夏智勛.動力學系統(tǒng)辨識與建模［M］.長沙：國防科技大學出版社，2007.）

［13］ HUBER.Robust Statistics［M］.New York：John Wiley，1981.

［14］ WU Fumei，YANG Yuanxi.An Extended Adaptive Kalman Filtering in Tight Coupled GPS／INS Integration［J］.Survey Review，2010，40（316）：146－154.

［15］ YANG Yuanxi，GAO Weiguang，Zhang Shuangcheng.Robust Kalman Filtering with Constraints：a Case Study for Integrated Navigation［J］.Journal of Geodesy，2010，84：373－381.

［16］ YANG Yuanxi.Properties of Adaptive Robust Filtering for Kinematic GPS Positioning［J］.Acta Geodaetica et Cartographica Sinica，2003，32（3）：189－192.（楊元喜.動態(tài)定位自適應濾波解的性質(zhì)［J］.測繪學報，2003，32（3）：189－192.）

［17］ LI Chong，HUANG Guanwen，TAN Li，et al.Adaptive Robust Kalman Filtering Applied in GPS Precise Point Positioning［J］.Science of Surveying and Mapping，2011，36（4）：22－24.（李沖，黃觀文，譚理等.抗差自適應卡爾曼濾波在GPS精密單點定位中的應用［J］測繪科學，2011，36（4）：22－24.）

［18］ WU Fumei，YANG Yuanxi.A New Two Step Adaptive Robust Kalman Filtering in GPS／INS Integrated Navigation System［J］.Acta Geodaetica et Cartographica Sinica，2010，39（5）：522－527.（吳富梅，楊元喜.一種兩步自適應抗差Kalman濾波在GPS／INS組合導航中的應用［J］.測繪學報，2010，39（5）：522－527.）

［19］ GAO Weiguang，YANG Yuanxi，ZHANG Shuangcheng.Adaptive Robust Kalman Flitering Based on the Current Statistical Model［J］.Acta Geodaetica et Cartographica Sinica，2006，35（1）：15－19.（高為廣，楊元喜，張雙成.基于當前加速度模型的抗差自適應KALMAN濾波［J］.測繪學報，2006，35（1）：15－19.）

［20］ YANG Yuanxi，GAO Weiguang.Comparison of Two Fading Filtering and Adaptively Robust Filter［J］.Geometrics and Information Science of Wuhan University，2006，31（11）：980－982.（楊元喜，高為廣.兩種漸消濾波與自適應抗差濾波的綜合比較分析［J］.武漢大學學報：信息科學版，2006，31（11）：980－982.）

［21］ GAO Weiguang，ZHANG Shuangcheng，WANG Fei，et al.Adaptive Robust Kalman Filtering Algorithm in GPS Navigation［J］.Science of Surveying and Mapping，2005，30（2）：98－100.（高為廣，張雙成，王飛，等.GPS導航中的抗差自適應Kalman濾波算法［J］.測繪科學，2005，30（2）：98－100.）