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

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

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

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

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

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

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

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

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

計(jì)算步驟為：

第一步：初始化

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

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

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

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

3）時(shí)間更新

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

4）量測(cè)更新

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

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

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

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

歸一化權(quán)值

第四步：重采樣

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

2 仿真實(shí)驗(yàn)與分析

圖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濾波算法對(duì)應(yīng)的速度誤差曲線。采用UPF算法計(jì)算得到的東向和北向速度誤差在(-0.8 m/s, +0.8 m/s)，天向速度誤差在(-1 m/s, +1 m/s)；而采用FMSRUPF算法計(jì)算得到的東向速度誤差在(-0.3 m/s, +0.3 m/s)，北向和天向速度誤差在(-0.4 m/s, +0.4 m/s)。圖4和圖5分別為UPF和FMSRUPF濾波算法對(duì)應(yīng)的位置誤差曲線。通過(guò)UPF算法計(jì)算得到的東向位置誤差控制在(-9 m, +9 m)，北向位置誤差和高度誤差控制在(-8 m, +8 m)；而通過(guò)FMSRUPF算法計(jì)算得到的東向和北向位置誤差控制在(-3 m, +3 m)，高度誤差為(-4 m, +4 m)。圖6給出了UPF和FMSRUPF算法在50次Monte Carlo仿真實(shí)驗(yàn)中的均方根誤差曲線，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)點(diǎn)，提出了一種新的衰減記憶平方根UPF算法。該算法利用衰減因子減小歷史信息對(duì)濾波的影響，增強(qiáng)當(dāng)前量測(cè)信息在濾波計(jì)算中的作用；然后，以協(xié)方差矩陣的平方根陣代替協(xié)方差矩陣進(jìn)行濾波解算，保證了協(xié)方差矩陣的對(duì)稱性和非負(fù)定性，提高了濾波算法數(shù)值穩(wěn)定性和解算精度。仿真實(shí)驗(yàn)表明，提出的FMSRUPF算法與UPF算法相比，能有效地改善濾波性能，提高導(dǎo)航濾波精度及穩(wěn)定性。下一步需要解決的是如何降低計(jì)算量的問(wèn)題。

（References）：

[1] 趙琳，王小旭，丁繼成，等. 組合導(dǎo)航系統(tǒng)非線性濾波算法綜述[J]. 中國(guó)慣性技術(shù)學(xué)報(bào)，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] 王朝輝，徐一兵，許亞偉. 衰減記憶無(wú)跡卡爾曼粒子濾波算法研究[J]. 微電子學(xué)與計(jì)算機(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] 董春敏，周偉，曹振坦，等. 平方濾波的優(yōu)越性分析[J]. 測(cè)繪與空間地理信息，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被動(dòng)目標(biāo)跟蹤算法[J]. 測(cè)控技術(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]. 中國(guó)慣性技術(shù)學(xué)報(bào)，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

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

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

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