基于CWPA模型的最優平滑算法性能及其應用

（1. 哈爾濱工程大學 自動化學院，哈爾濱 150001；2. 魁北克大學 ETS學院，蒙特利爾 H3C 1K3，加拿大）

（1. 哈爾濱工程大學 自動化學院，哈爾濱 150001；2. 魁北克大學 ETS學院，蒙特利爾 H3C 1K3，加拿大）

針對線性高斯系統的平滑問題，分析了RTS固定區間平滑與雙濾波器固定區間平滑兩種算法，提出了一種濾波存儲數據更少的RTS平滑新算法。結合平面內的運動追蹤問題，基于二維CWPA模型，仿真分析了卡爾曼濾波、RTS固定區間平滑以及雙濾波器平滑算法的估計性能。仿真結果表明，兩種固定區間平滑算法的估計效果等效，精度均優于卡爾曼濾波，對于實際問題中固定區間平滑算法的選用具有一定的參考價值。最后，結合雙濾波器結構提出了一種基于雙平滑器的艦載武器慣導傳遞對準精度評估方法，結果表明新方法相比于單一的平滑算法，可以獲取更優的綜合平滑性能，特別提升了水平姿態對準誤差的平滑估計性能。

最優平滑；卡爾曼濾波；RTS平滑器；雙濾波器平滑器；CWPA模型；傳遞對準精度評估

最優估計是指從受噪聲影響的量測量中估計出所需的狀態量，且使性能指標達到特定條件下的最優[1]。由于最優估計理論不僅能獲取狀態的估計值，往往還可以同時獲取能夠表征狀態估計性能的指標量，在自動控制、信號處理、通訊、圖像處理、導航、測繪、石油勘探和金融等諸多領域中，均得到了極為廣泛的應用[2]。

在已知一個時間段內量測信息的條件下，依據待估計狀態與該時間段所屬時間的關系，可將最優估計問題分為三類：預測、濾波和平滑。濾波估計具有良好的實時性，能夠滿足絕大多數的應用要求，因此得到了最為廣泛的研究與發展。對于平滑估計，在一定程度上屬于基于濾波估計的數據再處理算法，其較高的解算復雜程度和實時性的不足，限制了平滑的應用范圍[3]。

最優平滑算法可分為固定區間平滑、固定點平滑和固定滯后平滑三類[4]。固定區間平滑算法可以利用在固定的時間區間內得到的所有量測值，去估計該時間區間內某一時刻的狀態值。它往往可以獲得比固定點平滑和固定滯后平滑較高估計精度，因此在對參數估計精度要求較高的慣性導航領域得到了較多的應用。例如，文獻[5]利用RTS固定區間平滑算法評估了艦載武器慣導系統傳遞對準的性能，獲得了比固定點平滑更優的效果。文獻[6]利用RTS固定區間平滑算法有效實現了陀螺漂移模型的參數辨識，且表明固定區間平滑算法比卡爾曼濾波方法的估計精度更高。文獻[7]將 RTS固定區間平滑算法應用于實際飛行狀態的實時估計中。文獻[8]將固定區間平滑算法應用到了GPS/INS組合導航系統中，采用事后處理的方法，獲得了較好的定位精度。但現有文獻多為單一的RTS固定區間平滑算法的應用型研究，并沒有針對另一種國外常用的雙濾波器固定區間平滑算法或者上述兩種算法的性能進行實質性的分析，尤其缺少組合平滑方法方面的分析及應用研究。

本文結合線性狀態空間系統模型，依次分析了最為常用的 RTS平滑算法和雙濾波器平滑算法。針對RTS平滑算法中存在數據存儲量大的問題，提出了改進的 RTS平滑算法，并結合連續維納加速(Continue Wiener Process Acceleration，CWPA)數學模型，通過仿真分析了兩種平滑算法在噪聲驅動狀態估計問題中的性能。最后，設計了一種基于雙濾波器結構的艦載武器慣導傳遞對準精度評估方法，提高了評估系統的性能，特別是水平姿態誤差的平滑精度。

1 高斯線性模型與卡爾曼濾波

1.1 狀態空間模型

在現代控制理論中，通常將離散化后的線性系統表示為如下狀態空間模型形式：

式中：Φk,k-1為一步轉移陣， Hk為量測陣，Wk-1、Vk分別為系統噪聲、量測噪聲。

當系統噪聲與量測噪聲均設置為高斯白噪聲時，該線性系統同時具備高斯及線性特性。

1.2 經典卡爾曼濾波

基于系統高斯線性假設，Kalman于1960年提出了經典的線性濾波方法[9]。濾波方法可表示為：

① 基于系統狀態模型的一步預測，并給出表征一步預測性能的協方差陣：

2 RTS固定區間平滑算法

在Kalman及Bucy建立經典最優線性濾波方法理論體系后，Rauch、Tung和Striebelrts于1965年提出了經典的固定區間平滑算法，因此也稱為RTS固定區間平滑算法[10]。

文獻[11]在貝葉斯意義下，將RTS固定區間的平滑問題用條件概率分布表示為

結合條件概率密度公式，相鄰時刻狀態的聯合概率密度可表示為

基于高斯假設條件，利用極大似然估計準則，可推導出RTS平滑算法的表達為

由式(6)可知，RTS固定區間平滑有以下特性：

① 平滑解算過程相對于濾波過程是逆向的。因此，RTS固定區間平滑算法在傳遞對準精度評估等側重于初始狀態獲取的應用中，最終平滑值的讀取方式與普通前向濾波器估計值的讀取方式相反。

② 平滑過程利用了前向濾波過程中所產生的數據。因此，平滑解算需要在濾波過程中實時存儲數據，所存儲的數據為 4個矩陣，分別為估計值、一步轉移陣估計的均方差陣，以及一步預測均方差陣

RTS平滑算法的解算流程圖，如圖1所示。

圖1 RTS平滑算法解算流程圖Fig.1 Calculation flowchart of RTS smoother

圖 2 改進型RTS平滑算法解算流程圖Fig.2 Calculation flowchart of advanced RTS smoother

3 雙濾波器固定區間平滑

雙濾波器固定區間平滑(Two-Filter Smoother, TFS)，也被稱為前向-后向濾波器。其濾波結構由前向濾波器、后向濾波器以及數據融合三部分組成[12]。

前向濾波器在線性高斯估計問題中，等同于 1.2節中所論述的標準卡爾曼濾波器。后向濾波器則用于提供后向虛擬軌跡和后向量測更新[13]。

后向濾波方程的表示形式與前向濾波非常相似，但是后向濾波過程中的時間更新相對于實際時間是逆序過程。假設量測量所在的固定時間區間為[0,N]，記前向濾波步數為k，后向濾波步數為τ，則有τ= N-k ， τ= N,N -1,…, 0。

基于相同狀態空間模型以及時間逆序，可得后向濾波器的解算表達式為：

式中，上標“b”表示后向濾波。TFS固定區間平滑中還包含了信息融合過程，其表達式為：

圖 3 TFS平滑算法解算流程圖Fig.3 Calculation flowchart of TFS smoother

圖3給出了雙濾波器平滑的結構示意圖。為了保證后向濾波器解算的獨立性，需要避免將前向濾波的結果提供給后向濾波器。因此，對于虛擬的后向濾波過程，其后向濾波初值及后向濾波協方差陣很難精確獲取，這在很大程度上限制了TFS平滑算法的應用。而慣導系統屬于推算導航系統，TFS的后向濾波解算可以通過外部提供最終導航狀態進行逆向解算較為容易地實現[14]。因此，TFS在慣導領域具有特別的適用性。

4 兩種固定區間平滑算法性能分析

4.1 性能分析方法

針對上述兩種基本固定區間平滑算法，本文引入了CWPA模型，展開了基于CWPA模型的平滑算法性能分析研究。該模型被應用于平面內運動體的位置跟蹤問題，具體問題可描述為：運動體在坐標確定的二維平面內運動，有一受高斯白噪聲影響的定位傳感器，通過該傳感器測量運動體坐標以實現定位，并期望同時獲取運動體的速度及加速度信息[15]。由于該模型與簡化后的二維平面慣導模型十分相似，因此它對于通常忽略天向通道的艦船領域慣導研究具有較強的適用性。

本節利用較為簡單的CWPA模型，對擬應用于艦船領域慣導的平滑算法進行研究，為后續平滑算法的具體工程應用提供依據。

研究過程中具體的實施方法為：1）由軌跡發生器模擬產生真實的連續運動軌跡以及噪聲污染的離散量測信息；2）結合CWPA模型，利用卡爾曼濾波、RTS及TFS算法分別估計出運動體的位置與速度，并同時輸出估計值的均方根誤差；3）通過軌跡與均方根誤差研究各算法的性能：估計軌跡、估計速度曲線越接近真實軌跡，均方根誤差越小，表明算法性能越優。

特別要注意的是，平面坐標進行了標準單位化，并將估計得到的每一步離散運動體位置、速度信息進行了匯總，標識了起始點。此外，本文主要針對適用于離線處理的潛在應用進行研究，不對算法的實時性進行分析。

4.2 二維CWPA模型

對運動體的位置跟蹤問題進行數學描述，狀態變量為

式中：xk、yk分別表示運動體位置的橫、縱坐標；、分別表示運動體速度在橫、縱坐標的投影；、分別表示運動加速度在橫、縱坐標的投影。

運動體的追蹤問題可用線性時變模型表示為：

由于所述的卡爾曼濾波及兩種固定區間平滑算法均基于離散系統模型，因此需用泰勒級數展開法將式(10)離散化，記離散化相關時間為 Δt，則有：

系統噪聲矩陣為：

其量測噪聲矩陣為：

初始速度及加速度均設置為0，仿真進行50步。由系統隨機產生一組真實運動軌跡及其量測軌跡，如圖4所示，圖中細實線表示運動體真實的運動軌跡，離散點表示傳感器測得的量測量，圓圈處表示軌跡初始點，起始點的位置坐標為(0,0)。由圖4可知，由于傳感器存在量測噪聲，直接測量結果并不能很好地反應真實軌跡。

4.3 仿真結果

針對基于二維CWPA模型的運動追蹤估計問題，分別利用標準離散卡爾曼濾波、RTS平滑算法以及TFS平滑算法進行仿真分析。

首先，利用卡爾曼濾波(Kalman Filter, KF)進行仿真，獲取實時位置與速度濾波估計值。所得的濾波估計軌跡，如圖5所示。圖中，虛線表示真實的運動軌跡與速度，實線表示KF估計得到的軌跡與速度，圓圈所示為運動起始點，位置坐標為(0,0)；速度以二維向量的形式標識，格式為(x′,y′)，初始速度為(0,0)。

圖 4 真實運動軌跡及其量測量Fig.4 Real trajectory and measurements of moving object

由圖5可知，對于運動軌跡，KF估計得到的軌跡較為理想；但KF估計得到的速度與真實速度存在較大誤差。

圖 5 真實運動參數與卡爾曼濾波估計結果對比Fig.5 The real motion parameters and estimates by KF

利用RTS算法估計得到的平滑結果，如圖6所示，圖中虛線表示真實的運動軌跡與速度，實線表示RTS算法估計得到的軌跡與速度。

圖 6 真實運動參數與RTS平滑估計值對比Fig.6 Real motion parameters and estimates by RTS smoother

由圖6可知，對于運動軌跡，利用RTS平滑算法估計得到的軌跡，與真實軌跡幾乎重合；但RTS平滑算法估計得到的速度曲線更加平滑，數據穩定性增強了，與真實速度誤差較小，估計效果要優于卡爾曼濾波結果。利用TFS平滑算法估計得到的平滑結果，如圖7所示。

結合圖6、圖7可知，RTS和TFS固定區間平滑的估計效果幾乎一致，均能較好的跟蹤真實軌跡，得到的速度估計曲線也較為平滑。

三種估計方法的位置估值均方誤差，如表1所示。三種估計方法的速度估值均方誤差，如表2所示。

由仿真圖及均方誤差表可知，RTS與TFS這兩種平滑算法的估計性能均優于KF。這主要是由于兩種平滑算法都充分利用了平滑前濾波過程所產生的數據，提高了估計精度。而由RTS 與TFS這兩種固定區間平滑算法得到的估計結果曲線及各估計值的均方誤差均一致，表明這兩種平滑算法的估計效果是等效的。

圖 7 真實運動參數與TFS平滑估計值對比Fig.7 Real motion parameters and estimates by TFS smoother

表 1 三種估計方法位置估計值的均方誤差Tab.1 Errors of position estimates for the three algorithms

表 2 三種估計方法速度估計值的均方誤差Tab.2 Errors of velocity estimates for the three algorithms

上述結論對于平滑算法的應用具有重要的實際意義。一方面，由于RTS平滑算法在平滑值解算時，需要進行一步轉移協方差陣的求逆，這有可能導致誤差協方差陣失去正定性，從而導致平滑結果失效，此時，可以用TFS平滑算法替代，因為TFS僅在數據融合時進行了濾波協方差陣求逆運算；另一方面，TFS平滑算法對于后向濾波有著較高的要求，如果應用信息濾波或者虛擬后向濾波方法無法獲取理想效果，可以利用 RTS平滑算法替代，而且兩種算法具有同等的估計性能。

5 基于雙濾波器結構的雙平滑器精度評估方法

艦載武器慣導系統傳遞對準的精度評估是最優平滑算法較為常見的應用之一。在艦載武器慣導系統完成傳遞對準后，立即轉入評估過程，基于傳遞對準誤差在導航信息中傳播的特性，利用艦載武器慣導相對外部精確信息基準的誤差量測，結合RTS平滑、固定點平滑(Fixed Point Smoother, FPS)[12]等最優平滑算法，對對準結束時刻的對準誤差進行平滑估計。

但現有的對準精度評估研究，存有兩個明顯特征：

① 所研究的內容側重于評估方案的改進。文獻[16]論述了平滑估計方案相對于光學物理檢測手段的優勢，確立進行性能評估的經典方法。而文獻[3][5]均提出引入高精度航向基準信息對提升方位對準誤差評估性能的重要性，并解決外部基準信息引入所帶來的問題，則屬于對估計器匹配方法的改進、優選。

② 所應用的平滑算法較為單一。一方面，算法的選用范疇仍然局限于經典的RTS或FPS平滑算法；另一方面，算法使用時只選用單種平滑算法。

針對上述問題，本文結合對平滑算法的性能研究，基于雙濾波器結構提出一種新的雙平滑器傳遞對準精度評估方法。雙平滑器平滑(Two-Smoother Smoothing，TSS)中涉及的前向估計器選用FPS算法，后向估計器選用RTS平滑算法。該方法的解算流程如圖8所示。

圖 8 基于TSS算法的精度評估方法示意圖Fig.8 Evaluation scheme based on two-smoother smoothing

該方法的前向平滑中，FPS解算過程相對于 KF解算過程獨立；后向平滑中，RTS平滑基于經典KF，并呈后向解算的算法特性。因此，所設計的TSS算法能夠保證前向、后向平滑解算過程的獨立性。

通過對式(8)進行轉換，可得：

式(14)表明：TSS算法對全部狀態量的綜合估計性能要優于采用單一的前向FPS或后向RTS算法。

針對對準精度評估中最為關注的對準姿態誤差，表3給出了FPS、RTS及TSS算法在對準精度評估中水平姿態誤差估計值的均方誤差。

由表3可知，所提出的TSS算法水平姿態對準誤差估計值的均方誤差，要優于FPS及RTS。但必須指出，由于FPS算法的數據穩定性較差，重復仿真中會出現多次偏差明顯很大的估計值，應予以剔除。否則所得到FPS的水平姿態估計值均方誤差將過大，間接影響后續的數據融合以及對TSS算法的評價結果。因此，表3中FPS的均方誤差值稍優于RTS。此外，鑒于FPS算法對方位姿態對準誤差的估計數據穩定性較差，且受不同評估方案影響很大，表3中未對方位姿態對準誤差進行分析。

表3 三種平滑方法水平姿態對準誤差估計值的均方誤差Tab.3 Errors of horizontal misalignment estimates for the three smoothing algorithms

基于雙濾波器結構的平滑算法還可以作為平滑算法框架，針對前向平滑算法或后向平滑算法進行優化(例如對前向FPS的平滑數據穩定性進行改進)，甚至擴展至非線性平滑領域。對于一些靜態應用對象，還可以作為估計算法框架，有針對性地對前向估計器及后向估計器進行優選，例如：引入 UKF、PF等非線性濾波器[17]，設計出最適用于該對象的估計算法。

6 結 論

綜上所述，本文得到下述結論：所提出的改進型RTS固定區間平滑算法，可以減少其濾波過程所存儲的數據，進而提升RTS平滑算法的計算效率。仿真結果表明：RTS固定區間平滑算法與TFS平滑算法的估計性能具有等效性，可以獲取同等精度的軌跡、速度估計結果；所提出的基于雙濾波結構的艦載武器慣導傳遞對準精度評估方法，可以獲取較傳統單一FPS或RTS算法更優的平滑性能。最后，展望了基于該算法框架的后續研究方向。

（References）：

[1] 秦永元，張洪鉞，汪叔華. 卡爾曼濾波與組合導航原理[M]. 西安：西北工業大學出版社，1998：5-7, 16-20, 34.

[2] Arthur G. Applied optimal estimation[M]. The MIT Press, 1974: 1-5.

[3] 程建華，陳岱岱，田軍. 基于分類因子自適應濾波的慣導傳遞對準精度評估方法[J]. 中國慣性技術學報，2013，21(5)：598-603.

Cheng J H, Chen D D, Tian J. Transfer alignment accuracy evaluation for SINS based on classified factors adaptive filter[J]. Journal of Chinese Inertial Technology, 2013, 21(5): 598-603.

[4] 史忠科. 最優估計的計算方法[M]. 北京：科學出版社，2001：36-37.

[5] 程建華，陳岱岱. 艦船姿態輔助DGPS的傳遞對準精度評估方法[J]. 哈爾濱工程大學學報，2012，33(12)：1509-1514.

CHENG Jian-hua, CHEN Dai-dai. Transfer alignment accuracy evaluation based on DGPS assisted with the ship attitudes[J]. Journal of Harbin Engineering University, 2012, 33(12): 1509-1514.

[6] Nash R A, Kasper J F, Crawford B, et al. Application of optimal smoothing to the testing and evaluation of inertial navigation systems and components[J]. IEEE Transactions on Automatic Control, 1971, 16(6): 806-816.

[7] 史忠科. 固定區間平滑新算法及其在飛行試驗中的應用[J]. 自動化學報，1991，17(3)：323-329.

SHI Zhong-ke. A new algorithm of fixed-interval smoother and its application to flight test[J]. ACTA Automatic Sinica, 1991, 17(3): 323-329.

[8] 宮曉琳，張蓉，房建成. 固定區間平滑算法及其在組合導航系統中的應用[J]. 中國慣性技術學報，2012，20(6)：687-693.

GONG Xiao-lin, ZHANG Rong, FANG Jian-cheng. Fixedinterval smoother and its applications in integrated navigation system[J]. Journal of Chinese Inertial Technology, 2012, 20(6): 687-693.

[9] Kalman R E. A new approach to linear filtering and prediction problems[J]. Transactions of the ASME--Journal of Basic Engineering, 1960, 82(1): 35-45.

[10] Gong X L, Qin T T. Airborne earth observation positioning and orientation by SINS/GPS integration using CD R-T-S smoothing[J]. Journal of Navigation, 2014, 67(2): 211-225.

[11] Ait-El-Fquih B, Desbouvries F. On Bayesian fixed interval smoothing algorithms[J]. IEEE Transactions on Automatic Control , 2008, 53(10): 2437-2442.

[12] S?rkk? S. Bayesian filtering and smoothing[M]. Cambridge University Press, 2013: 139-142.

[13] Liu H, Nassar S, El-Sheimy N. Two-filter smoothing for accurate INS/GPS land-vehicle navigation in urban centers[J]. IEEE Transactions on Vehicular Technology, 2010, 59(9): 4256-4267.

[14] Shin E. Estimation Techniques for low-cost inertial navigation[D]. Calgary: The University of Calgary, 2005.

[15] Jouni H, Arno S, S?rkk? S. Optimal filtering with Kalman filters and smoothers: a manual for the Matlab toolbox EKF/UKF[R]. Aalto University, 2011: 11-13.

[16] Shortelle K J, Graham W R, Rabour C. F-16 flight tests of a rapid transfer alignment procedure[C]//Position Location and Navigation Symposium. USA, 1998: 379-386.

[17] Yu J, Lee J G, Park C G, et al. An off-line navigation of a geometry PIG using a modified nonlinear fixed-interval smoothing filter[J]. Control Engineering Practice, 2005, 13(11): 1403-1411.

基于CWPA模型的最優平滑算法性能及其應用

程建華1,2，陳岱岱1，趙 琳1，王冰玉1，Rene Landry2

Performance and application of optimal smoothing algorithms based on CWPA model

CHENG Jian-hua1,2, CHEN Dai-dai1, ZHAO Lin1, WANG Bing-yu1, Rene Landry2
(1. College of Automation, Harbin Engineering University, Harbin 150001, China; 2. Ecole de Technologie Superieure, Université du Québec, Montreal H3C 1K3, Canada)

Aiming at the smoothing problem in linear Gaussian systems, the RTS smoother and two-filter smoother were analyzed. A novel RTS smoother was proposed which need less filtering data to be stored than that of traditional one. By taking into account the in-plane motion tracking problem, a continue Wiener process acceleration(CWPA) model was introduced. Based on the CWPA model, the estimation performances of Kalman filter, RTS smoother and two-filter smoother were studied by simulation. The results show that the estimation effects of the two smoothers are equivalent, and both are better than that of Kalman filter. Finally, according with the framework of two-filter smoother, a two-smoother-based transfer alignment accuracy evaluation approach for airborne slave inertial navigation system was presented. Compared with the conventional schemes in which only single smoother is adopted, this new approach can get better overall smooth performance. In particular, it improves the smoothing-based estimation performance of horizontal attitude misalignment.

optimal smoothing; Kalman filter; RTS smoother; two-filter smoother; continue Wiener process acceleration model; transfer alignment accuracy evaluation

1005-6734(2014)06-0748-07

10.13695/j.cnki.12-1222/o3.2014.06.009

U666.1

A

2014-07-19；

2014-11-13

國家自然科學基金（61374007, 61104036）；中央高校基本科研業務費專項資金（HEUCFX41309）

陳岱岱（1988—），男，博士研究生，從事組合導航技術研究。E-mail：ins_dai@163.com

聯 系 人：程建華（1977—），男，副教授，碩士生導師。E-mail：ins_cheng@163.com