卡尔曼滤波.docx

基于修正卡尔曼滤波的目标跟踪
摘要:目前卡尔曼滤波算法被广泛应用于目标跟踪算法。标准卡尔曼滤波算法是建立在状态空间上的线性高斯滤波算法,是以精确的数学模型为前提的,然而,在实际工程中,常常发生模型和系统部匹配的情况,在该情况下采用标准卡尔曼滤波算法进行跟踪必然无法实现最优估计,通常会发生跟踪精度下降。本文提出一种修正的卡尔曼滤波算法,通过设置判定准则和修正准则,实时修正预测值,在滤波初始阶段可迅速降低估计误差,提高稳态是的滤波精度。仿真实例说明了算法的有效性。
关键词:卡尔曼滤波;修正预测值;运动模型
引言
精确而可靠地对机动目标进行跟踪是目标跟踪系统设计的一个主要目的,跟踪算法不仅要满足系统实时处理的要求,同时要保证较高的跟踪精度。
目前卡尔曼滤波算法被广泛应用于目标跟踪。标准卡尔曼滤波算法是建立在状态空间上的线性高斯滤波算法,是以精确的数学模型为前提的,然而,在实际工程中,常常发生模型和系统部匹配的情况,在该情况下采用标准卡尔曼滤波算法进行跟踪必然无法实现最优估计,通常会发生跟踪精度下降。造成卡尔曼滤波发散的主要原因是模型误差引起的预测值不准确,因此本文在分析造成卡尔曼滤波算法滤波精度下降或发散的基础原因之上,通过新息rk+1和量测噪声协方差R设置判定门限准则和修正准则,直接对卡尔曼滤波算法的一步预测值进行修正,从而提高了算法的滤波精度,改善了算法的性能。这种修正的方法理论简单,计算量小。能有效解决由于模型误差带来的误差精度下降问题。
卡尔曼滤波原理

卡尔曼滤波是解决以均方误差最小为准则的最佳线性滤波问题,它根据前一个估计值和最近一个观察数据来估计信号的当前值。它是用状态方程和递推方法进行估计的,而它的解是以估计值(常常是状态变量的估计值)的形式给出其信号模型是从状态方程和量测方程得到的。
卡尔曼过滤中信号和噪声是用状态方程和测量方程来表示的。因此设计卡尔曼滤波器要求已知状态方程和测量方程。它不需要知道全部过去的数据,采用递推的方法计算,它既可以用于平稳和不平稳的随机过程,同时也可以应用解决非时变和时变系统,因而它比维纳过滤有更广泛的应用。
离散系统的卡尔曼滤波
1)确定控制系统的状态空间方程:
x(t)=Ax(t)+Bu(t)+FW(t)
y(t)=Cx(t)+Du(t)+V(t)
其中W为过程噪声,V为观测噪声,它们被假设为高斯白噪声,它们的方差分别为Q和R。
2)将上述系统离散化为如下所示:
xk=k|k-1xk-1+k|k-1Wk
yk=Ckxk+Vk
上两式子中,xk是k时刻的系统状态,uk是k时刻对系统的控制量,yk是k时刻系统的观测量。
3)我们要利用系统的过程模型,来预测下一状态的系统。假设现在的系统状态是k,根据系统的模型,可以基于系统的上一状态而预测出现在状态:
xk|k-1=k|k-1xk-1
(1)
式(1)中,xk|k-1是利用上一状态预测的结果,xk-1是上一状态最优的结果。
4)到现在为止,我们的系统结果已经更新了,现在求预报误差方差矩阵:
Pk|k-1=k|k-1Pk-1k|k-1'+k|k-1Qk|k-1'
(2)
式(2)和式(1)是对系统的预测,Q为系统过程噪声的方差
5)现在我们有了现在状态的预测结果,然后我们再收集现在状态的测量值。结合预测值和测量值,我们可以得到现在状态的最优化估算值xk:
xk=xk|k-1+Kk(yk-Ckxk|k-1)
(3)
其中,Kk为卡尔曼增益:
Kk=Pk|k-1CkT(CkPk|k-1CkT+R)-1
(4)
6)到现在为止,我们已经得到了k状态下最优的估算值xk。但是为了要另卡尔曼滤波器不断的运行下去直到系统过程结束,我们还要更新k状态下xk的滤波误差方差:
Pk=(I-KkCk)Pk|k-1
(5)
其中,I为单位矩阵。按照上述5式自回归运算下去就可以实现滤波。
目标跟踪建模与估计
设状态向量X =xxxT,x,x,x分别为目标在运动方向上的位置、速度、加速度。设采样周期为T,则离散时间状态方程如下:
Xk=k-1 Xk-1+Gk-1Wk-1
Zk=Hk Xk+Vk-1
(6)
式中,Xk是k时刻的目标状态向量;Wk为状态噪声,EWk=0,EWkWTj=Qkδkj;Zk是k时刻的量测向量,Vk为量测噪声,EVk=0,EVkVTj=Rkδkj,与Wk相互独立; k和Hk分别为k时刻状态转移矩阵和量测矩阵。对于机动目标跟踪,因所建模型的不同,其表达式也不同,因本文采用的是匀加速直线运动和匀速直线运动,所以匀加速运动采用CA(常加速度)模型。
k=1TT2/201T001
Hk=100
(7)
匀速运动