Adaptively robust multi-sensor fusion algorithm based on square-root cubature Kalman filter
-
摘要:
为解决模型误差和异常量测值发生时平方根容积卡尔曼滤波（SRCKF）算法滤波性能下降甚至滤波发散的问题，提出了一种多传感器融合自适应鲁棒算法。基于新息协方差匹配原则设计了鲁棒子系统以抑制量测异常值，同时为克服模型误差使用基于新息修正的低复杂度自适应SRCKF（LCASRCKF）算法设计了自适应子系统，根据2种子系统的特点和局限提出全局融合架构，使系统可以充分平衡并利用滤波过程中先验的模型预测值信息和后验的量测值信息，最终降低估计误差。仿真结果表明：相比鲁棒多渐消因子容积卡尔曼滤波（RMCKF）等算法，所提融合算法在滤波精度、稳定性和收敛速度等方面有明显优势。
-
关键词:
- 平方根容积卡尔曼滤波 /
- 模型误差 /
- 异常量测值 /
- 多传感器融合 /
- 自适应滤波
Abstract:To deal with the problem that the square-root cubature Kalman filter (SRCKF) will have declined filtering performance or even filtering diverge when model errors and abnormal measurements occur, an adaptively robust multi-sensor fusion algorithm is proposed. Firstly, a robust subsystem is designed based on the innovation covariance matching principle to suppress abnormal measurements. Then, to overcome the model errors, an adaptive subsystem is designed based on the low complexity adaptive SRCKF (LCASRCKF) algorithm. Finally, according to the characteristics and limitations of the two subsystems, a global fusion architecture is proposed, which enables the system to fully balance and utilize the prior model prediction information and the posterior measurement information in the filtering process, and thus reduce the estimation error. The simulation results show that the proposed fusion algorithm has obvious advantages in terms of filtering accuracy, stability and convergence speed compared with the robust multiple fading factors cubature Kalman filter (RMCKF) and other algorithms.
-
表 1 平均RMSE对比（场景1）
Table 1. Comparison of mean RMSEs (scenario 1)
算法 x方向位置
平均RMSE/my方向位置
平均RMSE/m融合算法 4.7445 4.7114 SRCKF 21.0091 17.7316 LCASRCKF 6.2121 6.1881 RMCKF 6.5870 6.3279 
下载: 导出CSV
表 2 平均RMSE对比（场景2）
Table 2. Comparison of mean RMSEs (scenario 2)
算法 x方向位置
平均RMSE/my方向位置
平均RMSE/m融合算法 4.964 3 4.797 0 SRCKF 94.409 0 47.955 4 LCASRCKF 67.171 1 33.293 6 RMCKF 58.766 6 28.517 5
下载: 导出CSV
-
[1] ARASARATNAM I, HAYKIN S. Cubature Kalman filters[J]. IEEE Transactions on Automatic Control, 2009, 54(6): 1254-1269. doi: 10.1109/TAC.2009.2019800 [2] ARASARATNAM I, HAYKIN S, HURO T R. Cubature Kalman filtering for continuous-discrete systems: Theory and simulations[J]. IEEE Transactions on Signal Processing, 2010, 58(10): 4977-4993. doi: 10.1109/TSP.2010.2056923 [3] 王小旭, 潘泉, 黄鹤, 等. 非线性系统确定采样型滤波算法综述[J]. 控制与决策, 2012, 27(6): 801-812. doi: 10.13195/j.cd.2012.06.4.wangxx.012WANG X X, PAN Q, HUANG H, et al. Overview of deterministic sampling filtering algorithms for nonlinear system[J]. Control and Decision, 2012, 27(6): 801-812(in Chinese). doi: 10.13195/j.cd.2012.06.4.wangxx.012 [4] JULIER S J, UHLMANN J K. A general method for approximating nonlinear transformation of probability distributions[EB/OL]. (1996-11-01) [2021-04-01]. http://www.eng.ox.ac.uk/. [5] ZHANG A, BAO S D, GAO F, et al. A novel strong tracking cubature Kalman filter and its application in maneuvering target tracking[J]. Chinese Journal of Aeronautics, 2019, 32(11): 2489-2502. doi: 10.1016/j.cja.2019.07.025 [6] YUE Z, LIAN B W, TONG K X, et al. Novel strong tracking square-root cubature Kalman filter for GNSS/INS integrated navigation system[J]. IET Radar, Sonar & Navigation, 2019, 13(6): 976-982. [7] 徐博, 刘德政, 张勋. 基于交互式模型的多AUV协同导航鲁棒性滤波算法[J]. 系统工程与电子技术, 2017, 39(9): 2087-2093. doi: 10.3969/j.issn.1001-506X.2017.09.24XU B, LIU D Z, ZHANG X. Multiple AUV cooperative navigation algorithm of robust filter based on interacting model[J]. Systems Engineering and Electronics, 2017, 39(9): 2087-2093(in Chinese). doi: 10.3969/j.issn.1001-506X.2017.09.24 [8] ZHANG H W, XIE J W, GE J A, et al. Adaptive strong tracking square-root cubature Kalman filter for maneuvering aircraft tracking[J]. IEEE Access, 2018, 6: 10052-10061. doi: 10.1109/ACCESS.2018.2808170 [9] ZHANG H W, XIE J W, GE J A, et al. Strong tracking SCKF based on adaptive CS model for manoeuvring aircraft tracking[J]. IET Radar, Sonar & Navigation, 2018, 12(7): 742-749. [10] HAN B, HUANG H Q, LEI L, et al. An improved IMM algorithm based on STSRCKF for maneuvering target tracking[J]. IEEE Access, 2019, 7: 57795-57804. doi: 10.1109/ACCESS.2019.2912983 [11] 鲁平, 赵龙, 陈哲. 改进的Sage-Husa自适应滤波及其应用[J]. 系统仿真学报, 2007, 19(15): 3503-3505. doi: 10.3969/j.issn.1004-731X.2007.15.034LU P, ZHAO L, CHEN Z. Improved Sage-Husa adaptive filtering and its application[J]. Journal of System Simulation, 2007, 19(15): 3503-3505(in Chinese). doi: 10.3969/j.issn.1004-731X.2007.15.034 [12] MEHRA R. Approaches to adaptive filtering[J]. IEEE Transactions on Automatic Control, 1972, 17(5): 693-698. doi: 10.1109/TAC.1972.1100100 [13] 周东华, 席裕庚, 张钟俊. 一种带多重次优渐消因子的扩展卡尔曼滤波器[J]. 自动化学报, 1991, 17(6): 689-695.ZHOU D H, XI Y G, ZHANG Z J. A suboptimal multiple fading extended Kalman filter[J]. Acta Automatica Sinica, 1991, 17(6): 689-695(in Chinese). [14] 徐树生, 林孝工, 李新飞. 强跟踪自适应平方根容积卡尔曼滤波算法[J]. 电子学报, 2014, 42(12): 2394-2400. doi: 10.3969/j.issn.0372-2112.2014.12.009XU S S, LIN X G, LI X F. Strong tracking adaptive square-root cubature Kalman filter algorithm[J]. Acta Electronica Sinica, 2014, 42(12): 2394-2400(in Chinese). doi: 10.3969/j.issn.0372-2112.2014.12.009 [15] LI N, ZHU R H, ZHANG Y G. A strong tracking square-root CKF algorithm based on multiple fading factors for target tracking[C]//2014 Seventh International Joint Conference on Computational Sciences and Optimization. Piscataway: IEEE Press, 2014: 16-20. [16] 张浩为, 谢军伟, 葛佳昂, 等. 自适应CS模型的强跟踪平方根容积卡尔曼滤波算法[J]. 系统工程与电子技术, 2019, 41(6): 19-27. doi: 10.3969/j.issn.1001-506X.2019.06.03ZHANG H W, XIE J W, GE J A, et al. Strong tracking square-root cubature Kalman filter over adaptive current statistical model[J]. Systems Engineering and Electronics, 2019, 41(6): 19-27(in Chinese). doi: 10.3969/j.issn.1001-506X.2019.06.03 [17] ZHANG A, BAO S D, BI W H, et al. Low-cost adaptive square-root cubature Kalman filter for systems with process model uncertainty[J]. Journal of Systems Engineering and Electronics, 2016, 27(5): 945-953. doi: 10.21629/JSEE.2016.05.01 [18] 李春辉, 马健, 杨永建, 等. 低复杂度自适应容积卡尔曼滤波算法[J]. 北京航空航天大学学报, 2022, 48(4): 716-724.LI C H, MA J, YANG Y J, et al. Low complexity adaptive cubature Kalman filter algorithm[J]. Journal of Beijing University of Aeronautics and Astronautics, 2022, 48(4): 716-724 (in Chinese). [19] MARONNA R A, MARTIN R D, YOHAI V J, et al. Robust statistics: Theory and methods(with R)[M]. New York: John Wiley & Sons, 2019. [20] SOKEN H E, HAJIYEV C. Pico satellite attitude estimation via robust unscented Kalman filter in the presence of measurement faults[J]. ISA Transactions, 2010, 49(3): 249-256. doi: 10.1016/j.isatra.2010.04.001 [21] SOKEN H E, HAJIYEV C, SAKAI S. Robust Kalman filtering for small satellite attitude estimation in the presence of measurement faults[J]. European Journal of Control, 2014, 20(2): 64-72. doi: 10.1016/j.ejcon.2013.12.002 [22] YANG Y X. Robust Bayesian estimation[J]. Bulletin Géodésique, 1991, 65(3): 145-150. [23] HU J S, YANG C H. Second-order extended H∞ filter for nonlinear discrete-time systems using quadratic error matrix approximation[J]. IEEE Transactions on Signal Processing, 2011, 59(7): 3110-3119. doi: 10.1109/TSP.2011.2138700 [24] HUANG Y L, ZHANG Y G, LI N, et al. Robust student’s t based nonlinear filter and smoother[J]. IEEE Transactions on Aerospace and Electronic Systems, 2016, 52(5): 2586-2596. doi: 10.1109/TAES.2016.150722 [25] BILIK I, TABRIKIAN J. Maneuvering target tracking in the presence of glint using the nonlinear Gaussian mixture Kalman filter[J]. IEEE Transactions on Aerospace and Electronic Systems, 2010, 46(1): 246-262. doi: 10.1109/TAES.2010.5417160 [26] YANG Y X. Adaptive navigation and kinematic positioning[M]. Beijing: Surveying and Mapping Press, 2006: 95-97. [27] YANG Y X, HE H, XU G. Adaptively robust filtering for kinematic geodetic positioning[J]. Journal of Geodesy, 2001, 75(2-3): 109-116. doi: 10.1007/s001900000157 [28] 杨元喜, 任夏, 许艳. 自适应抗差滤波理论及应用的主要进展[J]. 导航定位学报, 2013, 1(1): 9-15. doi: 10.3969/j.issn.2095-4999.2013.01.003YANG Y X, REN X, XU Y. Main progress of adaptively robust filter with applications in navigation[J]. Journal of Navigation and Positioning, 2013, 1(1): 9-15(in Chinese). doi: 10.3969/j.issn.2095-4999.2013.01.003 [29] WANG Y D, SUN S M, LI L. Adaptively robust unscented Kalman filter for tracking a maneuvering vehicle[J]. Journal of Guidance, Control, and Dynamics, 2014, 37(5): 1696-1701. doi: 10.2514/1.G000257 [30] QIU Z B, QIAN H M, WANG G Q. Adaptive robust cubature Kalman filtering for satellite attitude estimation[J]. Chinese Journal of Aeronautics, 2018, 31(4): 806-819. doi: 10.1016/j.cja.2018.01.023 [31] 胡振涛, 曹志伟, 李松, 等. 基于容积卡尔曼滤波的异质多传感器融合算法[J]. 光电子·激光, 2014, 25(4): 697-703.HU Z T, CAO Z W, LI S, et al. Heterogeneous multi-sensor fusion algorithm based on cubature Kalman filter[J]. Journal of Optoelectronics·Laser, 2014, 25(4): 697-703(in Chinese). [32] 徐树生, 林孝工. 基于鲁棒CKF的多传感器全信息融合算法[J]. 电机与控制学报, 2013, 17(2): 90-97. doi: 10.3969/j.issn.1007-449X.2013.02.014XU S S, LIN X G. Multi-sensor all information fusion algorithm based on robust CKF[J]. Electric Machines and Control, 2013, 17(2): 90-97(in Chinese). doi: 10.3969/j.issn.1007-449X.2013.02.014 [33] 徐树生, 林孝工, 赵大威, 等. 强跟踪SRCKF及其在船舶动力定位中的应用[J]. 仪器仪表学报, 2013, 34(6): 67-73. doi: 10.3969/j.issn.0254-3087.2013.06.010XU S S, LIN X G, ZHAO D W, et al. Strong tracking SRCKF and its application in vessel dynamic positioning[J]. Chinese Journal of Scientific Instrument, 2013, 34(6): 67-73(in Chinese). doi: 10.3969/j.issn.0254-3087.2013.06.010 [34] 凌林本, 李滋刚, 陈超英, 等. 多传感器数据融合时权的最优分配原则[J]. 中国惯性技术学报, 2000, 8(2): 33-36.LING L B, LI Z G, CHEN C Y, et al. Optimal weight distribution principle used in the fusion of multi-sensor data[J]. Journal of Chinese Inertial Technology, 2000, 8(2): 33-36(in Chinese). -
下载:
点击查看大图
图(8) / 表(2)
计量
- 文章访问数: 312
- HTML全文浏览量: 75
- PDF下载量: 59
- 被引次数: 0
