CN108802707A

CN108802707A - 改进的用于目标跟踪的卡尔曼滤波方法

Info

Publication number: CN108802707A
Application number: CN201811018794.9A
Authority: CN
Inventors: 张群英; 吕晓华; 陈忠诚; 刘新
Original assignee: Institute of Electronics of CAS
Current assignee: Institute of Electronics of CAS
Priority date: 2018-08-31
Filing date: 2018-08-31
Publication date: 2018-11-13
Anticipated expiration: 2038-08-31
Also published as: CN108802707B

Abstract

本发明涉及一种改进的用于目标跟踪的卡尔曼滤波方法，该方法包括如下步骤：S1、建立观测目标的运动方程，该运动方程包括动态方程和观测方程，根据观测方程得到目标的观测值；S2、得到经过卡尔曼滤波后目标位置信息的滤波值；S3、设置阈值，并计算滤波值与观测值的残差，根据阈值和残差的关系执行步骤S4a或S4b；S4a、若残差不大于阈值，则直接将滤波值作为预测值，得到目标下一时刻的预测位置；S4b、若残差大于阈值，则将滤波值与观测值加权求和，得到更新后的预测值，并根据更新后的预测值得到目标下一时刻的预测位置；S5、重复上述步骤，得到目标的预测运动轨迹。本发明能提高目标跟踪的精度，减小目标机动时的跟踪误差。

Description

改进的用于目标跟踪的卡尔曼滤波方法

技术领域

本发明涉及目标跟踪领域，具体涉及一种改进的用于目标跟踪的卡尔曼滤波方法。

背景技术

目标跟踪在军事和民用领域都具有广泛的应用，如空中监视、卫星和飞船跟踪以及智能视频监控等领域。精准跟踪目标有利于精确了解对方目标的位置，是高科技武器系统及GPS系统的至关重要的核心技术。精准定位、跟踪目标是现在迫切需要解决的问题。目标跟踪问题实际上就是目标状态的跟踪滤波问题，即根据传感器已获得的目标测量数据对目标状态进行精确的估计。目标在运动过程中随着速度、角度、加速度等参数不断变化，使得目标的位置具有很强的相关性，但是由于定位跟踪过程中测量误差、系统噪声和干扰的存在，利用多次含有噪声的观测数据对目标运动状态进行跟踪，需要用滤波方法来获得统计最优的状态估计结果，其实质是最优滤波问题。因此，为提高对目标的跟踪性能，迫切需要研究更为优越的跟踪滤波方法。

传统的跟踪滤波方法有卡尔曼滤波(Kalman Filter，KF)方法及其改进形式扩展卡尔曼滤波(Extended Kalman Filter，EKF)方法以及不敏卡尔曼滤波(Unscented KalmanFilter，UKF)方法、粒子滤波(Particle Filter，PF)方法。其中，KF方法计算简单，但精度较差；EKF方法适用于弱非线性系统，过程简单，但是有很多不足之处：如当后验均值与真值相差较大时，易造成滤波发散；大多数情况下，雅可比矩阵很难计算，导致了在实际应用中的困难。UKF方法是EKF方法的一种替代算法，与EKF方法有着相似的运算复杂度，但是精度比EKF方法高。PF方法精度较高，但是当粒子数较多时计算复杂度过高，导致不能满足实时性要求。同时，上述跟踪方法在目标机动时不能快速的做出反应，导致跟踪误差较大。

可见，上述跟踪方法存在以下缺陷：计算精度差或者计算复杂，不能满足目标精确打击和跟踪需求，并且均不能针对目标的机动做出迅速的反应。针对现在精确跟踪目标的迫切需要，研究更准确的跟踪目标算法具有重要意义。

发明内容

针对上述问题，为了提高目标跟踪的精度，本发明提供了一种改进的用于目标跟踪的卡尔曼滤波方法，其特征在于，所述方法包括如下步骤：

S1、建立观测目标的运动方程，所述运动方程包括动态方程和观测方程，根据所述观测方程得到目标的观测值；

S2、得到经过卡尔曼滤波后目标位置信息的滤波值；

S3、设置阈值，并计算所述滤波值与所述观测值的残差，根据所述阈值和所述残差的关系执行步骤S4a或S4b；

S4a、若所述残差不大于所述阈值，则直接将所述滤波值作为预测值，得到目标下一时刻的预测位置；

S4b、若所述残差大于所述阈值，则将所述滤波值与所述观测值加权求和，得到更新后的预测值，并根据更新后的预测值得到目标下一时刻的预测位置；

S5、重复上述步骤，得到目标的预测运动轨迹。

在一些实施例中，在步骤S1中，所述观测方程的观测量包括目标的位置信息、速度信息和加速度信息。

在一些实施例中，在步骤S1中，所述状态方程中包括状态转移矩阵，所述观测方程中包括观测矩阵。

在一些实施例中，步骤S2具体包括：

S21、利用所述状态转移矩阵分别计算预测均值和预测协方差；

S22、利用所述观测矩阵计算观测预测值，利用所述观测矩阵和所述预测协方差计算新息方差矩阵；

S23、利用所述观测矩阵、所述预测协方差和所述新息方差矩阵得到卡尔曼增益矩阵；

S24、利用所述卡尔曼增益矩阵，得到所述滤波值。

在一些实施例中，在步骤S1中，目标的所述运动方程为：

x_k＝Fx_k-1+v_k

y_k＝Hx_k-1+w_k

其中，上式为所述动态方程，下式为所述观测方程，其中，x_k为k时刻的预测值，y_k为k时刻的观测值，F为所述状态转移矩阵，H为所述观测矩阵，v_k是均值为0且噪声协方差为Q_k的高斯白噪声，w_k是均值为0且噪声协方差为R_k的高斯白噪声。

在一些实施例中，所述预测均值采用下式计算：

x_k|k-1＝Fx_k-1|k-1

其中x_k-1|k-1为k-1时刻的预测值，x_k|k-1为k-1时刻至k时刻的预测均值。

在一些实施例中，所述预测协方差采用下式计算：

P_k|k-1＝FP_k-1k-1F^T+Q^k

其中，预测协方差P_k|k-1为预测的协方差矩阵，P_k-1|k-1为k-1时刻的协方差矩阵，Q^k为过程噪声。

在一些实施例中，所述观测预测值采用下式计算：

y_k|k-1＝Hy_k-1k-1

其中，y_k-1|k-1为k-1时刻的观测值，y_k|k-1为k-1时刻至k时刻的观测预测值。

在一些实施例中，所述新息方差矩阵采用下式计算：

S_k＝HP_k|k-1H^T+R

其中，S_k为新息方差矩阵，R为测量噪声方差。

在一些实施例中，所述卡尔曼增益矩阵K_k为：

在一些实施例中，所述滤波值采用下式计算：

x_k|k＝x_k-1k-1+K_k(y_k-y_k|k-1)

其中，x_k|k为滤波值。

在步骤S4b中，采用下式进行加权求和：

x_k＝αx_k|k+(1-α)y_k

其中，x_k为更新后的预测值，α为加权系数。

基于上述技术方案可知，本发明至少取得了以下有益效果：

本发明提供的方法，在目标发生机动时做出判断，计算滤波值与观测值的残差，若残差大于阈值，则通过融合观测值和滤波值，对目标发生机动时进行处理，能提高目标跟踪的精度，减小目标机动时的跟踪误差。

附图说明

图1为本发明实施例的改进的卡尔曼滤波方法的流程图；

图2为本发明实施例中观测目标的期望轨迹图；

图3为本发明实施例中观测目标的观测轨迹图；

图4为本发明实施例中观测目标的卡尔曼滤波轨迹图；

图5为本发明实施例中观测目标的卡尔曼滤波轨迹与改进的卡尔曼滤波轨迹对比图；

图6为本发明的实施例中卡尔曼滤波与改进的卡尔曼滤波得到的轨迹的跟踪误差对比图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将对本发明的技术方案进行清楚、完整地描述。显然，所描述的实施例是本发明的一部分实施例，而不是全部的实施例。基于所描述的本发明的实施例，本领域普通技术人员在无需创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。

除非另外定义，本发明使用的技术术语或者科学术语应当为本发明所属领域内具有一般技能的人士所理解的通常意义。

图1为本发明的改进的卡尔曼滤波方法的流程图，参照图1，本发明提供了一种改进的用于目标跟踪的卡尔曼滤波方法，该方法包括如下步骤：

S1、建立观测目标的运动方程，其中运动方程包括动态方程和观测方程，并根据观测方程得到目标的观测值；

S2、得到经过卡尔曼滤波后目标位置信息的滤波值；

S3、设置阈值b，并计算滤波值与观测值的残差d，根据阈值b和残差d的关系执行步骤S4a或S4b；

S4a、若残差d不大于阈值b，则直接将滤波值作为预测值，得到目标下一时刻的预测位置；

S4b、若残差d大于阈值b，则将滤波值与观测值加权求和，得到更新后的预测值，并根据更新后的预测值得到目标下一时刻的预测位置；

S5、重复上述步骤，得到目标的预测运动轨迹。

根据一些实施例，在步骤S1中，状态方程中包括状态转移矩阵，观测方程中包括观测矩阵。

根据一些实施例，步骤S2具体包括：

S21、利用状态转移矩阵分别计算预测均值和预测协方差；

S22、利用观测矩阵计算观测预测值，利用观测矩阵和预测协方差计算新息方差矩阵；

S23、利用观测矩阵、预测协方差和新息方差矩阵得到卡尔曼增益矩阵；

S24、利用卡尔曼增益矩阵，得到滤波值。

优选地，在步骤S1中，观测方程的观测量包括目标的位置信息、速度信息和加速度信息。现有技术中采用卡尔曼滤波方法进行跟踪滤波时，观测信息只采用了目标的位置信息，导致测量误差较大；本发明提出的改进的卡尔曼滤波方法，在观测量上添加了速度、加速度信息，即同时观测每时刻目标的位置、速度、加速度，三个量同时计算出目标的坐标，相较于只观测目标的位置信息有了校正作用，可以减小误差。

根据一些实施例，步骤S1中，假设目标做匀速直线运动，目标的运动方程如下：

x_k＝Fx_k-1+v_k (1)

y_k＝Hx_k-1+w_k (2)

其中，式(1)为动态方程，式(2)为观测方程，式中，x_k和y_k分别为k时刻的预测值和观测值，F为状态转移矩阵，H为观测矩阵，v_k和w_k是均值为0且噪声协方差分别为Q_k和R_k的高斯白噪声。k-1时刻的后验概率密度p(x_k-1|y^k-1)是均值为x_k-1|k-1、方差为P_k-1|k-1的高斯形式。

步骤S2中，预测均值采用下式计算：

x_k|k-1＝Fx_k-1|k-1(3)

其中，x_k-1|k-1为k-1时刻的预测值，x_k|k-1为k-1时刻至k时刻的预测均值。

预测协方差采用下式计算：

P_k|k-1＝FP_k-1|k-1F^T+Q^k (4)

观测预测值采用下式计算：

y_k|k-1＝Hy_k-1|k-1 (5)

新息方差矩阵采用下式计算：

S_k＝HP_k|k-1H^T+R (6)

其中S_k为新息方差矩阵，R为测量噪声方差，R与传感器测量精度有关。

卡尔曼增益矩阵K_k为：

滤波值采用下式计算：

x_k|k＝x_k-1|k-1+K_k(y_k-y_k|k-1) (8)

其中，x_k|k即为上述的滤波值。

进一步地，设置一个合适的阈值b，其中阈值b的选择与测量噪声R、目标的机动程度有关，计算滤波值x_k|k与观测值y_k之间的残差：

d＝x_k|k-y_k (9)

将d与b进行比较，当d≤b时，直接将滤波值x_k|k作为k时刻的预测值；

当d＞b时，加权滤波值与观测值：

x_k＝αx_k|k+(1-α)y_k (10)

将k时刻的预测值由x_k|k更替为式(10)中得到的x_k，得到k时刻的预测位置。式中，加权系数α的选择与测量噪声R有关，当R较小时α→0，R较大时α→1，一般根据仿真时具体情况而选择合适的值。

最后，重复以上步骤，得到目标的预测运动轨迹。

下面结合附图介绍本发明的一个具体实施例，本实施例中，对预设的轨迹进行了跟踪处理，处理过程如下：

图2为本实施例中观测目标的期望轨迹图，假设目标在前10s分别沿x轴、y轴做匀速直线运动；在10s末x方向加速，y方向减速；20s末x方向减速，y方向加速，得运动情况如图2所示。

利用公式(2)计算出目标的观测轨迹，如图3所示。此时，公式(2)中的观测矩阵为：

表示观测量仅包括目标的位置信息。

随后，将观测值的第一个值作为卡尔曼滤波的初始值，利用公式(3)和(4)计算出预测均值和方差矩阵。利用公式(5)、(6)和(7)分别计算观测预测值、新息方差矩阵和卡尔曼增益矩阵。

利用公式(8)计算滤波值，重复上述步骤得到目标的卡尔曼滤波轨迹图，如图4所示。

进一步地，对卡尔曼滤波方法进行改进。先将原观测矩阵H更改为：

即在原有观测量基础上加入速度、加速度信息，将每时刻预测的目标的位置信息与速度加速度同时计算出每一时刻的位置坐标。

并利用公式(9)计算滤波值与观测值之间的残差d。当残差d不大于阈值b时，将滤波值直接作为预测值；当残差d大于阈值b时，利用公式(10)得到滤波值与观测值加权后的值作为预测值。

本实施例中，阈值b的选择与测量噪声R、目标的机动程度有关。以目标转向角度为例，假设目标在k-1时刻的预测位置x_k-1|k-1，k时刻的预测位置x_k|k，则根据预测位置得到的目标移动的坐标变化为：横坐标x＝(x_k|k-x_k-1|k-1)×cosθ，纵坐标：y＝(x_k|k-x_k-1|k-1)×sinθ。目标在k时刻的观测位置y_k，则根据观测位置得到的目标移动的坐标变化为：横坐标：纵坐标：计算|x-x₁|和|y-y₁|，此时阈值b应由|b1-R|＝|x-x₁|，或|b2-R|＝|y-y₁|两个来判断，当b1或b2大于残差d时，利用公式(10)得到滤波值与观测值加权后的值作为预测值。

重复上述改进后的卡尔曼滤波方法的步骤，得到改进的卡尔曼滤波轨迹图和跟踪误差图，并与采用改进前的卡尔曼滤波方法得到的图进行对比，如图5和图6所示。从图中可以看出，按照本发明提供的方法对卡尔曼滤波方法进行改进后，提高了目标跟踪的精度，减小了跟踪误差。

本发明实施例中的改进的用于目标跟踪的卡尔曼滤波方法同时考虑了位置信息与速度、加速度信息，相较于只观测目标的位置信息有了校正作用，可以减小误差。并且在目标发生机动时做出判断，计算观测值与滤波值的差值，通过融合以差值为变量的观测值和滤波值，对目标发生机动时进行处理，减小目标机动时的跟踪误差。本发明提供的方法解决了滤波估计值误差较大的问题，并对目标发生机动时进行处理，整体提高了跟踪的精度。

以上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详细说明，应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims

1.一种改进的用于目标跟踪的卡尔曼滤波方法，其特征在于，所述方法包括如下步骤：

S2、得到经过卡尔曼滤波后目标位置信息的滤波值；

S5、重复上述步骤，得到目标的预测运动轨迹。

2.根据权利要求1所述的方法，其特征在于，在步骤S1中，所述观测方程的观测量包括目标的位置信息、速度信息和加速度信息。

3.根据权利要求2所述的方法，其特征在于，在步骤S1中，所述状态方程中包括状态转移矩阵，所述观测方程中包括观测矩阵。

4.根据权利要求3所述的方法，其特征在于，步骤S2具体包括：

S24、利用所述卡尔曼增益矩阵，得到所述滤波值。

5.根据权利要求4所述的方法，其特征在于，在步骤S1中，目标的所述运动方程为：

x_k＝Fx_k-1+v_k

y_k＝Hx_k-1+w_k

6.根据权利要求5所述的方法，其特征在于，所述预测均值采用下式计算：

x_k|k-1＝Fx_k-1|k-1

7.根据权利要求6所述的方法，其特征在于，所述预测协方差采用下式计算：

P_k|k-1＝FP_k-1|k-1F^T+Q^k

8.根据权利要求7所述的方法，其特征在于，所述观测预测值采用下式计算：

y_k|k-1＝Hy_k-1|k-1

9.根据权利要求8所述的方法，其特征在于，所述新息方差矩阵采用下式计算：

S_k＝HP_k|k-1H^T+R

其中，S_k为新息方差矩阵，R为测量噪声方差。

10.根据权利要求9所述的方法，其特征在于，所述卡尔曼增益矩阵K_k为：

11.根据权利要求10所述的方法，其特征在于，所述滤波值采用下式计算：

x_k|k＝x_k-1|k-1+K_k(y_k-y_k|k-1)

其中，x_k|k为滤波值。

12.根据权利要求11所述的方法，其特征在于，在步骤S4b中，采用下式进行加权求和：

x_k＝αx_k|k+(1-α)y_k

其中，x_k为更新后的预测值，α为加权系数。