一种自适应渐消卡尔曼滤波算法

一种自适应渐消卡尔曼滤波算法

0改进卡尔曼滤波算法的成像目标定位系统的设计注重对运动目标的精确可靠跟踪。跟踪算法不仅满足系统的实际处理要求，而且要确保跟踪精度。目前卡尔曼滤波算法被广泛应用于目标跟踪算法。标准卡尔曼滤波算法是建立在状态空间上的线性高斯滤波算法,是以精确的数学模型为前提的。然而,在实际工程中,常常发生模型和系统不匹配(即模型误差)的情况,对这样的系统采用卡尔曼滤波算法进行跟踪必然无法实现最优估计,通常会发生跟踪精度下降,滤波发散甚至跟踪丢失的问题。解决此类问题的主要方法有两种:一是通过改进卡尔曼滤波算法对由模型误差引起的滤波发散等问题进行修正,如扩展卡尔曼滤波(extendedKalmanfilter,EKF)、无迹卡尔曼滤波(unscentedKalmanfilter,UKF)、平方根滤波、渐消记忆滤波、自适应卡尔曼滤波等;二是通过对目标运动模型重新建模使模型误差降低,如singer模型、当前统计模型、IMM模型等。EKF和UKF滤波主要解决非线性(即量测模型非线性)引起的模型误差问题,平方根滤波、渐消记忆滤波、自适应滤波及这些算法的改进方法(如自适应平方根滤波、自适应渐消卡尔曼滤波等)主要解决滤波过程中由于目标状态突变或噪声估计不准确等引起的模型误差。造成卡尔曼滤波发散的主要原因是模型误差引起的预测值不准确,这些改进算法力图实时改变滤波增益或获得较为准确的信息项来对预测误差进行弥补,但这种做法使得改进算法存在延后修正和由于计算量大、计算精度有限带来的计算误差。因此,当模型为线性且系统噪声和传感器的量测噪声均服从高斯分布时,其滤波效果不如标准卡尔曼滤波算法,是准最优滤波算法,并且存在计算量大、计算繁杂等问题。本文在分析造成卡尔曼滤波算法滤波精度下降或发散详细原因的基础上,通过新息r(k+1)和量测噪声协方差R设置判定门限准则和修正准则,直接对卡尔曼滤波算法的一步预测值进行了修正,从而提高了算法的滤波精度,改善了算法的性能。这种修正的方法理论简单,计算量小,适应性强,能够有效解决由于模型误差带来的滤波精度下降和发散等问题,提高了滤波的精度和稳定性。1机动目标跟踪模型设状态向量为分别为目标在运动方向上的位置、速度和加速度。设采样周期为T,则离散时间状态方程和量测方程如下:式中,X(k)是k时刻目标状态向量;W(k)为状态噪声,E(W(k))=0,E(W(k)WT(j))=Q(k)δkj;Z(k)是k时刻的量测向量;V(k)为量测噪声,E(V(k))=0,E(V(k)VT(j))=R(k)δkj,与W(k)相互独立;Φ(k)和H(k)分别为k时刻状态转移矩阵和量测矩阵。对于机动目标跟踪,因所建模型的不同,其表达式不同,这里采用CA(常加速度)模型。离散时间卡尔曼滤波过程如下:只要预先给定状态估计初始值和滤波估计状态向量的协方差矩阵P(0|0),整个卡尔曼滤波过程就可以启动并持续递推下去。2预测值更改方法2.1目标状态突变、模型不准确和模型误差之间的误差消除的卡尔曼滤波算法当目标状态方程和量测方程已知,且状态噪声和量测噪声的统计特性已知时,造成卡尔曼滤波算法精度下降或发散的主要原因如下:(1)卡尔曼滤波通过迭代来消除估计误差,但在迭代过程中估计误差具有传递性,这种误差的传递影响达到稳态所需的迭代次数和稳态后估计的精确性。初始阶段,估计误差越小,达到稳态所需的迭代次数越小,估计精度越高。(2)采用卡尔曼滤波算法时,当系统达到稳态后,预测协方差趋于极小值,滤波增益也趋于极小值。此时,若目标状态发生突变,预测值与量测值之间的误差(即新息)增大,预测协方差和滤波器的增益不能随新息同步改变,从而失去对突变状态的跟踪能力或估计误差增加。(3)当目标运动模型不准确时,预测值偏离真实值过大,卡尔曼滤波算法失效。通过以上分析,对卡尔曼滤波算法的修正在保证原算法优势的基础上应满足:①在初始滤波阶段能够迅速降低估计误差;②在稳态阶段能够有效地消除由于目标状态突变造成的估计精度下降或跟踪失效的问题;③在目标运动建模不准确时,能够有效消除由于建模不准确带来的模型误差。由式(10)可以看出,影响滤波输出的因素有两个:预测值和修正值。其实,对第二项进行修正的本质原因是预测值的不准确。平方根滤波、渐消记忆滤波、自适应滤波及他们的改进算法主要是通过对第二项中增益项或新息项进行修正,从而修正估计值,弥补预测精度低造成的估计精度下降,但采用这些改进算法对第二项进行修正并不能实时对预测值进行修正(即延后修正),且存在计算量大和算法推导繁杂的问题。例如文献提出的基于新息协方差的自适应渐消法虽然减小了自适应渐消法的计算量,但依然只对k+2时刻的预测值根据修正后的k+1时刻的估计值进行调整,即存在延后修正的问题;其次,预测协方差的加权需要额外计算协方差矩阵,其理论和算法推导比较繁杂。因此,为了减少计算量,便于算法的理解,提高滤波精度,对于卡尔曼滤波算法的修正应该从其本质出发,直接对预测值进行实时修正。2.2修正预测值准则根据式(5)和式(10)可以看出,k+1时刻的预测值由k时刻的状态估计值决定,且k+1时刻的预测值影响k+1时刻的状态估计值。因此,要提高k+1时刻的状态估计精度应提高k+1时刻预测值的精度。要解决以上问题,首先,要确定预测值与量测值之间误差较大的一个判断准则;其次,要制定对修正的准则。在不考虑杂波和干扰的情况下,每个时刻的量测值是唯一确定的。如果以量测值为中心,以量测值的最大误差(即量测误差协方差的均方值)为半径作一圆,为了方便描述,称之为量测圆。可以肯定,真实值一定在量测圆内。如果当前时刻的预测值在量测圆内,则可以判定目标状态未发生突变;如果当前时刻的预测值在量测圆外,说明上一时刻的估计误差太大或目标状态发生突变,造成当前时刻预测误差增大;这与目标机动检测的原理是一致的。因此,通过修正预测值,使修正后的预测值落入量测圆内,再使用卡尔曼滤波算法进行估计,则必然能够消除或降低由于目标机动、误差累积和建模不准确带来的标准卡尔曼滤波算法失效或精度不高的问题,且这种方法比对增益项或新息项进行修正更容易直观理解。修正原理如图2所示。2.2.1量测误差与门限r(k)反映了预测值与量测值的误差程度,由于量测值本身是有误差的,因此,为了更贴近真实值,修正初始条件应该是建立在预测值与量测值之间的误差大于某一修正门限的基础之上的。令,R1,1(k)表示k时刻位置的量测误差(即R(k)中第一行第一列的数值)。因为文中只取一维量测,所以R(k)=R1,1(k),r(k)和r′(k)均为标量值,对于多维量测,则需分别取对应量测维的具体量测协方差数值。若|r(k+1)|>gate,则不准确。修正,通过式(5)重新计算。为了降低累积误差的影响,同时增加估计值对量测值的信任度,门限的设置为:gate=0.7r′(k+1)(系数0.7的选取并不固定,为了平衡量测值与预测值对修正预测值的影响,其值应在[0.5,0.9]区间内)。图2中实线圆的圆周,即判定门限gate,该圆称之为修正圆。从图中可以看出,修正准则应该保证落在修正圆外的点通过修正尽可能地落入修正圆内。2.2.2加加速度估计具体的修正规则如下:(1)修正位置估计值。根据新息的误差,对位置估计通过补偿进行修正;(2)修正速度估计值。根据新息的误差,对速度估计通过补偿进行修正;(3)修正加速度估计值。根据修正前后速度估计的变化值,对加速度估计通过补偿进行修正。式中,c1=0.7Γ,c2=0.1Γ,c3=0.3Γ。c1,c2,c3的选取与速度、加速度、位移的表达式有关,即s=ν0t+0.5at2,c1即公式中的位移,c2为速度,c3为加速度。目标跟踪过程中,如果要对速度或加速度修正较多,则c2或c3的取值应大于0.5,其余两个参数,一个取值小于0.5,另一个从公式推导计算得出。本文主要是对位置修正,因此c1取值较大。可以看出,修正的算法与标准卡尔曼滤波算法相比,每次迭代增加了一次比较和一次修正,与文献提出的基于新息协方差的自适应渐消算法相比,不必计算测量值协方差矩阵,无需对预测协方差矩阵进行加权修正。因此,修正的算法计算量比标准卡尔曼滤波算法稍大,比自适应渐消法大大减小。3加速度的均方根误差采用1000次Monte-Carlo仿真实现,评价指标为目标位置、速度和加速度的均方根误差(rootmeansquareerror,RMSE),均方根误差定义为式中,N为Monte-Carlo仿真的次数;j为第j次仿真;X(k)和分别表示k时刻跟踪目标状态的真实值和滤波估计值。初始估计值验证假设目标沿某一方向做匀加速直线运动,状态初始值为X=[01020]T,目标运动持续时间100s,采样间隔1s。系统噪声协方差取固定值Q=diag[1,1,1],观测噪声协方差为R=diag[100,100,100],初始估计值取第一次量测值。图3~图5分别为标准卡尔曼滤波和本文算法对目标进行跟踪时的位置、速度和加速度均方根误差。从图中可以明显看出,修正卡尔曼滤波算法,滤波稳态后的位置、速度和加速度均方根误差明显要低于卡尔曼滤波,且达到稳态所需的时间较短。由于目标初始时刻运动状态是未知的,所以标准卡尔曼滤波在初始阶段的误差要比稳态时大,而采用修正的卡尔曼滤波,可以有效地降低初始阶段的滤波误差,特别明显的是滤波初始阶段的位置误差要远远小于卡尔曼滤波(由于主要是对位置进行修正,速度和加速度修正幅度较小,图4和图5中滤波初始阶段,修正算法的均方根误差稍大于卡尔曼滤波算法)。仿真所需平均时间分别为:卡尔曼滤波8.7ms,修正卡尔曼滤波9.2ms。可以看出修正的卡尔曼滤波所需时间比标准卡尔曼滤波稍大。消除卡尔曼滤波的误差假设目标状态初始值为X=[0100]T,前20s做匀速运动,20~80s做匀加速运动,加速度为6m/s2,80~100s做匀加速运动,加速度为12m/s2。目标运动持续时间100s,采样间隔1s。系统噪声协方差取固定值Q=diag[1,1,1],观测噪声协方差为R=diag[100,100,100],初始估计值取第一次量测值。图6~图8分别为标准卡尔曼滤波、文献所提出的改进自适应渐消卡尔曼滤波和本文算法对目标进行跟踪时的位置、速度和加速度均方根误差。从图中可以明显看出,在加速度发生变化的时刻即20s和80s时刻,3种算法的位置、速度和加速度均方根误差明显发生突变。其中卡尔曼滤波算法在第一次状态突变后达到稳态的时间较长,且均方根误差较大;在第二次状态突变后,在较短时间内不能够达到稳态,均方根误差急剧增大。自适应渐消卡尔曼滤波可以在较短时间内使滤波达到稳态,但效果没有修正的卡尔曼滤波好。修正的卡尔曼滤波的位置、速度和加速度均方根误差明显要低于自适应渐消卡尔曼滤波。仿真所需平均时间分别为:卡尔曼滤波8.7ms,自适应渐消卡尔曼滤波16ms,修正卡尔曼滤波10.2ms。可以看出修正的卡尔曼滤波所需时间比自适应渐消法更小,比标准卡尔曼滤波稍大。研究算法仿真已知目标在二维平面内先沿X轴做400s匀速直线运动,然后做500s匀速转弯运动,再沿切线方向做200s匀速直线运动,然后再做400s的匀速转弯运动,最后沿X轴做400s匀速直线运动。目标的运动轨迹如图9所示,目标运动持续时间2000s,采样间隔1s。系统噪声协方差取固定值Q=diag[1,1,1],观测噪声协方差为R=diag[100,100,100],初始估计值取第一次量测值。图10、图11分别为自适应渐消卡尔曼滤波跟踪算法和本文算法对目标进行跟踪时的位置均方根误差。从图中可以看出,由于目标运动建模采用CA模型,不能准确描述转弯模型,因此运动建模不准确,使得自适应渐消法存在修正延后,状态估计不稳定。而修正的卡尔曼滤波不受运动建模不准确的影响,具有较好的稳定性,且收敛后的精度较高。以上3个仿真实例说明,修正的卡尔曼滤波主要起以下作用:(1)不存在模型误差时,对滤波过程进行微调,降低初始滤波阶段的估计误差,提高稳态时的滤波精度,缩短收敛时间。(2)当目标状态发生突变时,增加滤波过程对于测量值的信任度,实时修正预测值,消除或降低由于目标状态突变造成的滤波跟踪精度下降、滤波发散的问题。(3)当目标运动建模不准确时,在保证滤波精度的基础上,实时修正预测值,消除或降低由于建模不准确带来的模型误差。4修正算法测试当目标状态方程和量测方程已知,且状态噪声和量测噪声的统计