摘要：传统的扩展卡尔曼滤波（Extended Kalman filter，EKF）算法应用于未来高超、空天飞行器的组合导航系统时，因其模型线性化展开会导致模型不准确，从而引起导航精度下降；采用蒙特卡洛方法来实现递推贝叶斯估计问题的粒子滤波（Particle filter，PF）算法能有效避免引入线性化误差，具有一定的优势。据此，针对高超、空天飞行器在发射过程中通常需要直接获得发射惯性系下的高精度导航参数的需求，提高发射惯性系下弹载组合导航系统滤波算法的精确性就尤为重要，PF滤波算法无需对非线性系统进行线性化展开即可直接实现对非线性系统的状态误差估计。为此，本文将PF滤波算法引入空天飞行器SINS/GPS/CNS多信息融合组合导航系统，设计了发射系下基于联邦滤波器的PF滤波算法，实现了对组合导航系统状态参数的直接建模估计。算法仿真结果表明，相较于发射系下SINS/GPS/CNS组合导航系统联邦EKF滤波算法，PF滤波算法有效提高了组合导航系统滤波精度。
关键词：扩展卡尔曼滤波；粒子滤波；联邦滤波；SINS/GPS/CNS；组合导航
中图分类号：V448.2 文献标志码：A 文章编号：1005-2615（2015）03-0319-05
SINS/GPS/CNS Integrated Navigation System Federal PF Algorithm in Launch Inertial Coordinate System
Xiong Zhi，Pan Jialiang，Lin Aijun，Du Huajun，Yu Feng
Abstract：When the traditional extended Kalman filter （EKF） algorithm is used in integrated navigation system of future aircraft， it may lead to the inaccuracy of the model because of linearization and the decreasing of navigation precision. Particle filter （PF） solves Bayes estimation problem by using Monte Carlo method and can effectively avoid bringing in linearization error. Consequently， aimed at the requirement of high accuracy for the navigation system state parameters in launch inertial coordinate system， it is particularly important to improve the accuracy of filter algorithm for missile-borne integrated navigation system. The PF algorithm can directly achieve an error estimation without linearization of nonlinear system. This paper brings PF algorithm into the SINS/GPS/CNS integrated navigation system. PF algorithm is designed based on federal filter and the navigation system state parameters are estimated directly. The algorithm simulation results indicate PF algorithm effectively improves filtering precision compared with EKF algorithm and is very suitable for integrated navigation system.
基金项目：国家自然科学基金（61374115，61203188，91016019，60904091，61210306075）资助项目；江苏省六人人才高峰（2013-JY-013）资助项目；江苏高校优势学科建设工程资助项目；中央高校基本科研业务费专项资金资助项目；南京航空航天大学大学研究生创新基地（实验室）开放基金（kfjj201425）资助项目。
收稿日期：2015-03-08；修订日期：2015-04-20
Key words： extented Kalman filter； particle filter； federal filter； SINS/GPS/CNS； integrated navigation
针对未来高超、空天飞行器导航系统状态强非线性等特点，为实现飞行器高精度导航，有效手段之一是构建发射惯性系下的组合导航系统。多信息融合处理方法作为发射惯性系下组合导航系统的核心技术之一，与组合导航系统的导航精度紧密相关，正逐步受到重视而被广泛研究。在众多的滤波算法中，传统的线性扩展卡尔曼滤波算法（Extented Kalman filter，EKF）因系统模型线性化展开的要求而会影响估计的精度[1-3]，粒子滤波由于其突出的处理非线性、非高斯系统估计问题的能力而受到学者们的重视[3-5]，其核心思想是使用一组具有相应权值的随机样本集合逼近所需的后验概率密度，不需要对系统非线性方程进行线性化，因而可以有效避免引入线性化误差，提高导航精度。 现有研究的空天飞行器SINS/GPS/CNS多信息融合组合导航系统滤波算法主要采用了当地地理参考坐标系作为统一的导航参考坐标系，其组合滤波算法模型通常采用传统的EKF算法实现对飞行器的状态估计。现有的空天飞行器多信息融合组合导航系统滤波算法只能直接获取地理系下的飞行器导航参数信息，难以满足空天飞行器在发射主动段需要直接获取发射惯性系下的导航参数需求；此外，由于其采用EKF滤波算法实现对飞行器的状态估计，而EKF的基本方式是采用泰勒级数展开法对非线性系统进行泰勒展开，存在一阶近似，导致飞行器在高动态飞行条件下的滤波估计性能下降。为提高飞行器状态的动态估计性能，本文设计了发射系下SINS/GPS/CNS组合导航系统联邦粒子滤波（Particle filter，PF）算法。
本文在直接建立发射惯性系下的SINS/GPS/CNS组合导航系统状态模型和观测模型的基础上，采用基于联邦滤波器的PF滤波算法直接估计发射惯性系下导航系统的状态参数[6-7]，仿真结果表明，由于PF算法无需对状态模型进行一阶线性化展开，因而相较于基于联邦滤波器的EKF算法能获得更高精度的导航结果，同时由于采用联邦滤波器结构，有效提高了导航系统的可靠性和容错性，因而十分适合于在高精度组合导航系统中应用。
1 SINS/GPS/CNS联邦PF滤波器设计
1.1 联邦PF滤波器方案
联邦PF滤波器方案如图1所示，其中：，Pi（i=1，2）分别表示第i个子滤波器的估计值和方差阵，和Pf分别表示全局估计值和方差阵。SINS/GPS子滤波器和SINS/CNS子滤波器分别采用PF滤波算法进行滤波处理，主滤波器进行全局最优融合，以得到高精度导航参数[8-9]。主滤波器融合算法如下
最后，根据主滤波器得到，Pf进行PF滤波算法的重新采样，进行粒子状态的递推更新。
1.2 组合导航系统数学模型
本文选取发射惯性系作为导航坐标系，SINS/GPS/CNS组合导航系统相关解算方程如下
式中：q为姿态四元数；p为发射惯性系下的位置值；v为发射惯性系下的速度值；g为地球的万有
图3为采用PF与EKF算法求解得到的姿态误差曲线对比图，图4为采用PF与EKF算法求解得到的位置误差曲线对比图，图5为采用PF与EKF算法求解得到的速度误差曲线对比图。并根据相应的仿真数据，可以获得各类导航参数的RMS均方误差统计结果，如表1所示。
估计误差标准差相关参数曲线如图6所示。
从图3～6和表1可以看出，采用发射系下SINS/GPS/CNS组合导航系统联邦PF滤波算法求解得到导航误差参数均小于采用发射系下SINS/GPS/CNS组合导航系统联邦EKF滤波算法求解得到的相应导航误差参数，PF滤波精度较EKF有显著的提升。表明在同样的仿真验证条件下，空天飞行器SINS/GPS/CNS多信息融合组合导航系统滤波算法选用PF滤波算法实现对状态的非线性估计，可以获得比线性化展开的EKF滤波算法更优的性能。
3 结束语
应用于未来高超、空天飞行器并建立于发射惯性系下的组合导航系统状态方程具有强非线性，且各状态相互耦合影响，传统的扩展卡尔曼滤波算法由于需要对模型进行线性化展开从而影响导航系统精度；粒子滤波算法因不需要进行线性化展开而具备特殊的优势，受到越来越多学者的重视。鉴于此，本文在建立发射惯性系下SINS/GPS/CNS组合导航系统模型的基础上，采用基于联邦滤波器的粒子滤波算法对导航参数进行估计。仿真结果表明，相较于采用EKF原理实现的联邦滤波器估计算法，粒子滤波能获得更高精度的导航结果，使其在高精度组合导航系统中成为一种有效的选择。
参考文献：
[1] Rigatos G G. Nonlinear Kalman filters and particle filters for integrated navigation of unmanned aerial vehicles [J]. Robotics and Autonomous Systems， 2012，60（7）：978-995.
[2] Vani Cheruvu， Priyanka Aggarwal， Vijay Devabhaktuni. A novel voronoi based particle filter for multisensor data fusion[J]. Applied Mathematics，2012（11）：1787-179.
[3] Kotecha J H，Djuric P M. Gaussian particle filtering[J]. IEEE Transactions on Signal Processing，2003，51（10）：2592-2601.
[4] Olivier Brun， Vincent Teulière， Jean-Marie Garcia.Parallel particle filtering[J]. J Parallel Distribted Computing，2002，62（7）：1186-1202.
[5] Chen Xiyuan，Shen Chong，Zhao Yuefang，et al. Study on GPS/INS system using novel filtering methods for vessel attitude determination[J]. Mathematical Problems in Engineering，2013（2013）：1-8.
[6] 吴志峰，吴军，王蕊，基于双联邦UKF算法的组合导航数据融合方法[J].弹箭与制导学报，2009，29（5）：106-110. Wu Zhifeng，Wu Jun，Wang Rui. Intergrated navigation data fusion method based on double federated PF algorithm[J]. Journal of Projectiles， Rockets， Missiles and Guidance， 2009，29（5）：106-110.
[7] 李天伟，孙少杰，彭伟骅，等.混合联邦-粒子滤波在组合导航系统中的应用[J].计算机仿真，2013，30（4）：13-16.
Li Tianwei，Sun Shaojie，Peng Weihua，et al. Mixture federated-particle filter and its application on integrated navigation system[J]. Computer Simulation，2013，30（4）：13-16.
[8] Wang Rong， Liu Jianye， Xiong Zhi，et al. Double-layer fusion algorithm for EGI-based system[J].Aircraft Engineering and Aerospace Technology，2013，85（4）：258-266.[9] Xiong Zhi， Chen Jihui， Wang Rong，et al. A new dynamic vector formed information sharing algorithm in federated filter[J]. Aerospace Science and Technology， 2013，29（1）：37-46.
[10] Katalin Gyorgy，András Kelemen，László Dávid. Unscented Kalman filters and particle filter methods for nonlinear state estimation[J]. Procedia Technology，2014，12（1）：65-74.
[11] 孙化东.基于粒子滤波的INS/GPS组合导航滤波算法[J].光学与光电技术，2012，10（1）：96-98.
Sun Huadong. Particle filtering algorithm for INS/GPS integrated navigation[J]. Optics & Optoelectronic Technology， 2012，10（1）：96-98.
[12] Myers M R，Jorge A B，Mutton M J，et al. A comparison of extended Kalman filter， particle filter， and least squares localization methods for a high heat flux concentrated source [J]. International Journal of Heat and Mass Transfer，2012，55（9）：2219-2228.
[13] Leela K B，Raju K P. Performance analysis of particle filter for launch vehicle trajectory[J]. IETE Journal of Education，2013，54（2）：83-91.