改进的Sage-Husa自适应算法及应用_邓洪高.pdf

基金项目:基金项目:国家自然科学基金(61561016，61861008);广西科 技 重 大 专 项 基 金(AC16380014，AA17202048，AA17202033，AA17204002);广西自然科学基金青年科学基金项目(2019GXNSFBA245072);桂 林 电 子 科 技 大 学 研 究 生 教 育 创 新 计 划 资 助 项 目(2020YCxs027)收稿日期:20210412修回日期:20210603第 40 卷第 2 期计算机仿真2023 年 2 月文章编号:10069348(2023)02001905改进的 Sage-Husa 自适应算法及应用邓洪高，马海淘，纪元法，孙希延(桂林电子科技大学，广西 桂林 541004)摘要:针对标准卡尔曼滤波和 Sage-Husa 自适应卡尔曼滤波不能同时满足实时在线估计状态量测噪声阵和抑制滤波发散的问题，提出一种改进的 Sage-Husa 自适应卡尔曼滤波算法。在检测到量测异常时，在协方差匹配技术的最严格收敛判断条件，计算出次优滤波方法中新的状态估计均方误差阵的加权系数，通过新的状态估计均方误差阵对原来的状态估计均方误差阵进行修正，能有效抑制滤波器的发散。提高实际应用中，Sage-Husa 自适应卡尔曼滤波算法在复杂不稳定环境中的稳定性与可靠性。将改进的算法放在 BDS/INS 系统中进行实验验证，实验结果表明，相比常规 Sage-Husa 自适应滤波算法，改进的 Sage-Husa 自适应卡尔曼滤波算法改善了上述系统的可靠性和自适应能力，最终有效的提高组合导航系统的性能。关键词:组合导航;自适应滤波;次优滤波;加权系数中图分类号:TN967.2文献标识码:BImproved Sage-Husa Adaptive Algorithm and its ApplicationDENG Honggao，MA Haitao，JI Yuanfa，SUN Xinyan(Guilin University of Electronic Technology，Guilin Guangxi 541004，China)ABSTACT:Aiming at the problem that the standard Kalman filter and Sage-Husa adaptive Kalman filter cannot sat-isfy the realtime online state measurement noise matrix and suppress filter divergence at the same time，an improvedSage-Husa adaptive Kalman filter algorithm is proposed When the measurement abnormality is detected，in the moststringent convergence judgment condition of the covariance matching technology，the weighting coefficient of the newstate estimation mean square error matrix in the suboptimal filtering method is calculated，and the original state esti-mation meansquare error matrix is compared with the original The state estimation meansquare error matrix is cor-rected，which can effectively suppress the divergence of the filter The algorithm can improve the stability and reliabil-ity of the Sage-Husa adaptive Kalman filter algorithm in complex and unstable environments in practical applicationsThe improved algorithm was put in the BDS/INS system for experimental verification The experimental results showthat compared with the conventional Sage-Husa adaptive filter algorithm，The improved Sage-Husa adaptive Kalmanfilter algorithm effectively improves the reliability and adaptive ability of the system Finally，it effectively improvesthe performance of the integrated navigation systemKEYWODS:Integrated navigation;Adaptive filtering;Suboptimal filtering;Weighting coefficient1引言惯性导航系统(inertial navigation system)和卫星导航系统(satellite navigation system)各自都在技术上具有其一些优缺点，比如传统的惯性导航系统其最大的优点之一就是技术上的自主性强，隐蔽性好1，缺点之二就是误差可能会使其随着一段时间不断地积累而逐渐增大;卫星导航系统的优点之一就是它们的导航精度很高，且这个误差也不会因为时间而增大;但各个系统单独在使用过程中都会因为受到一定程度上的限制，而两个系统之间互补能力较强，因此在惯性/卫星导航系统在当前被公认的最佳组合导航系统。Kalman 自从研究出来以来，得到了广阔的应用场景，标准卡尔曼滤波对于系统很多个状态估计参数都要求设置为已知，才能够直接求得对状态估计的最优理解，比如对于系统噪声的各种统计学特性、系统模型和对于量测的噪声都要求设置为已知，91并且都是准确的，而往往在实际的应用中对于系统模型进行参数设置是不够准确的，原因为由于系统噪声、量测时间噪声受多种影响因素的直接影响导致是未知的，为了解决问题，提出了 Sage-Husa 自适应滤波23。在进行滤波的同时，利用量测数据对噪声统计特性进行更正，来提高高阶滤波的精度，减少估计误差。Sage-Husa 自适应滤波增加了噪声统计特性的计算过程，随着滤波的进行，在某个时间段内，因观测量误差增大而导致新息中引入大量误差，可能会使噪声阵出现负定性的情况，引发滤波结果发散，降低系统的稳定性和可靠性4。针对标准卡尔曼滤波和 Sage-Husa 自适应卡尔曼滤波不能同时满足实时在线估计状态量测噪声阵和抑制滤波发散的问题，本文提出一种改进的 Sage-Husa 自适应滤波，采用引入量测异常检测处理的方法，保证了噪声阵的半正定性或正定性56，抑制滤波器的发散，能够提高 Sage-Husa 自适应算法对组合导航模型的适应能力，本文分别运用标准 KF、Sage-Husa 自适应滤波和改进的 Sage-Husa 自适应滤波在相同条件下对系统进行滤波，比较不同算法的速度误差和位置误差，得出该算法提升了导航系统的定位精度。2组合导航系统数学模型建立如下的 BDS/INS 组合导航系统状态方程X=F(t)X(t)+G(t)W(t)(1)式中:状态变量为X=E，N，U，vE，vN，vU，L，h，bx，by，bz，x，y，zT(2)高斯白噪声随机误差变量为W=gx，gy，gz，0，0，0，ax，ay，az(3)误差系数矩阵 G 为G=Cnb033033093093093033033I33(4)状态系数矩阵 F 为F=FNFS0FM1515(5)其中，为 9 个导航参数矩阵FS=Cnb033033Cnb033033(6)FM=Diag 0 0 0 1Tgx1Tgy1Tgz(7)在经过卡尔曼滤波算法的修正后，为了能得到更高精度的位置和速度信息Z(t)=H(t)X(t)+V(t)(8)其中 Z=VINSVBDSPINSPBDST，VINS，VBDS分别为惯导系统计算出的速度信息和 BDS 输出的速度信息;PINS，PBDS分别为惯导系统计算出的位置信息和 BDS 输出的位置信息 H=HV，HPT，其中 HV=033diag(1，1，1)039，HP=036diag m，ncosL，1033，因此可以得到 BDS/INS 组合导航系统的量测方程为Z(t)=VVHP X(t)+Vv(t)Vp(t)(9)3Sage-Husa 自适应卡尔曼滤波算法3.1Sage-Husa 自适应卡尔曼滤波算法由上述内容可知，对组合导航系统离散化后可表示为Xk=k，k1Xk1+WkZk=HkXk+Vk(10)其中，Wk和 Vk为带有时变统计特性的噪声阵，设统计特性为:E(Wk)=qkvar(Wk)=QkE(Vk)=rkvar(Vk)=k对于上述所表示的系统，Sage-Husa 自适应卡尔曼滤波算法的步骤如下Xk/k1=k，k1Xk1+qk1Pk/k1=k，k1Pk1Tk，k1+Qk1Kk=Pk/k1HTk(HkPk/k1HTk+k)1ek=Zk HkXk/k1rk1Xk=Xk/k1+KkekPk=(I KkHk)Pk/k1(11)式中，rk1，qk1，Qk1，k由时变噪声统计估计器推导获得qk=(1 dk)qk1+dk(X k，k1Xk1)Qk=(1 dk)Qk1+dk(KkekeTkKTk+Pkk，k1Pk1Tk，k1)r=(1 dk)k1+dk(Zk HkXk1)=(1 dk)k1+dk(ekeTk HkPk1HTk)式(10)中，dk=(1b)/(1bk+1)，b 为遗忘因子，且 0b1，其取值范围一般限制在 0.95，0.99 之内。上述式中，从表面上看起来 Sage-Husa 自适应滤波算法可以在 Q 和 都是不确定的情况下，按照式(11)的流程可以把 Q 和 同时都计算出来7，但在实际上，Sage-Husa 自适应滤波算法是无法把它们同时估计计算出来的，只能在其中一个参数已知的条件下，估计出另一个参数，通过式(11)可知但新息 ek发生异常时，会同时影响 Q 和 的计算结果，很难保证滤波结果的准确性。另外从 Q 和 的计算可以看出，因为减号的存在，可能会使 Q 和 随时为非正定性，从而引发滤波发散现象。因此对算法进行改进显得很有必要的。3.2简化 Sage-Husa 自适应卡尔曼滤波算法为了提高 BDS/INS 系统的稳定性和滤波的计算效率，一般在滤波过程中会认为系统噪声是已知固定的8，这时仅仅需要对量测噪声 进行估计计算，Sage-Husa 自适应滤波算02法也就变成了一种简化的 Sage-Husa 自适应滤波算法，简化后的算法还有很好的自适应性。因为系统噪声是稳定的，即Q 是常量，q=0，r=0，经过简化的 Sage-Husa 自适应滤波算法为9 Xk/k1=k，k1Xk1Pk/k1=k，k1Pk1Tk，k1+Qk1Kk=Pk/k1HTk(HkPk/k1HTk+k)1?ek=Zk HkXk/k1Xk=Xk/k1+Kk?ekPk=(I KkHk)Pk/k1dk=(1 b)/(1 bk+1)k=(1 dk)k1+dk(?ek?eTk HkPk，k1HTk)(12)4Sage-Husa 自适应滤波算法的改进4.1量测异常检测在实际的工程应用中，通常是采用基于协方差匹配的滤波异常检测法，来实现对量测数据异常与动力学模型异常的检测。其中，基于协方差 匹 配 技 术 的 滤 波 发 散 的 判 断 依据为10