衰減記憶平方根UPF算法及其在組合導航中的應用

(1.西北工業大學 自動化學院，西安 710072；2.航天恒星科技有限公司，北京 100086)

(1.西北工業大學 自動化學院，西安 710072；2.航天恒星科技有限公司，北京 100086)

針對用UPF算法進行導航濾波計算可能出現協方差矩陣不對稱或負定的問題，在研究衰減記憶濾波和平方根濾波的基礎上，以Unscented粒子濾波為框架，提出一種新的衰減記憶平方根UPF算法。該算法利用衰減因子減小歷史信息對濾波的影響，增強當前量測信息在濾波計算中的作用。然后，以協方差矩陣的平方根陣代替協方差矩陣進行濾波解算，保證了協方差矩陣的對稱性和非負定性，提高了濾波算法數值穩定性和解算精度。將提出的算法應用到SINS/SAR組合導航系統中進行仿真驗證，并與UPF算法進行比較，結果表明，提出算法的濾波性能明顯優于UPF算法，能提高組合導航系統的濾波精度。

衰減因子；平方根濾波；衰減記憶濾波；Unscented粒子濾波；協方差矩陣

卡爾曼濾波 (Kalman Filter，KF)將狀態空間思想引入到最優濾波理論中，用狀態方程描述系統動態模型，用觀測方程描述系統觀測模型，可處理多維信號、非平穩信號和時變信號的濾波問題，在工程實際中得到了廣泛應用。但卡爾曼濾波要求已知系統精確的數學模型，且要求系統模型為線性，噪聲信號為統計特性已知的高斯噪聲，這大大限制了KF的應用[1]。

為了克服 KF的缺陷，一些學者研究提出了擴展卡爾曼濾波(EKF)，其基本思想是將非線性系統模型進行Taylor展開，略去二階項以及二階以上的高階項，得到非線性系統的線性化模型，然后對線性化后的系統模型采用卡爾曼濾波算法進行計算。EKF雖然在非線性濾波中得到了廣泛應用，但略去Taylor展開式的高階項會引起較大的線性化誤差，導致濾波誤差增大甚至發散。

Julier等人提出了無跡卡爾曼濾波(Unscented Kalman Filter，UKF)算法，UKF通過無跡變換(Unscented transformation，UT)對非線性系統狀態的后驗概率密度函數進行近似。與EKF相比，UKF無需計算非線性狀態函數和量測函數的雅克比矩陣，且理論上可以實現泰勒級數三階項所達到的精度，具有計算簡單，收斂性好等優點。然而，在噪聲先驗統計特性未知的情況下，UKF濾波精度會下降甚至發散。

Fagin和Sorenson提出了衰減記憶濾波算法[2-3]，該算法通過衰減因子限制卡爾曼濾波器的記憶長度，減小歷史信息對濾波的影響，增強當前量測信息在濾波計算中的作用，從而提高了濾波精度。但是，該方法要求計算矩陣的特征值和跡，計算量較大。平方根濾波算法采用矩陣分解的方法，在濾波過程中不計算協方差矩陣，而是用協方差矩陣的平方根陣代替協方差陣進行迭代運算，解決了常規算法中由于計算誤差和噪聲信號等因素引起協方差矩陣不對稱或負定而導致的濾波發散問題[4-5]，保證了協方差矩陣的非負定性，其缺點是計算量比標準的濾波計算大[6-9]。

本文在研究衰減記憶濾波和平方根濾波的基礎上，以Unscented粒子濾波為框架，提出一種新的衰減記憶平方根 UPF (Fading Memory Square Root Unscented Particle Filter，FMSRUPF) 算法。該算法利用衰減因子減小歷史信息對濾波的影響，增強當前量測信息在濾波計算中的作用。然后，以協方差矩陣的平方根陣代替協方差矩陣進行濾波解算，保證了協方差矩陣的對稱性和非負定性，提高了濾波算法數值穩定性和解算精度。將提出的算法應用到SINS/SAR組合導航系統中進行仿真驗證，并與UPF算法進行比較，結果表明，提出算法的濾波性能明顯優于UPF算法，能提高組合導航系統的濾波精度。

1 一種衰減記憶平方根UPF算法

考慮非線性系統：

式中， Qk和 Rk分別為噪聲 wk和 vk的協方差矩陣，δkj為Dirc函數。

衰減記憶平方根UPF算法的流程如圖1所示。

圖1 衰減記憶平方根UPF算法流程圖Fig.1 Architecture for fading memory square root unscented particle filter

計算步驟為：

第一步：初始化

第二步：用FMSRUPF濾波器更新粒子(k=1,2,…,n)

2）構造衰減因子 αk

衰減因子的確定方法不唯一。一些學者按濾波器收斂判據選擇衰減因子，要求理論預計誤差大于實際誤差[10]。另一些學者在自適應算法中用“記憶長度”決定衰減因子，當估計誤差較大時選用較大的衰減因子，反之則選擇小的衰減因子。文獻[11]提出了一種新的衰減濾波算法，推導出兩類衰減因子：第一類是用最速下降法確定衰減因子，計算時需經過多次迭代，且公式復雜，計算過程復雜，不適合工程實際應用；第二類衰減因子采用一步算法，其解析式為

式中， αk為衰減因子，tr[·]為矩陣求跡符號，n為粒子數， Mk和 Nk的表達式分別為[12]

3）時間更新

式中，qr{·}表示矩陣的QR分解，αk為衰減因子。

4）量測更新

該步驟中使用了線性代數中的 QR分解、Cholesky分解，以協方差平方根陣代替協方差陣參加迭代運算，從而增強狀態協方差矩陣更新過程中的數值穩定性，保證了協方差矩陣的非負定性。同時，利用衰減因子 αk調節量測值的權重，減小歷史信息對濾波的影響，增強當前量測信息在濾波計算中的作用。

衰減記憶平方根UPF算法利用了以下三種線性代數中的運算技巧，降低了算法的計算復雜度。在此，簡單的介紹這三種代數算法：

1） QR分解。協方差矩陣P分解為 P =SST， S可通過QR分解得到，qr{·}表示矩陣的QR分解。

2）Cholesky分解。協方差矩陣P分解為P =SST，S稱為P的平方根矩陣。chol{·}表示矩陣P的Cholesky分解，則 S =chol{P}。

歸一化權值

第四步：重采樣

第六步： k = k+1，返回第二步，計算下一時刻的狀態估值。

2 仿真實驗與分析

圖2 UPF算法速度誤差曲線Fig.2 Velocity error of UPF

圖3 FMSRUPF算法速度誤差曲線Fig.3 Velocity error of FMSRUPF

圖4 UPF算法位置誤差曲線Fig.4 Position error of UPF

圖2和圖3分別為UPF和FMSRUPF濾波算法對應的速度誤差曲線。采用UPF算法計算得到的東向和北向速度誤差在(-0.8 m/s, +0.8 m/s)，天向速度誤差在(-1 m/s, +1 m/s)；而采用FMSRUPF算法計算得到的東向速度誤差在(-0.3 m/s, +0.3 m/s)，北向和天向速度誤差在(-0.4 m/s, +0.4 m/s)。圖4和圖5分別為UPF和FMSRUPF濾波算法對應的位置誤差曲線。通過UPF算法計算得到的東向位置誤差控制在(-9 m, +9 m)，北向位置誤差和高度誤差控制在(-8 m, +8 m)；而通過FMSRUPF算法計算得到的東向和北向位置誤差控制在(-3 m, +3 m)，高度誤差為(-4 m, +4 m)。圖6給出了UPF和FMSRUPF算法在50次Monte Carlo仿真實驗中的均方根誤差曲線，FMSRUPF的均方根誤差明顯小于UPF的均方根誤差。

圖5 FMSRUPF算法位置誤差曲線Fig.5 Position error of FMSRUPF

圖6 UPF和FMSRUPF的均方根誤差曲線Fig.6 Root mean square error curve of UPF and FMSRUPF

表1 SINS/SAR組合導航系統速度誤差及均方根誤差比較Tab.1 Comparison of position errors and velocity errors for SINS/SAR integrated navigation system

3 結 論

本文以Unscented粒子濾波為基本框架，吸收了衰減記憶濾波和平方根濾波各自的優點，提出了一種新的衰減記憶平方根UPF算法。該算法利用衰減因子減小歷史信息對濾波的影響，增強當前量測信息在濾波計算中的作用；然后，以協方差矩陣的平方根陣代替協方差矩陣進行濾波解算，保證了協方差矩陣的對稱性和非負定性，提高了濾波算法數值穩定性和解算精度。仿真實驗表明，提出的FMSRUPF算法與UPF算法相比，能有效地改善濾波性能，提高導航濾波精度及穩定性。下一步需要解決的是如何降低計算量的問題。

（References）：

[1] 趙琳，王小旭，丁繼成，等. 組合導航系統非線性濾波算法綜述[J]. 中國慣性技術學報，2009，17(1)：46-52.

ZHAO Lin, WANG Xiao-xu, DING Ji-cheng, et al. Overview of nonlinear filter methods applied in integrated navigation system[J]. Journal of Chinese Inertial Technology, 2009, 17(1): 46-52.

[2] 王朝輝，徐一兵，許亞偉. 衰減記憶無跡卡爾曼粒子濾波算法研究[J]. 微電子學與計算機，2012，29(8)：50-52.

WANG Chao-hui, XU Yi-bing, XU Ya-wei. Research on memory attenuation UKF particle filter algorithm[J]. Microelectronics & Computer, 2012, 29(8): 50-52.

[3] CHEN Ke-xin, LI Shao-yuan. Unfalsified adaptive PID control with a fading memory cost function[C]//The 33rd Chinese Control Conference. Nanjing, China, 2014, D: 5179-5183.

[4] 董春敏，周偉，曹振坦，等. 平方濾波的優越性分析[J]. 測繪與空間地理信息，2012，35(1)：66-68.

DONG Chun-min, ZHOU Wei, CAO Zhen-tan, et al. The superiority analysis of the square root Kalman filter[J]. Geomatics & Spatial Information Technology, 2012, 35(1): 66-68.

[5] Tang Xiaojun, Yan Jie, Zhang Dudu. Square root sigma point Kalman filtering for spacecraft relative navigation[J]. Acta Astronautica, 2010, 66(5/6): 704-713.

[6] NING Xiaolin, HUANG Panpan. Adaptive points range square-root cubature Kalman filter for Mars approach navigation[C]//The 33rd Chinese Control Conference. Nanjing, China, 2014, A: 903-908.

[7] FU Gui-xia, WANG Hong-jian, LI Cun, LI Juan. Improved square root cubature particle filter based navigation method for UUV[C]//The 32rd Chinese Control Conference. Xi’an, China, 2013: 5095-5100.

[8] LIU Ji, GU Xing-sheng. Improved square-root UKF algorithm for state estimation of nonlinear systems[J]. Journal of Donghua University, 2010, 27(1): 74-80.

[9] Tang Xiaojun, Yan Jie, Zhong Dudu. Square-root sigmapoint Kalman filtering for spacecraft relative navigation [J]. Acta Astronautica, 2009, 66(5): 704-713.

[10] 章飛，周杏鵬，陳小惠. 基于衰減記憶濾波的平方根UKF被動目標跟蹤算法[J]. 測控技術，2010，29(4)：22-26.

ZHANG Fei, ZHOU Xing-peng, CHEN Xiao-hui. Square-root UKF passive target tracking algorithm based on memory attenuation filtering[J]. Measurement & Control Technology, 2010, 29(4): 22-26.

[11] Yang Y X, Cui X Q. Adaptively robust filter with multi adaptive factors[J]. Survey Review, 2008, 309(40): 260-270.

[12] GAO She-sheng, XUE Li, WEI Wen-hui. Fading adaptive UPF(Unscented Particle Filtering) algorithm and its application to integrated navigation[J]. Journal of Northwestern Polytechnical University, 2012, 30(1): 27-31.

[13] 趙巖，高社生，楊一，等. 自適應SDV-UPF算法及其在緊組合中的應用[J]. 中國慣性技術學報，2014，22(1)：83-88.

ZHAO Yan, GAO She-sheng, YANG Yi, et al. Adaptive SVD-UPF algorithm and application to tightly-coupled integrated navigation[J]. Journal of Chinese Inertial Technology, 2014, 22(1): 83-88.

[14] Zhong Y, Gao S, Li W. A quaternion-based method for SINS/SAR integrated navigation system[J]. IEEE Transactions on Aerospace and Electronic Systems, 2012, 48(1): 514-524.

衰減記憶平方根UPF算法及其在組合導航中的應用

高 怡1，高社生1，吳佳鵬2

Fading-memory square-root unscented particle filter algorithm and its application in integrated navigation system

GAO Yi1, GAO She-sheng1, WU Jia-peng2
(1. Department of Automatics Control, Northwestern Polytechnical University, Xi’an 710072, China; 2. Space star Technology Co., LTD, Beijing 100086, China)

In view that covariance matrix may have asymmetry or negative definite problems in unscented particle filtering, a new unscented particle filtering algorithm is presented based on fading memory and square root. This method uses the fading factor to improve the information utilization of the current measurement and reduce the influence of historical information. In the process of filtering, the square root of the covariance matrix is used in iterative calculation instead of the covariance matrix, to ensure the symmetry and positive definite of covariance matrix and effectively improve the computational efficiency and numerical stability. By applying the proposed algorithm in SINS /SAR integrated navigation system, the simulation results demonstrate that the fading memory square-root UPF algorithm outperforms the UPF ones, and can significantly improving the filter accuracy of the integrated system.

fading factor; square root filter; fading memory filter; unscented particle filter; covariance matrix.

1005-6734(2014)06-0777-05

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

TP301.6

A

2014-07-17；

2014-11-24

國家自然科學基金（61174193）；國家自然科學基金資助（批準號：61174193）；中國航天科技集團公司衛星應用研究院創新基金資助（批準號：2012-1521）

高怡（1978—），女，博士，從事信息工程及控制的研究。E-mail：gaoyi_nwpu@163.com

聯 系 人：高社生（1956—），男，教授，博士生導師。E-mail：gshshnpu@163.com