衰減記憶平方根UPF算法及其在組合導(dǎo)航中的應(yīng)用

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

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

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

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

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

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

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

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

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

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

考慮非線性系統(tǒng)：

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

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

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

計算步驟為：

第一步：初始化

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

2）構(gòu)造衰減因子 αk

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

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

3）時間更新

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

4）量測更新

該步驟中使用了線性代數(shù)中的 QR分解、Cholesky分解，以協(xié)方差平方根陣代替協(xié)方差陣參加迭代運算，從而增強狀態(tài)協(xié)方差矩陣更新過程中的數(shù)值穩(wěn)定性，保證了協(xié)方差矩陣的非負(fù)定性。同時，利用衰減因子 αk調(diào)節(jié)量測值的權(quán)重，減小歷史信息對濾波的影響，增強當(dāng)前量測信息在濾波計算中的作用。

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

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

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

歸一化權(quán)值

第四步：重采樣

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

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濾波算法對應(yīng)的速度誤差曲線。采用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濾波算法對應(yīng)的位置誤差曲線。通過UPF算法計算得到的東向位置誤差控制在(-9 m, +9 m)，北向位置誤差和高度誤差控制在(-8 m, +8 m)；而通過FMSRUPF算法計算得到的東向和北向位置誤差控制在(-3 m, +3 m)，高度誤差為(-4 m, +4 m)。圖6給出了UPF和FMSRUPF算法在50次Monte Carlo仿真實驗中的均方根誤差曲線，F(xiàn)MSRUPF的均方根誤差明顯小于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組合導(dǎo)航系統(tǒng)速度誤差及均方根誤差比較Tab.1 Comparison of position errors and velocity errors for SINS/SAR integrated navigation system

3 結(jié) 論

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

（References）：

[1] 趙琳，王小旭，丁繼成，等. 組合導(dǎo)航系統(tǒng)非線性濾波算法綜述[J]. 中國慣性技術(shù)學(xué)報，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]. 微電子學(xué)與計算機，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] 董春敏，周偉，曹振坦，等. 平方濾波的優(yōu)越性分析[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被動目標(biāo)跟蹤算法[J]. 測控技術(shù)，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] 趙巖，高社生，楊一，等. 自適應(yīng)SDV-UPF算法及其在緊組合中的應(yīng)用[J]. 中國慣性技術(shù)學(xué)報，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算法及其在組合導(dǎo)航中的應(yīng)用

高 怡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

國家自然科學(xué)基金（61174193）；國家自然科學(xué)基金資助（批準(zhǔn)號：61174193）；中國航天科技集團公司衛(wèi)星應(yīng)用研究院創(chuàng)新基金資助（批準(zhǔn)號：2012-1521）

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

聯(lián) 系 人：高社生（1956—），男，教授，博士生導(dǎo)師。E-mail：gshshnpu@163.com