改进的强跟踪自适应UKF算法及其在大方位失准角对准中的应用

李 明，柴洪洲，靳凯迪，王 敏，宋开放

改进的强跟踪自适应UKF算法及其在大方位失准角对准中的应用

李 明，柴洪洲，靳凯迪，王 敏，宋开放

（信息工程大学地理空间信息学院，郑州 450001）

针对无迹卡尔曼滤波（UKF）易受系统模型参数失配、状态变化情况影响，导致滤波精度下降甚至发散问题，提出一种改进的强跟踪自适应无迹卡尔曼滤波（STAUKF）。将强跟踪滤波（STF）与UKF滤波结合，并引入多重渐消因子，有针对性地自动调节状态估计均方误差阵。根据新息向量构造检验门限函数，提高了滤波对有用历史信息的利用率。进一步引入简化的萨格-胡萨（Sage-Husa）滤波，自适应调节量测噪声方差，较传统Sage-Husa算法减少了计算量，提高了算法的鲁棒性。最后采用海上实测数据进行实验验证，并与UKF滤波、强跟踪UKF滤波（STUKF）比较。结果表明，该算法优势明显，有效缩短了大方位失准角误差收敛时间，提高了组合导航精度。较UKF滤波方位角收敛时间缩短了93%，东、北、天方向速度均方根误差分别降低89%、93%和82%，位置均方根误差分别降低98%、94%和97%。

强跟踪滤波；
自适应；
无迹卡尔曼滤波；
多重渐消因子；
组合导航

捷联惯性导航系统（strapdown inertial navigation system, SINS）具有无源、隐蔽性高、短时高精度等优点，但导航误差随时间累积较快，需要其他传感器辅助导航[1]。全球卫星导航系统（global navigation satellite system，GNSS）定位精度高，有助于提高导航精度、完好性和可用性[2]。SINS/GNSS组合导航被广泛用于海洋航行、航空、汽车导航等领域[3]。

为处理组合导航非线性系统问题，文献[5]采用扩展卡尔曼滤波（extended Kalman filter，EKF）处理大方位失准角下组合导航系统的非线性问题，但EKF通过泰勒（Taylor）展开对非线性系统进行一阶线性化近似，忽略二阶以上误差项，引入了截断误差，且需要解算复杂的雅克比矩阵，易导致滤波性能次优甚至发散[6]。文献[7]通过仿真实验证明了无迹卡尔曼滤波（unscented Kalman filter, UKF）较EKF具有更高的估计精度，是一种处理非线性误差模型的实用方法。文献[8]基于粒子滤波，通过计算高斯核的最优宽带参数，提高了粒子滤波的计算效率。粒子滤波与UKF滤波核心思想均为通过选择西格马（Sigma）样本点近似概率密度分布，但UKF只需要确定性选择Sigma点，计算量远小于粒子滤波。当粒子滤波近似概率密度远离真实概率密度时，滤波效果变差甚至发散[9]。文献[10]针对卫星信号受到遮挡等导致观测质量下降的情况，提出基于QR分解的容积卡尔曼滤波（cubature Kalman filter，CKF），并应用于SINS/GNSS组合导航非线性系统，减少了计算量，提高了滤波精度。但CKF滤波对先验信息统计特性较为敏感，不准确的先验信息将导致CKF滤波严重震荡，降低组合导航精度[11]。文献[12]通过蒙特卡罗仿真实验，表明CKF滤波只对线运动敏感，对角运动并不敏感，而UKF滤波可同时敏感线运动和角运动，2种运动方向的激励作用均可缩短误差收敛时间。综合上述分析，UKF滤波用于非线性组合导航系统优势明显，但UKF滤波容易受系统模型参数失配影响，且对系统突变状态较为敏感，降低滤波鲁棒性。

针对UKF滤波在系统模型参数失配情况下鲁棒性降低，系统状态变化跟踪能力不足的问题，本文提出一种改进的强跟踪自适应无迹卡尔曼滤波。首先，将UKF滤波与强跟踪滤波结合，提高了滤波对于系统模型参数失配问题的鲁棒性；
然后，相对于传统单渐消因子强跟踪滤波，本文引入多重渐消因子，更具针对性的自动调节状态估计均方误差阵；
进一步根据新息序列构造了检验门限函数，相较于传统强跟踪滤波持续引入渐消因子放大状态均方误差阵，提高了有效历史信息的利用率；
为进一步提高算法对于时变量测噪声的自适应能力，加入简化的萨格-胡萨（Sage-Husa）滤波，并构造了指数渐消遗忘因子，较传统Sage-Husa滤波降低了计算量，提高了滤波自适应能力。

1.1 坐标系定义

1.2 动态对准误差模型

位置误差模型可表示为

2.1 无迹卡尔曼滤波

根据式（1）、式（2）和式（6）误差模型，并增广陀螺漂移和加速度计偏置构建15维非线性状态方程和6维线性量测方程为：

UKF滤波通过无迹变换传播和更新非线性模型的状态均值和误差协方差阵，选取有限个加权Sigma采样点近似系统状态的先验统计特性，再通过非线性方程得到系统状态的后验概率密度分布特性[14]。UKF滤波计算过程为：

状态预测值及其均方差阵更新为：

2）量测更新。量测方程为线性系统，得量测更新过程[16]为：

2.2 强跟踪滤波基本原理

STF需要满足的条件为

式（24）使STF满足最小方差估计的基础上，使其残差序列时刻保持正交，最大化利用其中有用信息，加强对模型参数失配情况的鲁棒性，提高对状态变化的实时跟踪能力[17]。

2.3 改进的强跟踪自适应无迹卡尔曼滤波

结合式（18）至式（22）及式（25）至式（34）得改进的强跟踪自适应无迹卡尔曼滤波量测更新过程为：

为验证本文所提算法的有效性，采用浙江省舟山市海上船载实测数据进行实验验证，实测轨迹如图1所示，船载实验设备如图2所示。数据采集于2021年11月15日，GNSS设备采用华测P5型接收机，输出频率1 Hz，可输出GNSS三维速度和三维位置信息。SINS设备采用Novatel Span ISA-100C光纤捷联惯导系统，输出频率200 Hz，SINS标称参数详见表1。采用高精度数据处理软件（Novatel inertial explorer, IE）的GNSS/SINS平滑组合解算结果作为参考值。

图1 舟山船载海上实验轨迹

图2 船载实验设备

表1 海上实验SINS主要参数

载体运动中SINS参数一般不会与标称参数完全契合，且一般会随时间变化，真实的SINS参数较难获得。为验证本文算法对于系统模型参数失配的鲁棒性，SINS设备参数采用表1数据。初始方位角加入1 °横滚角误差和俯仰角误差，以及90 °方位角误差。为方便实验结果对比和分析，定义UKF滤波为算法1，融合强跟踪滤波的STUKF为算法2，本文所提算法为算法3。

横滚角和俯仰角对比参考值并不明显，只给出方位角对比参考值曲线，如图3所示。由图3可知，算法3只需要约40 s可以跟踪到载体实际方位角变化，之后与参考值曲线吻合较好；
算法2次之，需要约200 s；
而算法1所需时间最长，直到1400 s时才能跟踪到载体实际方位角变化。表明系统模型参数失配情况下，算法2和算法3较算法1具有更强跟踪载体真实姿态变化的能力，且算法3略优于算法2。图4～图6给出了4种算法横滚角、俯仰角和方位角误差的对比曲线，相应均方根误差（100 s后）对比情况如表2所示。

图3 方位角对比参考值曲线

图4 横滚角误差对比曲线

图5 俯仰角误差对比曲线

图6 方位角误差对比曲线

表2 姿态均方根误差对比表 (°)

由图4和图5可知：算法1的横滚角误差和俯仰角误差曲线均出现较大幅度波动，俯仰角误差曲线波动更为明显；
而算法2只出现较小幅度波动，是因为强跟踪UKF滤波对于系统模型参数失配情况具有较强的鲁棒性；
算法3波动幅度最小，尤其横滚角和方位角误差曲线几乎未出现明显波动，算法3鲁棒性最强。算法3需要100 s方位角误差收敛到0°；
算法2次之，需要650 s；
而算法1只能在1400 s时方位角误差收敛至1 °，最终收敛精度为0.5°。算法3较算法2收敛时间缩短85%，较算法1收敛时间缩短93%，最终收敛精度提高0.5 °。从表2可知，算法3较算法1和算法2的均方根误差俯仰角分别降低73%和48%，横滚角分别降低78%和17%，方位角分别降低98%和90%。证明系统模型参数失配情况下，算法3姿态角收敛性能优势明显，所需收敛时间短，误差收敛精度高；
算法2次之；
算法1最差。为进一步对比和分析3种算法组合导航系统三维速度和三维位置，图7给出了参考值东向和北向速度，图8至图13给出了3种算法速度和位置误差对比曲线，相应的均方根误差对比情况如表3所示。

图7 参考值东向速度和北向速度

图8 东向速度误差对比曲线

图9 北向速度误差对比曲线

图10 天向速度误差对比曲线

图11 北向位置误差对比曲线

图12 东向位置误差对比曲线

图13 高度误差对比曲线

图14 GNSS速度误差对比曲线

表3 组合导航系统速度和位置均方根误差对比

由图3和图7可知，载体在200 ～300、600～ 1000、1200 ～1300 s之间多次出现较大幅度的方位角和速度变化，可由此进一步验证本文算法的强跟踪能力。由图8至图13可知：在组合导航系统状态变化期间，算法1三维速度和位置误差曲线均出现较大幅度波动，鲁棒性较差；
而算法2只出现较小波动，体现了强跟踪滤波状态变化下的鲁棒性；
对比算法1和算法2，算法3几乎未出现波动情况，误差曲线基本保持平滑，说明算法3进一步提高了系统状态变化情况下的鲁棒性。

由表3可知，算法3较算法1和算法2均方根误差的东向速度分别降低89%和42%，北向速度分别降低93%和60%，天向速度分别降低82%和50%，东向位置分别降低98%和80%，北向位置分别降低94%和50%，高度分别降低97%和90%。由以上分析可得出结论，本文算法较算法1和算法2具有系统模型参数失配情况下更好的鲁棒性，以及状态变化情况下更强的跟踪能力，组合导航性能优势明显。强跟踪滤波精度提升明显，但稳定性稍有不足，作者将进一步研究和提升其稳定性。

根据SINS/GNSS组合导航滤波过程的新息向量构造检验门限函数，将多渐消因子强跟踪滤波与UKF滤波结合，并引入简化的Sage-Husa滤波，提出一种改进的强跟踪自适应无迹卡尔曼滤波算法。采用海上船载数据进行实验验证，结果表明本文所提算法在模型参数失配情况下具有较强的鲁棒性和更好的跟踪系统状态变化的能力。总的来说，缩短了失准角误差的收敛时间，提高了组合导航精度，性能优势明显，可以为远海航行提供参考。

[1] XU Xiaofei, NIE Zhixi, WANG Zhengjie, et al. Performance assessment of BDS-3 PPP-B2b/INS loosely coupled integration[J]. Remote Sensing, 2022, 14(13): 2957.

[2] DU Zhenqiang, CHAI Hongzhou, XIAO Guorui, et al. The realization and evaluation of PPP ambiguity resolution with INS aiding in marine survey[J]. Marine Geodesy, 2020, 44(2): 136-156.

[3] NING Yipeng, WANG Jian, HAN Houzeng, et al. An optimal radial basis function neural network enhanced adaptive robust Kalman filter for GNSS/INS integrated systems in complex urban areas[J]. Sensors, 2018, 18(9): 3091.

[4] HAN Songlai, WANG Jinling. A novel initial alignment method for GPS/INS integration with large initial heading error[C]//The Institute of Navigation. Proceedings of the 22nd International Technical Meeting of the Satellite Division of The Institute of Navigation (ION GNSS 2009). Manassas, Virginia: The Institute of Navigation, Inc., 2009: 935-945[2022-08-16].

[5] LIM J N, CHAN G P. A rapid and adaptive alignment under mooring condition using adaptive EKF and CNN-based learning[J]. Sensors, 2020, 20(15): 4069.

[6] WAN E A, MERWE R. The unscented Kalman filter for nonlinear estimation[C]//The Institute of Electrical and Electronic Engineers（IEEE）. Proceedings of the IEEE 2000 Adaptive Systems for Signal Processing, Communications, and Control Symposium. Palms Springs：IEEE, 2000: 153-158[2022-08-16].

[7] 刘海鹏, 张科, 李恒年, 等. 捷联惯导系统大失准角下的初始对准研究[J]. 航空计算技术, 2012, 42(4): 39-42.

[8] SHAO Haijun, MIAO Lingjuan, GAO Weixi, et al. Ensemble particle filter based on KLD and its application to initial alignment of the SINS in large misalignment angles[J]. IEEE Transactions on Industrial Electronics, 2018, 65(11): 8946-8955.

[9] SUN Jin, XU Xiaosu, LIU Yiting, et al. Initial alignment of large azimuth misalignment angles in SINS based on adaptive UPF[J]. Sensors, 2015, 15(9): 21807-21823.

[10] 李晓明, 赵长胜, 谭兴龙. 改进的容积卡尔曼滤波的组合导航定位算法[J]. 测绘科学, 2020, 45(9): 25-30.

[11] 孙枫, 吴旭, 曹通, 等. 基于变分贝叶斯Cubature KF的SINS海上对准方法[J]. 华中科技大学学报(自然科学版), 2013, 41(1): 80-84.

[12] 徐景硕, 王勇军, 刘亚. 大失准角情形下UKF与CKF的比较研究[J]. 电光与控制, 2017, 24(9): 42-46.

[13] 严恭敏, 严卫生, 徐德民. 基于欧拉平台误差角的SINS非线性误差模型研究[J]. 西北工业大学学报, 2009, 27(4): 511-516.

[14] JULIER S J. Unscented filtering and nonlinear estimation[J]. Proceedings of the IEEE, 2004, 92(3): 401-422.

[15] GAO Bingbing, HU Gaoge, GAO Shesheng, et al. Multi-sensor optimal data fusion for INS/GNSS/CNS integration based on unscented Kalman filter[J]. International Journal of Control, Automation and Systems, 2018, 16(1): 129-140.

[16] 王维, 胡高歌, 高社生, 等. 基于极大似然准则的INS/GNSS组合导航自适应UKF滤波算法[J]. 中国惯性技术学报, 2017, 25(5): 656-663.

[17] WANG Lijun, WU Lifeng, GUAN Yong, et al. Online sensor fault detection based on an improved strong tracking filter[J]. Sensors, 2015, 15(2): 4578-4591.

[18] 严恭敏, 翁浚. 捷联惯导算法与组合导航原理[M]. 西安：西北工业大学出版社, 2019: 163-165.

[19] 薛海建, 王解, 郭晓松, 等. SINS非线性自对准中的强跟踪UKF算法设计[J]. 上海交通大学学报, 2015, 49(09): 1429-1434.

Improved strong tracking adaptive UKF algorithm and its application in large azimuth misalignment

LI Ming, CHAI Hongzhou, JIN Kaidi, WANG Min, SONG Kaifang

（Institute of Geospatial Information，Information Engineering University，Zhengzhou 450001，China）

Aiming at the problem that the unscented Kalman filter (UKF) is easily affected by the mismatch of system model parameters and state changes, which leads to the decrease of filtering accuracy and even divergence, this paper proposes an improved strong tracking adaptive unscented Kalman filter (STAUKF). Strong tracking filter (STF) and UKF are combined, and multiple fading factors are introduced to automatically adjust the mean square error matrix of state estimation. The test threshold function is constructed according to the innovation vector, which improves the utilization rate of useful historical information by filtering. Furthermore, a simplified Sage-Husa filter is introduced to adaptively adjust the measurement noise variance, which reduces the amount of computation and improves the robustness of the algorithm compared with the traditional Sage-Husa algorithm. Finally, the experimental verification is carried out using the measured data at sea, and compared with UKF and strong tracking UKF (STUKF), the results show that the algorithm has obvious advantages, effectively shortens the convergence time of large azimuth misalignment errors, and improves the accuracy of integrated navigation. Compared with the UKF, the azimuth convergence time is shortened by 93%, the velocity root mean square error in the east, north, and up directions is reduced by 89%, 93%, and 82%, respectively, and the position root mean square error is reduced by 98%, 94%, and 97%, respectively.

strong tracking filter; adaptive; unscented Kalman filter; multiple fading factor; integrated navigation

P228

A

2095-4999(2022)06-0165-08

李明，柴洪洲，靳凯迪，等. 改进的强跟踪自适应UKF算法及其在大方位失准角对准中的应用[J]. 导航定位学报, 2022, 10(6): 165-172.（LI Ming, CHAI Hongzhou, JIN Kaidi, et al.Improved strong tracking adaptive UKF algorithm and its application in large azimuth misalignment[J]. Journal of Navigation and Positioning, 2022, 10(6): 165-172.）

10.16547/j.cnki.10-1096.20220622.

2022-07-11

国家自然科学基金项目（42074014）。

李明（1990—），男，河北唐山人，硕士研究生，工程师，研究方向为GNSS/SINS组合导航。

柴洪洲（1969—），男，河北沧州人，博士，教授，研究方向为大地测量数据处理与水下导航定位。

猜你喜欢无迹失配方位角基于无迹卡尔曼滤波的室内定位系统物联网技术(2022年7期)2022-07-21考虑桥轴线方位角影响的曲线箱梁日照温差效应结构工程师(2022年2期)2022-07-15基于无差拍电流预测控制的PMSM电感失配研究电机与控制应用(2022年4期)2022-06-27T2-FLAIR 失配征预测IDH 突变-无1p/19q 共缺失型胶质瘤的研究进展国际医学放射学杂志(2021年3期)2021-11-30没停鸭绿江(2021年26期)2021-11-11一款高性价比失配负载的设计与制作电子制作(2021年11期)2021-06-17善行无迹领导文萃(2021年5期)2021-04-02近地磁尾方位角流期间的场向电流增强空间科学学报(2020年4期)2020-04-22基于停车场ETC天线设备的定位算法实现现代计算机(2018年19期)2018-08-01基于特征分解的方位向多通道SAR相位失配校正方法雷达学报(2018年3期)2018-07-18

推荐访问:对准 算法 自适应