雙自適應(yīng)因子濾波算法

蘇天祥，文援蘭，朱 俊

1.61892部隊(duì)，廣東 汕頭515071；2.國(guó)防科技大學(xué)航天科學(xué)與工程學(xué)院，湖南 長(zhǎng)沙410072；3.宇航動(dòng)力學(xué)國(guó)家重點(diǎn)實(shí)驗(yàn)室，陜西 西安710043

1 引 言

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

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

2 濾波模型

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

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

狀態(tài)方程

觀測(cè)方程

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

狀態(tài)向量預(yù)報(bào)誤差方程

觀測(cè)向量誤差方程

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

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

式（5）對(duì)求導(dǎo)，并令其導(dǎo)數(shù)為0，得

由矩陣恒等變換可得

2.2 自適應(yīng)因子選取

動(dòng)力學(xué)模型信息自適應(yīng)因子βk和觀測(cè)模型自適應(yīng)因子αk可參照抗差估計(jì)方法中指數(shù)型兩段法對(duì)其進(jìn)行計(jì)算［5］，其中

2.2.1αki自適應(yīng)因子選?。?9－22］

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

2.2.2βki自適應(yīng)因子選取

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

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

對(duì)狀態(tài)預(yù)測(cè)殘差第i項(xiàng)分量~VˉXki進(jìn)行標(biāo)準(zhǔn)化處理，即的均方差［6］，取λβ為閾值，其范圍是λβ∈ ［1.0，3.0］

3 仿真場(chǎng)景

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

（1）在t∈ ［0，1500］期間，汽車以v＝20m／s勻速直線運(yùn)動(dòng)。

（2）在t∈ ［0 ，5 00）期間，汽車以v＝20m／s的速度勻速直線運(yùn)動(dòng)，汽車中途在t∈［50 0 ，550）以a＝1m／s2的加速度作勻加速運(yùn)動(dòng)，在t∈ ［55 0 ，1500］期間，汽車以v＝20m／s的速度勻速直線運(yùn)動(dòng)。其中工作模式（2）中汽車在t∈ ［50 0 ，550）作勻加速運(yùn)動(dòng)是模擬動(dòng)力學(xué)模型不準(zhǔn)對(duì)估計(jì)的影響。

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

方案1：用標(biāo)準(zhǔn)Kalman濾波算法估計(jì)汽車位移S，簡(jiǎn)稱SKF。

方案3：用抗差自適應(yīng)Kalman濾波算法［1］估計(jì)位移S，簡(jiǎn)稱RAF。

方案4：采用非簡(jiǎn)化預(yù)測(cè)殘差的雙自適應(yīng)子濾波算法估計(jì)位移S，簡(jiǎn)稱DAF。

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

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

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

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

時(shí)間更新如下

測(cè)量更新如下

4 結(jié)果分析

方案2、方案3和方案4中自適應(yīng)因子的比較門限值采用相同設(shè)置，即觀測(cè)自適應(yīng)因子λα＝2.5，動(dòng)力學(xué)模型信息自適應(yīng)因子λβ＝1.5。

算例1：汽車整個(gè)過(guò)程處在工作模式1下。此算例主要用于驗(yàn)證動(dòng)力學(xué)模型信息和觀測(cè)模型誤差均正常時(shí)，雙自適應(yīng)因子濾波算法在通常的情況下是否與標(biāo)準(zhǔn)Kalman濾波一致，4種算法的性能如圖2—圖5所示。

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

圖2 標(biāo)準(zhǔn)Kalman估計(jì)誤差Fig.2 The estimation error of the standard Kalman

圖3 SDF估計(jì)誤差Fig.3 The estimation error of SDF

圖4 RAF估計(jì)誤差Fig.4 The estimation error of RAF

圖5 DAF估計(jì)誤差Fig.5 The estimation error of DAF

圖6 標(biāo)準(zhǔn)Kalman估計(jì)誤差Fig.6 The estimation error of standard Kalman

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

圖7 SDF估計(jì)誤差Fig.7 The estimation error of SDF

圖8 RAF估計(jì)誤差Fig.8 The estimation error of RAF

圖9 DAF估計(jì)誤差Fig.9 The estimation error of DAF

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

圖10 標(biāo)準(zhǔn)Kalman估計(jì)誤差Fig.10 The estimation error of standard Kalman

圖11 SDF估計(jì)誤差Fig.11 The estimation error of SDF

圖12 RAF估計(jì)誤差Fig.12 The estimation error of RAF

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

（1）標(biāo)準(zhǔn)Kalman濾波算法易受到誤差的擾動(dòng)出現(xiàn)較大波動(dòng)。尤其是當(dāng)動(dòng)力學(xué)模型信息不精確時(shí)，標(biāo)準(zhǔn)Kalman濾波則會(huì)出現(xiàn)較大波動(dòng)，見(jiàn)圖2、圖6和圖10。

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

圖13 DAF估計(jì)誤差Fig.13 The estimation error of DAF

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

5 結(jié) 論

本文提出了雙自適應(yīng)因子濾波算法，通過(guò)兩個(gè)自適應(yīng)因子分別調(diào)節(jié)動(dòng)力學(xué)模型誤差和觀測(cè)模型誤差對(duì)濾波估值的影響，理論上推導(dǎo)了雙自適應(yīng)因子算法的濾波模型，并進(jìn)行了建模和仿真驗(yàn)證，結(jié)果表明該算法具有同時(shí)抵制觀測(cè)異常和動(dòng)力學(xué)模型異常影響的能力。但該算法適用于動(dòng)力學(xué)模型短時(shí)間出現(xiàn)小范圍異常誤差，若動(dòng)力學(xué)模型長(zhǎng)時(shí)間不準(zhǔn)確或者動(dòng)力學(xué)模型異常誤差過(guò)大，則可能導(dǎo)致濾波發(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ì)［J］.測(cè)繪學(xué)報(bào)，1989，18（2）：115－120.）

［2］ YANG Yuanxi.Adaptively Robust Least Squares Estimation［J］.Acta Geodaetica et Cartographica Sinica，1996，25（3）：205－211.（楊元喜.自適應(yīng)抗差最小二乘估計(jì)［J］.測(cè)繪學(xué)報(bào)，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.（楊元喜，何海波，徐天河.論動(dòng)態(tài)自適應(yīng)濾波［J］.測(cè)繪學(xué)報(bào)，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.（楊元喜，任夏，許艷.自適應(yīng)抗差濾波理論及應(yīng)用的主要進(jìn)展［J］.導(dǎo)航定位學(xué)報(bào)，2013，1：9－15.）

［6］ YANG Yuanxi.Adaptive Navigation and Kinematic Positioning ［M］.Beijing：Surveying and Mapping Press，2006.（楊元喜.自適應(yīng)動(dòng)態(tài)導(dǎo)航定位［M］.北京：測(cè)繪出版社，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.（黃觀文，楊元喜，張勤.開(kāi)窗分類因子抗差自適應(yīng)序貫平差用于衛(wèi)星鐘差參數(shù)估計(jì)與預(yù)報(bào)［J］.測(cè)繪學(xué)報(bào)，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.（張麗萍，曾安敏.用分類因子自適應(yīng)序貫平差法確定坐標(biāo)轉(zhuǎn)換參數(shù)［J］.大地測(cè)量與地球動(dòng)力學(xué)，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ǎo)航星座自主定軌技術(shù)研究［D］.長(zhǎng)沙：國(guó)防科技大學(xué)，2012.）

［12］ LIU Jun，XIA Zhixun.Dynamics System Identification and Modeling［M］.Changsha：Press of National University of Defense Technology，2007.（劉君，夏智勛.動(dòng)力學(xué)系統(tǒng)辨識(shí)與建模［M］.長(zhǎng)沙：國(guó)防科技大學(xué)出版社，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.（楊元喜.動(dòng)態(tài)定位自適應(yīng)濾波解的性質(zhì)［J］.測(cè)繪學(xué)報(bào)，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.（李沖，黃觀文，譚理等.抗差自適應(yīng)卡爾曼濾波在GPS精密單點(diǎn)定位中的應(yīng)用［J］測(cè)繪科學(xué)，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.（吳富梅，楊元喜.一種兩步自適應(yīng)抗差Kalman濾波在GPS／INS組合導(dǎo)航中的應(yīng)用［J］.測(cè)繪學(xué)報(bào)，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.（高為廣，楊元喜，張雙成.基于當(dāng)前加速度模型的抗差自適應(yīng)KALMAN濾波［J］.測(cè)繪學(xué)報(bào)，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.（楊元喜，高為廣.兩種漸消濾波與自適應(yīng)抗差濾波的綜合比較分析［J］.武漢大學(xué)學(xué)報(bào)：信息科學(xué)版，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導(dǎo)航中的抗差自適應(yīng)Kalman濾波算法［J］.測(cè)繪科學(xué)，2005，30（2）：98－100.）