A robust adaptive filtering algorithm based on predicted residuals in integrated navigation
-
摘要:
全球卫星导航系统(GNSS)和惯性导航系统(INS)进行融合可以有效提高导航系统的精度及可靠性,现已被广泛使用。卡尔曼滤波是常用的融合导航算法,但由于载体运动状态变化及观测粗差的影响,滤波的性能会严重降低。为了解决以上问题,构建抗差自适应卡尔曼滤波(RAKF)提高组合导航的状态估计的性能至关重要。设计了一种基于预测残差的抗差自适应滤波组合导航算法,通过构建基于预测残差的自适应因子,结合抗差估计算法,有效解决GNSS/INS组合导航中观测异常和动力学模型的异常扰动,提高滤波的稳定性、可靠性及精度。实验结果表明:与基于标准卡尔曼滤波GNSS/INS组合导航相比,所设计算法在有观测异常和动力学模型异常扰动的情况下可以有效提高组合导航的定位精度;在松组合和紧组合2种模式中,3D定位精度分别提升了45.9%和46.8%。
-
关键词:
- 预测误差 /
- 卡尔曼滤波 /
- 自适应滤波 /
- 抗差估计 /
- GNSS/INS组合导航
Abstract:Integration of the global navigation satellite system (GNSS) and the inertial navigation system (INS) can effectively improve the accuracy and reliability of the navigation system, and therefore has been widely used in many applications. In particular, the Kalman filter is commonly used in the fusion algorithm, but its performance will be severely degraded due to the vehicle motion and the low quality GNSS measurements. To deal with the above issues, a robust adaptive Kalman filter (RAKF) based algorithm has been proposed for the GNSS/INS fusion. By the construction of adaptive factor based on predicted residuals in the robust estimation process, the proposed algorithm can effectively improve the stability, reliability and accuracy of the system in the conditions of abnormal disturbance in the GNSS measurements and vehicle dynamic model. The simulation results show that the 3D positioning results obtained by the proposed algorithm is superior to the standard Kalman filter in both loosely and tightly coupled integrated navigation, with an improvement of 45.9% and 46.8%, respectively.
-
表 1 惯性导航系统参数设置
Table 1. Parameters settings of INS
传感器 参数 设置值 陀螺仪 随机常数/((°)·h−1) 0.05 随机漂移均方差/((°)·h−1) 0.05 一阶马尔可夫驱动白噪声方差/((°)·h−1) 1×10−9 相关时间/h 104 加速度计 相关时间/h 104 一阶马尔可夫驱动白噪声方差/(m·s−2) 0.3×9.81×10−6 表 2 滤波参数初值
Table 2. Initial value of filtering parameters
参数 组合
模式初值 ${\boldsymbol{\varPhi }}$ 松/紧 ${{\boldsymbol{I}}_{18 \times 18}}$ ${\boldsymbol{Q}}$ 松/紧 ${\text{diag} }\left( \underbrace {0,\cdots,0}_{\rm{9} }, \underbrace {4.3\times10^{-14},\cdots}_{ {3} },\underbrace {5\times10^{-21},\cdots}_{\rm{3} },0,0,0\right)$ $ {\boldsymbol{R}} $ 松组合 ${\text{diag} }\left(2.25,2.25,2.25 \right)$ 紧组合 ${\text{diag} }\left(\underbrace {2.25,\cdots,2.25}_{\rm{9}}\right)$ ${\boldsymbol{H}}$ 松组合 $[{ {\boldsymbol{I} }_{6 \times 6} }\quad{ {{\boldsymbol{0}}}_{6 \times 12} }]$ 紧组合 $[{ {\boldsymbol{I} }_{9 \times 9} }\quad{ {{\boldsymbol{0}}}_{9 \times 9} }]$ ${{\boldsymbol{\bar P}}_{\boldsymbol{0}}}$ 松/紧 ${\text{diag} }\left( 2.25,2.25,2.25,\;4,4,4,\;{ 1 } \times {\text{1} }{ {\text{0} }^{ - 8} }{\text{, 9} }{\text{.6} } \times {\text{1} }{ {\text{0} }^{ - 7} }\right.$, $\left.{\text{5} }{\text{.9} } \times {\text{1} }{ {\text{0} }^{ - 4} },{ { {2.25,2.25,2.25 } } } \right)$ 表 3 松组合导航场景1中的位置RMSE
Table 3. Position RMSE of loosely coupled integrated navigation in Scenario 1
m 算法 东向 北向 天向 3D位置 算法1 0.80 1.36 1.68 2.30 算法2 0.68 1.25 1.60 2.13 本文算法 0.68 1.25 1.60 2.13 表 4 松组合导航场景2中位置RMSE
Table 4. Position RMSE of loosely coupled integrated navigation in Scenario 2
m 算法类型 东向 北向 天向 3D位置 算法1 1.96 2.32 2.43 3.90 算法2 1.93 2.00 2.47 3.72 本文算法 0.67 1.22 1.60 2.11 表 5 紧组合导航场景1中位置RMSE
Table 5. Position RMSE of tightly coupled integrated navigation in Scenario 1
m 算法 东向 北向 天向 3D位置 算法1 0.74 1.28 0.78 1.66 算法2 0.64 1.29 0.68 1.59 本文算法 0.64 1.29 0.68 1.59 表 6 紧组合导航场景2位置RMSE
Table 6. Position RMSE of tightly coupled integrated navigation in Scenario 2
m 算法 东向 北向 天向 3D位置 算法1 1.54 2.47 1.09 3.10 算法2 1.44 2.28 1.03 2.89 本文算法 0.68 1.34 0.68 1.65 表 7 松组合和紧组合导航中算法的性能
Table 7. Performance of algorithm in loosely and tightly coupled integrated navigation
% 组合模式 3D定位精度提升 场景1 场景2 松组合 7.5 45.9 紧组合 4.2 46.8 -
[1] 彭慧. 多模型自适应滤波及其应用研究[D]. 成都: 电子科技大学, 2016: 5-6.PENG H. Research on multiple model adaptive estimation filtering mehtods and application study[D]. Chengdu: University of Electronic Science and Technology of China, 2016: 5-6(in Chinese). [2] 杨元喜, 张丽萍. 中国大地测量数据处理60年重要进展第一部分: 函数模型和随机模型进展[J]. 地理空间信息, 2009, 7(6): 1-5.YANG Y X, ZHANG L P. Progress of geodetic data processing for 60 years in China. Part 1: Progress of functional and stochastic model[J]. Geospatial Information, 2009, 7(6): 1-5(in Chinese). [3] MEHRA R. On the identification of variances and adaptive Kalman filtering[J]. IEEE Transactions on Automatic Control, 1970, 15(2): 175-184. doi: 10.1109/TAC.1970.1099422 [4] 杨元喜, 徐天河. 基于移动开窗法协方差估计和方差分量估计的自适应滤波[J]. 武汉大学学报(信息科学版), 2003, 28(6): 714-718.YANG Y X, XU T H. An adaptive Kalman filter combining variance component estimation with covariance matrix estimation based on moving window[J]. Geomatics and Information Science of Wuhan University, 2003, 28(6): 714-718(in Chinese). [5] 杨元喜, 何海波, 徐天河. 论动态自适应滤波[J]. 测绘学报, 2001, 30(4): 293-298. doi: 10.3321/j.issn:1001-1595.2001.04.004YANG Y X, HE H B, XU T H. Adaptive robust filtering for kinematic GPS positioning[J]. Acta Geodaetica et Cartographic Sinica, 2001, 30(4): 293-298(in Chinese). doi: 10.3321/j.issn:1001-1595.2001.04.004 [6] 杨元喜, 任夏, 许艳. 自适应抗差滤波理论及应用的主要进展[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 [7] 高为广, 封欣, 朱大为. 基于神经网络构造的GPS/INS自适应组合导航算法[J]. 大地测量与地球动力学, 2007, 27(2): 64-67.GAO W G, FENG X, ZHU D W. GPS/INS adaptively integrated navigation algorithm based on neural network[J]. Journal of Geodesy and Geodynamics, 2007, 27(2): 64-67(in Chinese). [8] 高为广, 何海波, 陈金平. 自适应UKF算法及其在GPS/INS组合导航中的应用[J]. 北京理工大学学报, 2008, 28(6): 505-509. doi: 10.15918/j.tbit1001-0645.2008.06.006GAO W G, HE H B, CHEN J P. An adaptive UKF algorithm and its application for GPS/INS integrated navigation system[J]. Transactions of Beijing Institute of Technology, 2008, 28(6): 505-509(in Chinese). doi: 10.15918/j.tbit1001-0645.2008.06.006 [9] 杨元喜. 动态系统的抗差Kaliman滤波[J]. 解放军测绘学院学报, 1997, 14(2): 79-84.YANG Y X. Robust Kalman filter for dynamic systems[J]. Journal of the PLA Institute of Surveying and Mapping, 1997, 14(2): 79-84(in Chinese). [10] 杨元喜. 抗差估计理论及其应用[M]. 北京: 八一出版社, 1993: 2-3.YANG Y X. Robust estimation theory and application[M]. Beijing: Bayi Press, 1993: 2-3(in Chinese). [11] 邱望彦. 面向通用航空的微惯性/GNSS/ADS组合导航信息融合技术研究[D]. 南京: 南京航空航天大学, 2020: 13-14.QIU W Y. Research on information fusion technology of MEMS INS/GNSS/ADS integrated navigation system for general aviation aircrafts[D]. Nanjing: Nanjing University of Aeronautics and Astronautics, 2020: 13-14(in Chinese). [12] 杨忠辉. 基于北斗的通航飞机定位方法研究[D]. 天津: 中国民航大学, 2019: 26-27.YANG Z H. Research on positioning method of general aviation aircraft based on BDS[D]. Tianjin: Civil Aviation University of China, 2019: 26-27(in Chinese). [13] 徐天河, 杨元喜. 改进的Sage自适应滤波方法[J]. 测绘科学, 2000, 25(3): 22-24.XU T H, YANG Y X. The improved method of Sage adaptive filtering[J]. Developments in Surveying and Mapping, 2000, 25(3): 22-24(in Chinese). [14] 吴富梅, 聂建亮, 何正斌. 利用预测残差和选权滤波构造的分类因子在GPS/INS组合导航中的应用[J]. 武汉大学学报(信息科学版), 2012, 37(3): 261-264.WU F M, NIE J L, HE Z B. Classified adaptive filtering to GPS/INS integrated navigation based on predicted residuals and selecting weight filtering[J]. Geomatics and Information Science of Wuhan University, 2012, 37(3): 261-264(in Chinese). [15] 高为广, 陈谷仓. 结合自适应滤波和神经网络的GNSS/INS抗差组合导航算法[J]. 武汉大学学报(信息科学版), 2014, 39(11): 1323-1328.GAO W G, CHEN G C. Integrated GNSS/INS navigation algorithms combining adaptive filter with neural network[J]. Geomatics and Information Science of Wuhan University, 2014, 39(11): 1323-1328(in Chinese). [16] YANG Y, HE H, XU G. Adaptively robust filtering for kinematic geodetic positioning[J]. Journal of Geodesy, 2001, 75(2): 109-116. [17] 吴富梅. GNSS/INS组合导航误差补偿与自适应滤波理论的拓展[J]. 测绘学报, 2011, 40(3): 401.WU F M. Error compensation and extension of adaptive filtering theory in GNSS/INS integrated navigation[J]. Acta Geodaetica et Cartographica Sinica, 2011, 40(3): 401(in Chinese). [18] 付心如. INS/GNSS组合导航抗差自适应Kalman滤波方法研究[D]. 阜新: 辽宁工程技术大学, 2017: 41-42.FU X R. Study on the method of INS/GNSS integrated navigation robust adaptive Kalman filtering[D]. Fuxin: Liaoning Technical University, 2017: 41-42(in Chinese).