严恭敏, 刘 璠,, 李梓阳, 周 琪

(1. 西北工业大学自动化学院, 西安 710072;2. 中国航空工业集团公司西安飞行自动控制研究所, 西安 710076)

0 引言

卡尔曼滤波(Kalman filter, KF)是一种利用随机线性系统状态空间模型,通过系统输出的观测数据,对系统状态进行最优估计的方法,在导航制导与控制、通信以及图像处理等众多领域有着广泛的应用。

理论上,只有在随机线性系统的结构参数和噪声统计特性参数都准确已知的条件下,KF才能获得状态的最优估计。如果实际量测的统计特性与系统建模参数不匹配,会导致KF状态估计精度下降,严重时还可能会引起滤波发散。卡方检测(Chi-square test, C2T)方法是一种传统的量测故障检测方法[1],在状态空间建模准确的前提下,通过KF的新息及其均方差阵构造卡方统计量,可检测出量测是否存在异常,如果量测正常则进行量测更新,反之,如果量测异常则放弃量测更新。然而,实际应用中的系统噪声参数往往难以准确确定,从而使得卡方检测统计量的阈值设置成为一大难题。如果卡方检测阈值设置太大,则可能将异常量测引入滤波器,降低滤波估计精度;如果阈值设置太小,则可能排除了一些正常的量测,使得量测利用率下降,同样也会降低状态估计精度。造成这一问题的根源在于传统卡方检测的二值化量测异常判断原则,量测要么被判断为正常、要么被判断为异常,再没有其他任何中间情形。

借鉴Sage-Husa自适应滤波方法[2-5](Sage-Husa adaptive KF, SHAKF)或抗差自适应滤波方法[6-7],至少可以更精细地将量测状况划分为“正常/信任”“可疑/部分信任”和“异常/丢弃”这三种类别,针对不同的量测类别修改量测噪声方差阵的大小或赋予不同的量测利用权重,或者直接根据KF新息大小计算权重,实现权重大小的无缝连续过渡。但与上述两种自适应滤波的思路不完全相同,本文通过分析传统卡方检测的新息权重二值化利用特点,给出了新息权重的连续化利用方法,将权重的二值化取值改进为连续化取值,并将其推广,从而获得了多维量测的多分量卡方检测方法。文中将所提新方法称为软卡方检测KF方法(soft Chi-square test KF,SC2TKF),传统方法可被称为硬卡方检测KF方法。显然,新方法更具普遍性,传统硬卡方检测可以看作是新软卡方检测方法的一个特例。最后,利用惯导/卫导组合进行了软卡方检测方法的KF仿真验证,结果显示新方法具有比Sage-Husa自适应滤波更好的滤波精度和稳定性。

1 传统的量测故障卡方检测原理

随机线性系统的状态空间模型记为

(1)

式中,Xk是n×1维的状态矢量,Zk是m×1维的量测矢量;Φk/k-1,Γk-1,Hk是已知的系统结构参数,分别称为n×n维的状态一步转移矩阵、n×l维的系统噪声分配矩阵、m×n维的量测矩阵;Wk-1是l×1维的系统噪声矢量,Vk是m×1维的量测噪声矢量,两者都是零均值的高斯白噪声矢量序列,且它们之间互不相关,即满足如下Kalman滤波关于噪声的基本假设条件

(2)

其中,Qk是半正定的,Rk是正定的,δkj为克罗内克函数。

针对系统式(1)的Kalman滤波公式为

(3)

由量测新息及其均方差阵可构造统计量[1]

(4)

其中,λk服从自由度为m的卡方分布,即λk～χ2(m)。在量测正常情况下,统计量λk的数值应当比较小;而如果量测出现异常,λk将变得较大,量测正常与否一般以某选定的阈值TDm作为判断门限,即

(5)

这便是Kalman滤波的量测故障卡方检测原理。在滤波过程中可以计算统计量λk,根据其大小实时监测量测是否异常,进而决定是否进行量测更新,式(3)中的量测及其均方差阵更新方程可改写为

(6)

显然,当χk=1即λk≤TDm时,进行正常的Kalman滤波量测更新;而当χk=0即λk>TDm时,则放弃量测更新,达到隔离异常量测的效果。

2 新的软卡方检测方法

传统卡方检测方法有效的前提条件是滤波系统模型准确无误。在惯导/卫导组合导航等实际应用中,一般Kalman滤波的系统结构参数建模会相对比较准确且稳定,而噪声参数会由于运动状态、建模不完善、系统老化或运行环境等原因而变化,难以完全精确建模,这样就会使得卡方检测阈值TDm难以严格地按理论方法准确确定。阈值设置过大会造成故障检测概率降低,存在将较多的异常值引入滤波量测的风险,从而造成滤波误差增大;阈值设置过小又会造成故障虚警概率增大,经常性的虚警降低了量测利用率,量测修正作用减小也会降低滤波估计精度。当然,对于高精度惯导系统而言,随机性弃用卫导定位量测50%以上,比如卫导量测即使从1 Hz降为0.1 Hz,对组合导航性能影响也不大;但对于低精度惯导系统,卫导利用从1 Hz变为0.5 Hz对组合精度的影响可能就比较显著了。

传统卡方检测方法的结果是二值化的,非0即1,再无任何中间状态。该方法在雷达目标探测等领域中主要以有/无为指标,其应用是非常合理的,但是,针对组合导航等场合,在量测信息正常(信任)与异常(丢弃)之间还可能存在大量的中间状态(可疑),仅简单地使用卡方检测二值化结果就不太合适了,这也有悖于目前常用的量测自适应滤波和抗差滤波等方法的理念。比如,在Sage-Husa量测自适应Kalman滤波中,通过量测新息的大小自动调整量测噪声方差阵的大小,相当于是将所有量测信息都进行了综合利用,与正常量测相比,量测新息中等大小(可疑)时降低量测权重,新息明显异常时量测权重很小,若权重趋于0则等效于弃用。论文根据自适应滤波的新息利用特点,将式(6)改进为

(7)

(8)

值得特别指出的是,如果式(1)中量测是多维的,传统卡方检测将多维量测视为整体,只要有任何一个分量出现异常,卡方检测方法也会同时降低其他正常量测分量的新息利用率,这种处理方式不是很合理,为了避免该缺陷,需对各量测分量逐一做卡方检测。如果在Kalman滤波模型中,各量测分量之间是不相关的,即量测噪声均方差阵Rk为对角线矩阵,则采用序贯滤波处理后,各序贯量测更新的卡方检测就是相当于对单个量测分量的逐一处理。如果Rk为非对角线矩阵,不妨记rk(i)为新息rk的第i(i=1,2,…,m)个分量,Ak(ii)为新息均方差阵Ak的第i行i列对角线元素,且记统计量

(9)

不难理解,各统计量λk(i)均服从自由度为1的卡方分布,即有λk(i)～χ2(1)。

参考式(7),推广和建立由λk(i)构造的Kalman滤波量测更新方法,如下

(10)

(11)

(12)

在式(10)中,修改了滤波增益Kk的计算方式,其目的是保证之后均方差阵Pk计算的对称性,显然,式(7)是式(10)中ρk各分量都相等时的特殊情形。理论上,ρk可取对称正定阵,但为了简便,一般取为如式(11)所示的对角阵。在式(12)中,参数TD1是自由度为1的卡方统计量λk(i)的故障检测设置阈值,当取显著性水平为0.05时有TD1≈3.8。

3 仿真验证

3.1 惯导/卫导组合导航模型

以惯导/卫导松组合导航为例进行仿真实验验证,系统状态15维、量测3维,具体建模如下

(13)

其中

3.2 仿真条件

采用PSINS工具箱软件模拟一段车载行驶轨迹,轨迹包含静止、加减速、匀速、转弯、爬坡等阶段,总仿真时间约1 000 s,行驶里程约7.5 km。行车速度如图1所示,轨迹相对位置变化的二维平面图如图2所示,图中标识“★”为载车行驶起始点。

图1 载车行驶速度Fig.1 The vehicular velocities

图2 载车行驶平面轨迹Fig.2 The vehicular trajectory

根据轨迹仿真生成惯性传感器数据和卫导定位数据,其中惯性数据采样100 Hz、卫导测量1 Hz。再进行惯性导航解算和惯导/卫导组合导航Kalman滤波解算,仿真过程中注入的各种误差如表1所列。

表1 仿真误差设置

卫导的位置量测误差(纬、经、高,3个分量)均设置为零均值的高斯白噪声,正常时间段内方差为(1 m)2,以下两个时间段除外:(1)200～400 s之间的噪声方差设置为(10 m)2;(2)600～800 s之间设置为污染噪声,以卫导高度测量误差为例,其表示为

(14)

其中,“w.p.”表示“以…概率”(with probability)之意。式(14)表示噪声δH在正常情况下以90%的大概率服从方差为(1 m)2的高斯分布,视为正常噪声;而在异常情况下以10%的小概率服从方差为(100 m)2的高斯分布,视为野值噪声。

一组高度量测误差仿真的示例如图3所示,由图可见,在200～400 s之间高度误差总体波动变大,模拟噪声方差变化;而在600～800 s之间个别误差幅值跳变很大,模拟小概率的野值跳变。

图3 卫导高度测量误差Fig.3 GNSS altitude measurement error

3.3 仿真结果

惯导/卫导组合导航的姿态失准角、速度误差和位置误差的结果如图4所示,从图中可以看出:在200～400 s之间随着卫导的量测噪声增大,组合导航的误差也相应变大,该段误差的量级与全程卫导误差方差都设置为(10 m)2的结果是基本一致的;在600～800 s之间状态估计基本不受卫导量测噪声野值的影响,滤波器具有良好的野值隔离效果,与只存在小方差(1 m)2时的量测噪声一样,始终保持较高的滤波精度。

(a) 姿态误差

(b) 速度误差

(c) 位置误差图4 基于软卡方检测的估计误差Fig.4 State estimation error based on SC2TKF

此外,还对本文新方法(SC2TKF)和传统Sage-Husa量测噪声自适应滤波(SHAKF)作了对比仿真,进行了50次的蒙特卡洛仿真并统计各导航参数误差,结果表明SC2TKF更加稳定,其中纬度误差的均方根(root mean square,RMS)误差统计如图5所示,其他误差情况类似,不再一一列出。值得注意的是,在SHAKF中还有一个渐消因子需要精心设置(文中取b=0.5),若渐消因子设置不合适,对滤波结果有较大的负面影响。综上,新方法无需设置任何参数,具有使用方便和适应性强等优点。

图5 不同滤波方法的纬度误差比较Fig.5 Comparison of latitude error by using SC2TKF vs SHAKF

4 结论

自适应Kalman滤波的方法很多,广义上说,能根据实时量测自动调整滤波器参数并获得良好状态估计效果的方法均可称为自适应滤波,比如Sage-Husa自适应滤波[2-3]、抗差自适应滤波[4,6-7]、强跟踪滤波[8]以及近年来一系列文献中根据变分贝叶斯估计原理给出的RSTKF、MCKF、SSMKF和CERKF等众多方法[9-13]。在组合导航实际系统中,除基于量测噪声自适应的经典Sage-Husa自适应滤波比较实用外,其他自适应方法理论推导结果和仿真效果虽然较好,但往往前提假设过于理想或者参数设置繁琐,难以适用于复杂高维系统,因而少有实际应用的报道。继Sage-Husa自适应滤波之后,论文提出的软卡方检测Kalman滤波方法有望在组合导航领域成为一种更加实用的自适应滤波方法。