用于实时弹道滤波的Sage-Husa改进算法

段鹏伟,宫志华,徐 旭,赵春霞

(中国人民解放军63861部队,吉林 白城 137001)

随着测控技术的飞速发展,靶场测控装备的种类和数量迅速增加,获得的实时弹道测量数据也日益丰富。面对着种类和数量繁多的实时弹道数据,实时弹道测量数据融合为测控系统准确地获取弹丸的实时飞行状态和控制状态提供了有效的手段,提升了试验指挥决策和安控判决的实时性和准确性。对于实时数据融合,实时数据滤波是一个不可或缺的环节。

常规的卡尔曼滤波通常假定系统噪声和量测噪声满足高斯-马尔可夫假设条件,以各种先验信息来确定它们的协方差矩阵,再以恒定的协方差矩阵进行递推滤波。但是在实际的实时弹道滤波处理过程中,由于受弹丸特性、弹丸距离、弹丸姿态和试验环境因素等影响,量测噪声往往具有时变的统计特性,属于非平稳时间序列,这时噪声的统计特性将难以确定。若仍然采用常规卡尔曼滤波,势必造成滤波精度的下降,严重时将导致滤波发散。

对于未知统计特性噪声的自适应滤波问题,1969年SAGE和HUSA对当时的序贯自适应估计算法进行了总结,提出了具有代表性的基于新息的自适应滤波算法,即Sage-Husa卡尔曼滤波算法,简称为Sage-Husa算法,可以在线估计线性离散系统的系统噪声和量测噪声。NARASIMHAPPA等将改进的Sage-Husa算法与自回归模型结合,应用于光陀螺仪去噪,取得了良好的滤波效果。程建华等去除了Sage-Husa算法中系统噪声和量测噪声估计中的部分减号项,保证噪声协方差矩阵的半正定性,并将工程简化算法用于舰船捷联惯导传递对准。王佳伟等对算法进行了简化改进,给出了滤波异常判断准则,并成功应用于修正引信滚转角测量中。李静等和郝亮等分别将CKF算法和UKF算法与Sage-Husa算法结合起来,对车辆状态参数进行实时在线估计。还有一些学者将Sage-Husa算法的噪声估计方法应用于其他滤波算法,都得到了较为理想的滤波结果。

上述改进算法大都受应用领域限制,通常假定量测噪声统计特性未知但相对稳定,忽略量测噪声统计特性的时变性,采用较大的遗忘因子进行递推计算,估计的量测噪声协方差矩阵随着时间的推移趋于稳定,因此无法对时变统计特性量测噪声的进行准确估计。而且这些改进没有对滤波算法的抗差性以及如何保证噪声协方差矩阵无偏估计的正定性进行深入分析。

因此,本文提出了改进的Sage-Husa自适应卡尔曼滤波算法(ISHAKF算法),对Sage-Husa算法的实时自适应性、抗差自适应性和滤波稳定性等进行改进,实现实时自适应滤波,以满足靶场测控系统对实时弹道测量数据的实时滤波需求。

1 算法改进

1.1 Sage-Husa卡尔曼滤波算法

对于线性离散模型,系统的状态方程和量测方程为

=-1-1+-1
=+

(1)

式中:为时刻的状态估计向量,-1为-1时刻到时刻的状态矩阵,-1为系统噪声向量,为时刻对应的测量向量,为量测矩阵,为量测噪声向量。且E(-1)=0,E(-1)=0,cov(,)=,,cov(,)=,,和分别为系统噪声协方差矩阵和量测噪声协方差矩阵,,为狄拉克函数。

引进遗忘因子,仅对量测噪声协方差矩阵进行估计,则算法可简化为

(2)

(3)

(4)

(5)

(6)

(7)

(8)

=(1-)(1-+1)

(9)

(10)

1.2 量测噪声协方差矩阵正定改进

(11)

同时,可以计算得出:

(12)

(13)

1.3 遗忘因子实时自适应改进

Sage-Husa算法中,遗忘因子一般取095～099,在逐渐增大后,趋于(1-)。这种算法对量测噪声协方差恒定但未知的系统进行滤波,能够兼顾估计的准确性和稳健性。但是对于量测噪声统计特性时变甚至突变的系统,这种算法将降低量测噪声估计的准确性和实时性,从而降低实时滤波的精度。

(14)

则遗忘因子可由来确定:

(15)

式中:,分别为遗忘因子的下限和自适应动态范围;,,为控制常量。为了快速适应量测噪声统计特性的突变,遗忘因子下限可取065～08;由遗忘因子的一般取值,+取值在095～099;基于正态分布概率,为了保证系统稳定判定准确率大于99,可取17～19,可取7～9,大于9。用于控制量测噪声协方差变小时,量测噪声协方差估计的快速收敛;用于保证量测噪声协方差较为平稳时的稳定估计;用于控制量测噪声估计更新的拒绝域,>时,量测噪声估计不更新,令=0。

(16)

在新息方差较为稳定时,这种可变的自适应遗忘因子保持了稳定性;同时在新息方差出现较大变化时,能够迅速响应变小。

1.4 卡尔曼增益矩阵抗差自适应改进

对遗忘因子进行自适应改进后,当判定系统统计特性为非稳定状态时,遗忘因子减小,中当前估计值权重增大,这样可以保证算法对量测噪声统计特性估计的准确性和实时性。另一方面,在遗忘因子为1时,虽然判定量测信息异常,量测噪声协方差矩阵没有更新,但是新息向量偏差较大,而且卡尔曼增益矩阵没有改变,这时若仍然对预测值进行修正更新,必将导致滤波结果误差偏大。滤波算法的抗差性能对于实时数据滤波的工程应用具有重要实际意义。因此,为了在一定程度上兼顾滤波算法的抗差自适应性,参考遗忘因子的确定方法,仍以中间变量对增益矩阵进行改进。

(17)

(18)

式中:,为控制常量,与式(15)中的值相同,一般大于15,可根据先验误差信息和预估误差信息中极大值来确定,用于控制对预测值进行修正更新的拒绝域。

算法的抗差自适应性与算法的实时自适应性是一对无法调和的矛盾,因此,可以根据需求侧重,适当调整控制常量。越大,算法的实时自适应性越强,反之,算法的抗差自适应性越强。

由式(17)和式(18)替换Sage-Husa算法中的式(5),由式(13)～式(16)替换Sage-Husa算法中的式(8)和式(9),即形成了ISHAKF算法。

2 仿真验证

对上述改进分别进行仿真对比,以验证改进的正确和与实用性。

仿真条件设定:在量测噪声统计特性未知的条件下,弹丸飞行的状态模型为未知的二维变加速模型,方向和方向的加速度都设定为

仿真时长=100 s,采样间隔为Δ=0.05 s,弹道状态初值为=0 m,=0 m,=1 000 m/s,=0 m/s,仿真弹道数据真值如图1所示。

图1 仿真弹道数据曲线图

2.1 正定改进分析

量测噪声均方差设定为(),分别采用Sage-Husa算法和经过正定改进的Sage-Husa算法对仿真的弹道测量数据进行滤波,以验证正定性改进的有效性。由于状态模型及其统计特性都未知,为了保证算法的兼容性,采用匀加速状态模型,优选系统噪声协方差矩阵=10·,以保证滤波效果和稳定性,为6×6单位对角阵。两种算法的滤波残差结果如图2和图3所示。

图2 Sage-Husa算法滤波残差曲线

图3 正定改进的Sage-Husa算法滤波残差曲线

2.2 遗忘因子改进分析

所有仿真条件与正定性改进比对仿真相同,分别采用经过正定改进的Sage-Husa算法和经过正定及遗忘因子改进的Sage-Husa算法对仿真的弹道测量数据进行滤波,以检验遗忘因子的改进效果,Sage-Husa算法取遗忘因子=0.97。

遗忘因子改进主要是为了准确估计量测噪声,以保证在量测噪声突变时,递推滤波能够快速收敛,两种算法对量测噪声均方差的实时估计结果如图4所示。

图4 遗忘因子改进对量测噪声均方差实时估计的影响

从图中对比可以看到,一方面,为了保证算法的量测噪声方差突变的自适应性,经过正定及遗忘因子改进的Sage-Husa算法估计量测噪声的结果抖动剧烈一些。另一方面,在收敛速度上,当量测噪声方差突然变大时,两种算法都能够快速响应,但是正定及遗忘因子改进的Sage-Husa算法收敛速度较快,这也使得该算法在此刻的滤波效果略优于正定改进的Sage-Husa算法。在量测噪声方差突然变小时,对于量测噪声方差的估计,正定及遗忘因子改进的Sage-Husa算法能够迅速收敛,而正定改进的Sage-Husa算法收敛稍慢,这使得它在此刻的滤波收敛速度同样较慢。通过50次蒙特卡罗实验,两种算法在量测噪声方差突然变小时的滤波残差均方差结果如图5所示。

图5 遗忘因子改进对滤波残差均方差的影响

在Sage-Husa算法中,遗忘因子不仅反映历史和当前量测信息对量测噪声估值的贡献大小,还直接影响量测信息和模型信息对滤波结果权重,从而影响量测噪声统计特性突变时的弹道滤波效果。因此,经过遗忘因子改进之后,可进一步提高算法对量测噪声统计特性的准确估计能力和对其变化的实时自适应能力。

2.3 抗差性改进分析

在上一仿真条件的基础上,每10 s对弹道测量数据的5个连续采样点叠加-200 m的误差,采用经过遗忘因子及正定改进但是未经过抗差性能改进的Sage-Husa算法与ISHAKF算法对仿真的测量数据进行滤波,两种算法的滤波残差结果如图6和图7所示。

图6 遗忘因子及正定改进的Sage-Husa算法滤波残差曲线

图7 ISHAKF算法滤波残差曲线

从图6中可以看到,对于未经过抗差性能改进的Sage-Husa算法,无论量测噪声方差大小,测量数据只要存在野值,都会对滤波效果产生影响,该算法基本上不具备抗差性能。从图7中可以看到,当弹道测量数据中的野值点相对量测噪声较大时,ISHAKF能够有效判定野值并消除野值影响,能够保持滤波的稳定性。但是当野值相对量测噪声相差不大时,由于算法对时变量测噪声具有自适应能力,导致算法无法判定野值。同时,当连续野值采样点超过一定数量时,即使野值相对量测噪声较大,算法会逐渐向含有野值的弹道数据收敛,这与理论分析结果一致。因此,算法仅能够对相对较大的野值具有一定的抗差能力,而且当连续野值点数量过多,滤波结果仍会出现偏移。

3 实例验证

对某型导弹弹道数据进行实时滤波处理,其中方向数据如图8所示。对弹道方向测量数据进行ISHAKF实时滤波处理,以高精度弹道测量数据为真值,可以得到测量数据的残差、滤波后残差和量测噪声的实时均方差估计,结果如图9所示。

图8 某型导弹弹道x方向测量数据

图9 IHAKF滤波残差和量测噪声均方差估计

从图9中可以看到,在线估计的量测噪声均方差与测量数据残差的均方差极为吻合。在滤波效果上,该测量数据残差均值为0.4 m,均方差为122.1 m,采用ISHAKF滤波后,残差均值0.2 m,均方差为42.5 m,消除了超过65%的随机误差,可以有效消除常规卡尔曼滤波算法由量测噪声变化导致的滤波发散现象。

4 结束语

为解决靶场测控系统实时数据滤波处理过程中量测噪声统计特性自适应的实际问题,对Sage-Husa卡尔曼滤波算法进行了多种有效改进。

针对量测噪声方差较小时,Sage-Husa算法会出现量测噪声估计矩阵非正定的问题,以量测噪声协方差矩阵的残差估计替换新息估计,将量测噪声协方差估计矩阵变换为半正定矩阵和正定矩阵之和的形式,弥补了量测噪声协方差估计矩阵非正定导致滤波异常的缺陷。针对测噪声统计特性的自适应问题,通过设计新的自适应遗忘因子,解决了量测噪声统计特性突变时,Sage-Husa算法收敛较慢的问题,兼顾了量测噪声估计的实时性和稳定性。针对抗差性能,通过对卡尔曼增益矩阵的改进,增强了算法的鲁棒性;综合上述算法改进,构建了ISHAKF算法。

通过仿真比对验证和弹道数据滤波实例验证,证明了在实时弹道测量数据滤波方面,ISHAKF算法克服了Sage-Husa算法的欠缺,具有更高的实时性、自适应性和鲁棒性,滤波效果提升明显。