2. 海军航空大学,山东省烟台市,264001;
3. 山东东海热电有限公司,山东省龙口市东海工业园,265713
无迹卡尔曼滤波UKF在GNSS/SINS组合导航系统中具有高精度特性[1]。但UKF是在量测噪声方差恒定的基础上建立的,当量测噪声方差发生变化时,其滤波精度会降低。目前主要采用基于新息的自适应滤波方法解决该问题。田甜[2]将自适应因子引入UKF算法中以消除粗差对系统的影响;陈光武等[3]利用UKF对组合导航进行处理,当GPS信号丢失时利用训练好的神经网络来增强UKF的滤波效果,但该算法严重依赖训练样本的质量与数量;吴涛等[4]引入时变噪声估计器来估计时变噪声以提高UKF滤波的自适应能力,但其状态方程依然采取线性化处理方式,限制了滤波精度的提高;马晓杰等[5]引入自适应因子以减小滤波器初值偏差和系统模型异常扰动对UKF的影响,但该算法在量测噪声异常扰动的情况下性能不佳。
基于Sage-Husa的自适应滤波在组合导航系统中应用广泛,但该方法存在对滤波初值敏感、直接使用时滤波易发散以及噪声统计特性估计有偏差等缺陷[6]。而基于变分贝叶斯原理的自适应滤波方法可以克服上述缺点,并能实现系统状态与量测噪声方差的同步估计[7]。基于上述讨论,本文重点针对量测噪声异常扰动情况,引入变分贝叶斯原理,提出一种自适应UKF算法以适应量测噪声方差的变化。
1 组合导航非线性滤波模型以ENU地理坐标系为导航坐标系,基于UKF的GNSS/SINS组合导航系统状态方程为[7]:
{˙X(t)=f(X(t),ωbib,fb,t)X(t)=[γθφvEvNvUλLH]T | (1) |
式中,X(t)为系统状态向量;γ、θ、φ分别为横滚角、俯仰角和航向角;vE、vN、vU分别为E、N和U向速度;λ、L、H分别为经度、纬度和高度;ωibb为陀螺仪输出;fb为加速度计输出;t为时间。
假设导航系统的先验状态、过程噪声与未知量测噪声间相互独立。选取GNSS输出速度和位置作为观测量,即
{Z(t)=H(t)X(t)+V(t)Z(t)=[vEvNvUλLH]TH(t)=[06×3I6×6] | (2) |
式中,Z(t)和V(t)分别为量测向量和量测噪声向量,且满足V(t)∈N(0, R(t))。
2 组合导航自适应UKF算法 2.1 变分贝叶斯原理变分贝叶斯算法近似使用一种分解的自由形式递归逼近误差状态和量测噪声方差的联合后验分布,然后通过递归算法中的定点迭代估计状态和量测噪声的方差[8]。在系统噪声方差已知、量测噪声方差未知的情况下,变分贝叶斯算法可分为预测阶段和更新阶段[9]。预测阶段可利用C-K(Chapman-Kolmogorov)方程拟合出误差状态和量测噪声方差的条件分布。
预测阶段:
p(Xk,Rk∣Z1:k−1)=∫p(Xk∣Xk−1)p(Rk∣Rk−1)⋅p(Xk−1,Rk−1∣Z1:k−1)⋅dXk−1⋅dRk−1 | (3) |
更新阶段:
p(Xk,Rk∣Z1:k)∝p(Zk∣Xk,Rk)p(Xk,Rk∣Z1:k−1) | (4) |
虽然式(3)和式(4)在理论上最优,但由于引入了噪声方差矩阵及多维数值积分,因此在实际情况中很难求解。而变分贝叶斯原理可以采用近似的方法,利用多个已知分布近似求解后验分布。根据变分贝叶斯原理,式(3)、(4)中的量测方差与联合预测和后验分布可转换为如下的逆Gamma分布和高斯分布:
p(Xk,Rk∣Z1:k−1)=N(Xk∣ˆXk∣k−1,Pk∣k−1)⋅m∏i=1IG(σ2k,i∣αk∣k−1,i,βk∣k−1,i) | (5) |
p(Xk,Rk∣Z1:k)=N(Xk∣ˆXk∣k,Pk∣k)⋅m∏i=1IG(σ2k,i∣αk∣k,i,βk∣k,i) | (6) |
式中,σk, i2为高斯分布的未知噪声方差。
引入预测加权系数ρi∈(0, 1],对式(3)中的量测噪声方差的预测分布作一阶近似:
{αk∣k−1,i=ρiαk−1∣k−1,iβk∣k−1,i=ρiβk−1∣k−1,i | (7) |
式中,αk-1|k-1, i和βk-1|k-1, i为逆Gamma分布的2个参数,详细推导过程见文献[10]。
2.2 自适应UKF算法将变分贝叶斯原理与UKF算法结合,得到适用于GNSS/SINS组合导航系统的自适应UKF算法。系统增广状态向量如下:
{Xa=[XW]Tχa=[(χX)T(χW)T]T | (8) |
式中,χa的维数定义为n。具体步骤如下:
1) 初始化。
{ˆX0=E[X0]P0=E[(X0−ˆX0)(X0−ˆX0)T]ˆXa0=E[Xa]=[ˆXT00]TPa0=E[(Xa0−ˆXa0)(Xa0−ˆXa0)T]=[P00Q] | (9) |
2) 样点计算。
{χa0,k−1=ˆXak−1χai,k−1=ˆXak−1+(√(n+λ)Pak−1)i,i=1,⋯,nχai,k−1=ˆXak−1−(√(n+λ)Pak−1)i,i=n+1,⋯,2n | (10) |
式中,
{Wm0=λn+λWc0=λn+λ+(1−α2+β)Wmi=Wci=12(n+λ),i=1,⋯,2n | (11) |
式中,λ=α2(n+κ)-n;α、β和κ为比例因子。
3) 时间更新。
{αk∣k−1=ρ⋅αk−1∣k−1βk∣k−1=ρ⋅βk−1∣k−1 | (12) |
{χXi,k∣k−1=f(χXi,k−1,χWi,k−1)ˆXk∣k−1=2La∑i=0WmiχXi,k∣k−1 | (13) |
Pk∣k−1=2n∑i=0Wci[XXi,k∣k−1−ˆXk∣k−1][XXi,k∣k−1−ˆXk∣k−1]T | (14) |
4) 量测更新。
ˆZk∣k−1=HˆXk∣k−1 | (15) |
{PZZ=HPk∣k−1HT+ˆRPXZ=Pk∣k−1HT | (16) |
{αk∣k=12+αk−1∣k−1β−k∣k=βk∣k−1ˆRk+1=diag(β−k∣k⋅/αk∣k)βk∣k=(Zk−HˆXk∣k−1)⋅(Zk−HˆXk∣k−1)/2+diag[Hk∣k−1HT]/2+β−k∣k | (17) |
式中,
5) 滤波更新。
{Kk=PXZP−1ZZˆXk=ˆXk∣k−1+Kk(Zk−ˆZk∣k−1)Pk=Pk∣k−1−KkPZZKTk=Pk∣k−1−KkPTXZ | (18) |
以小型飞行器作为载体构建仿真系统,载体飞行航迹包含加速、爬升、转弯等各种运动。载体起飞初始经度、纬度、高度分别为29°E、118°N、50 m,初始航向角为90°,飞行时间为3 600 s,捷联解算周期为0.02 s,滤波周期为1 s。滤波初始参数设置如下:三维姿态角误差均为0.5°,三维速度误差均为0.1 m/s,三维位置误差均为5 m。设定陀螺随机游走驱动噪声及陀螺白噪声均为0.2°/h,加速度计随机游走驱动噪声及陀螺白噪声均为0.5×10-5g。GNSS位置误差和测速误差分别为8 m和0.2 m/s,采样周期为1 s。在仿真实验中,式(12)、式(17)中 ρ、α、β的初始值分别取为:
{ρ=[0.990.990.990.990.990.99]α=[111111]β=[0.10.10.1222] | (19) |
根据GNSS测量精度设定及式(2),正常情况下,GNSS量测噪声的均方差可以表示为:
Rroot =sqrtm(R)=diag([0.20.20.2888]) | (20) |
式中,sqrtm(·)为对矩阵中每个元素取开平方的函数;diag(·)为用于构造对角矩阵的函数。
在仿真过程的不同时段内设定GNSS量测噪声的突变与缓变2种情况的均方差:
rreal ={6rroot ,600<t⩽1200rroot [1+9sin(2π⋅t−24001200)],2400<t⩽3000rroot , others | (21) |
根据上述设定,分别进行基于常规UKF算法及自适应UKF算法的GNSS/SINS组合导航系统性能仿真验证。图 1~3分别为位置误差、速度误差及姿态误差对比曲线。
![]() |
图 1 位置误差 Fig. 1 Position error |
![]() |
图 2 速度误差 Fig. 2 Velocity error |
![]() |
图 3 姿态误差 Fig. 3 Attitude error |
由图 1~3可见,在0~600 s内,GNSS量测噪声的实际均方差与正常情况完全一致,此时常规UKF算法与本文算法得到的各导航参数误差完全相同;而在GNSS量测噪声的均方差发生突变与缓变的2个阶段内,本文算法得到的各导航参数误差明显小于常规UKF算法。
为了评估本文算法对GNSS量测噪声均方差变化的跟踪能力,绘制本文算法对GNSS位置量测噪声均方差的跟踪图(图 4)。由图可见,本文算法能实时跟踪并反映出GNSS位置量测噪声均方差的变化情况,但有一定的滞后性。对于速度量测噪声均方差,本文算法得到的跟踪图与图 4相似。
![]() |
图 4 GNSS位置均方差跟踪图 Fig. 4 Tracking figures of MSE of GNSS position |
为了详细说明GNSS量测噪声均方差变化终止后各导航参数的误差变化情况,列出不同时段内部分导航参数的误差曲线(图 5、6)。
![]() |
图 5 1 200~2 400 s期间姿态误差曲线 Fig. 5 Attitude error curve from 1 200 s to 2 400 s |
![]() |
图 6 3 000~3 600 s期间速度误差曲线 Fig. 6 Velocity error curve from 3 000 s to 3 600 s |
由图 5、6可见,本文算法能够实时跟踪GNSS量测噪声均方差的变化,当GNSS量测噪声均方差变化终止并回归正常状态后,本文算法得到的各导航参数也能够快速收敛至正常状态,并最终达到与常规UKF算法相同的滤波精度。由图 5的姿态误差曲线可见,当GNSS量测噪声均方差突变终止后,本文算法收敛速度较快。由图 6的速度误差曲线可见,当GNSS量测噪声均方差缓变终止后,常规UKF算法收敛速度较快。综上所述,整体而言,本文算法的自适应性较强且性能高于常规UKF算法。
4 结语在GNSS/SINS组合导航系统常规UKF算法的时间更新及量测更新过程中引入变分贝叶斯估计理论,提出GNSS/SINS组合导航系统的自适应UKF算法以提高系统在量测噪声统计特性发生变化时的滤波精度。仿真结果表明,该算法能有效跟踪量测噪声均方差的变化,提高组合导航系统的滤波精度。当量测噪声均方差变化终止时,该算法也能快速收敛至正常状态。相比于常规UKF算法,本文算法优势明显。
由于本文自适应滤波算法涉及到3个参数,因此3个参数对量测噪声均方差估计精度的影响规律是下一步的研究重点。
[1] |
洪志强. 无迹卡尔曼滤波的改进算法及其在GPS/INS组合导航中的应用研究[D]. 抚州: 东华理工大学, 2019 (Hong Zhiqiang. Improved Algorithm of Unscented Kalman Filter and Its Application in GPS/INS Integrated Navigation[D]. Fuzhou: East China University of Science and Technology, 2019)
( ![]() |
[2] |
田甜. 基于自适应UKF的SINS/GPS紧组合导航系统的应用研究[D]. 青岛: 青岛科技大学, 2019 (Tian Tian. Study on the Application of Based on Unscented Kalman Filter of SINS/GPS Tight Coupling Integrated Navigation System[D]. Qingdao: Qingdao University of Science and Technology, 2019)
( ![]() |
[3] |
陈光武, 程鉴皓, 杨菊花, 等. 基于改进神经网络增强自适应UKF的组合导航系统[J]. 电子与信息学报, 2019, 41(7): 1 766-1 773 (Chen Guangwu, Cheng Jianhao, Yang Juhua, et al. Improved Neural Network Enhanced Navigation System of Adaptive Unsented Kalman Filter[J]. Journal of Electronics and Information Technology, 2019, 41(7): 1 766-1 773)
( ![]() |
[4] |
吴涛, 戴卿, 匡秀梅, 等. 组合导航数据融合自适应UKF算法研究[J]. 西安航空学院学报, 2020, 38(3): 71-77 (Wu Tao, Dai Qing, Kuang Xiumei, et al. A Novel Efficient Adaptive UKF Algorithm for Integrate Navigation Data Fusion Processes[J]. Journal of Xi'an Aeronautical University, 2020, 38(3): 71-77)
( ![]() |
[5] |
马晓杰, 林雪原, 孙巧妍, 等. 一种改进的UKF滤波算法在BDS/SINS组合导航系统中的应用研究[J]. 大地测量与地球动力学, 2021, 41(4): 351-356 (Ma Xiaojie, Lin Xueyuan, Sun Qiaoyan, et al. Improved UKF Algorithm for BDS/SINS Integrated Navigation System[J]. Journal of Geodesy and Geodynamics, 2021, 41(4): 351-356)
( ![]() |
[6] |
付梦印, 邓志红, 张继伟. Kalman滤波理论及其在组合导航系统中的应用[M]. 北京: 科学出版社, 2003 (Fu Mengyin, Deng Zhihong, Zhang Jiwei. Kalman Filtering Theory and Its Application in Integrated Navigation System[M]. Beijing: Science Press, 2003)
( ![]() |
[7] |
Vrettas M D, Cornford D, Opper M. Estimating Parameters in Stochastic Systems: A Variational Bayesian Approach[J]. Physica D: Nonlinear Phenomena, 2011, 240(23): 1 877-1 900 DOI:10.1016/j.physd.2011.08.013
( ![]() |
[8] |
Lim K L, Wang H. MAP Approximation to the Variational Bayes Gaussian Mixture Model and Application[J]. Soft Computing, 2018, 22(10): 3 287-3 299 DOI:10.1007/s00500-017-2565-z
( ![]() |
[9] |
Davari N, Gholami A. Variational Bayesian Adaptive Kalman Filter for Asynchronous Multirate Multi-Sensor Integrated Navigation System[J]. Ocean Engineering, 2019, 174: 108-116 DOI:10.1016/j.oceaneng.2019.01.012
( ![]() |
[10] |
Sarkka S, Nummenmaa A. Recursive Noise Adaptive Kalman Filtering by Variational Bayesian Approximations[J]. IEEE Transactions on Automatic Control, 2009, 54(3): 596-600
( ![]() |
2. Naval Aeronautical University, Yantai 264001, China;
3. Shandong Donghai Thermal Power Co Ltd, Donghai Industrial Park, Longkou 265713, China