CN117351042B - 一种基于高斯和滤波的任意步时延目标跟踪方法 - Google Patents

Abstract

本发明公开了一种基于高斯和滤波的任意步时延目标跟踪方法，涉及目标跟踪领域，将存在多步随机时延的跟踪系统中的似然函数建模为高斯混合的形式，并使用高斯和容积卡尔曼滤波实现对状态的估计，以此完成量测数据存在任意步时延目标的跟踪定位。本发明解决了在非线性跟踪系统中由于存在任意步随机量测数据时延导致的跟踪精度下降甚至发散的问题，提高目标跟踪的准确性。

Description

一种基于高斯和滤波的任意步时延目标跟踪方法

技术领域

本发明涉及目标跟踪技术领域，更具体的说是涉及一种基于高斯和滤波的任意步时延目标跟踪方法。

背景技术

目前，卡尔曼滤波凭借其简单高效的计算方式被广泛应用于目标跟踪领域，通过各种传感器获取与目标间的相对信息，从而对目标的位置、速度和加速度进行估计，在军事、自动驾驶、交通管制中具有广泛应用。

但其在工程上的应用受限于量测数据是否实时到达的制约，当数据在系统中各组件之间进行传输时，会存在数据随机延迟的情况，而且数据延迟可能是单步或者多步，这时如果采用传统的卡尔曼滤波方法(EKF、CKF等)处理该类目标跟踪问题时，若不对数据时延进行处理则必然会导致不能获得状态的最优估计，从而导致跟踪精度下降甚至发散。

因此，如何在存在数据延迟的情况下提高目标跟踪精度是本领域技术人员亟需解决的问题。

发明内容

有鉴于此，本发明提供了一种基于高斯和滤波的任意步时延目标跟踪方法，解决了在非线性跟踪系统中由于存在任意步随机量测数据时延导致的跟踪精度下降甚至发散的问题。

为了实现上述目的，本发明采用如下技术方案：

一种基于高斯和滤波的任意步时延目标跟踪方法，包括以下步骤：

步骤1：采集被跟踪目标的状态向量，并进行增广，获得增广矩阵，表达式为：

X_k＝F(X_k-1)+Bw_k-1

其中，X_k为增广矩阵， 为被跟踪目标n维的状态向量；k表示离散的时间序列，表示k时刻；d表示最大延迟步数；为增广状态下的状态转移矩阵，是利用目标运动方程对当前状态进行一步预测，获取目标下一时刻的状态预测值；f(·)为已知的非线性目标运动方程；/>表示方差为Q_k的n维过程噪声；B＝[I_n 0 ... 0]^T，I_n为n维单位矩阵；T表示转置；

步骤2：利用增广矩阵表示被跟踪目标的量测方程，并根据状态向量和量测方程获得量测数据；令状态向量中两方向位置数据加上方差为Rk的随机高斯白噪声，获得量测数据；

采用增广矩阵表示量测方程y_k，表示为：

y_k＝h(CⁱX_k)+v_k-i，i＝0，1，...d

式中，Cⁱ为m×(d+1)m维矩阵，d表示最大延迟步数，其中第im至(i+1)m列元素为单位矩阵，其余为0，即CⁱX_k＝x_k-i，x_k-i表示第k时刻延迟了i步的被跟踪目标n维的状态向量；h(·)表示传感器量测向量；v_k-i表示第k时刻延迟了i步的方差为R_k的m维量测噪声；

步骤3：根据量测方程引入二元随机变量获得被跟踪目标的量测数据被延时接收到的高斯混合似然函数；

步骤31：引入二元随机变量表示k-i时刻的量测数据在k时刻被接收到，1表示被接收到，0表示未被接收到，表示为：

步骤32：延迟了i步的量测数据被接收到的发生概率表示为：

其中，μⁱ表示量测数据延迟i步的概率，满足y_k＝z_k-i代表延迟了i步的量测数据被接收到；

步骤33：带有任意步随机时延的非线性目标跟踪系统的高斯混合似然函数p(y_k|X_k)，表示为：

其中，N(·)代表高斯分布；R_k-i表示k-1时刻的量测噪声方差；

步骤4：状态向量的先验概率密度p(X_l-1|y_1：k-1)表示为：

其中，y_1：k-1表示从开始到k-1时刻所有的量测数据；X_k-1为k-1时刻的增广矩阵；为上一时刻状态后验估计值，P_k-1|k-1为上一时刻状态的估计协方差矩阵；

步骤5：利用容积卡尔曼滤波进行处理，得到状态向量的一步预测估计值和预测协方差P_k|k-1，表示为：

其中，为先验状态容积点；j表示维数；n表示过程噪声的维度；Q_k-1表示k-1时刻的过程噪声的方差；S_k-1满足P_k-1|k-1＝S_k-1(S_k-1)^T；ζ_j为下式的第j列元素：

步骤6：将高斯混合似然函数分解到多个高斯滤波器中进行并行运算，并根据量测数据计算各个高斯滤波器的权值，进行加权融合，获得高斯噪声条件下具有多步延迟的量测数据；根据最大延迟步数d构建d+1个高斯滤波器；

步骤61：计算第i个(共d+1个)高斯滤波器的量测协方差及互协方差/>表示为：

其中，为预测状态容积点，j表示维数，S_k满足P_k|k-1＝S_k(S_k)^T，表示延迟i步的量测预测值；i≤d+1，d表示最大延迟步数，具有d步延迟则有d+1个高斯滤波器；

步骤62：第i个高斯滤波器的增益矩阵表示为：

则，第i个高斯滤波器的延迟i步所对应的后验状态估计值和误差协方差/>表示为：

步骤63：计算第i个高斯滤波器的各状态估计值的权值αⁱ，表示：

步骤64：根据每个高斯滤波器的权值进行加权融合，获得输出状态均值及相应协方差，表示为：

其中，状态均值即为具有多步延迟的量测数据。

经由上述的技术方案可知，与现有技术相比，本发明公开提供了一种基于高斯和滤波的任意步时延目标跟踪方法，可有效解决存在任意步的时间延迟问题，将存在多步随机时延的跟踪系统中的似然函数建模为高斯混合的形式，并使用高斯和容积卡尔曼滤波实现对状态的估计，以此完成量测数据存在任意步时延目标的跟踪定位，有效提高存在数据延迟情况下的目标跟踪精度。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。

图1附图为本发明提供的针对任意步时延的高斯和滤波方法结构示意图；

图2附图为本发明提供的位置跟踪误差仿真结果示意图；

图3附图为本发明提供的速度跟踪误差仿真结果示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明实施例公开了一种针对任意步时延的高斯和滤波方法

(1)模型描述

考虑如下带有任意步随机时延的非线性跟踪模型

x_k＝f(x_k-1)+w_k-1 (1.1)

z_k＝h(x_k)+v_k (1.2)

式中，为被跟踪目标n维的状态向量，为各方向的位置、速度，/>为传感器的m维量测向量，f(·)和h(·)分别为已知的非线性目标运动方程和传感器量测方程，和/>分别为方差为Q_k的n维过程噪声和方差为R_k的m维量测噪声。下角标k,k-1代表离散的时间序列。由于通信链路的不可靠性，跟踪滤波器实际接收到的数据y_k与真实的量测数据z_k不同，即

y_k＝z_k-i，i＝0，1，...d (1.3)

假设d为事先已知的最大延迟步数，则y_k＝z_k-i代表延迟了i步的量测数据被接收到，且该事件发生的概率可以表示为p(y_k＝z_k-i)＝μⁱ，μⁱ为延迟i步的概率，满足

基于高斯和滤波的任意步时延目标跟踪方法

Step1：状态增广。

由于多步随机延迟的存在，系统所接收到的量测数据y_k不仅与当前时刻的状态x_k有关，还与之前时刻的状态x_k-1有关，因此，需要对状态向量进行增广，即

X_k＝F(X_k-1)+Bw_k-1 (1.4)

其中， 为增广状态下的状态转移矩阵，B＝[I_n 0 ... 0]^T，I_n为n维单位矩阵。

则利用增广矩阵的量测方程可以写为

y_k＝h(CⁱX_k)+v_k-i，i＝0，1，...d (1.5)

式中，CiXk＝xk-i。

Step2：似然函数改写

为了明确表示量测在某一步延迟这一事件，引入二元随机变量同一时间有且只有一个元素为1，其余均为0，当该元素为1时表示量测延迟了i步被滤波器接收到，即/>代表k-i时刻的量测数据在k时刻被接收到。由于量测的延迟i步概率为μⁱ，因此有

利用以上变量，带有任意步随机时延的非线性目标跟踪系统的似然函数p(y_k|X_k)可以表示为

式中，N(·)代表高斯分布；R为第k-i时刻的量测噪声。由于式表示的概率密度函数分布为高斯混合的形式，因此可以使用高斯和滤波来处理这种问题。

Step3：状态时间更新

在所提算法中，假设状态的先验概率密度p(X_k-1|y_1：k-1)为高斯分布，y_1：k-1为从开始到k-1时刻所有的量测数据，为

式中，为上一时刻状态后验估计值，P_k-1|k-1为上一时刻状态的估计协方差矩阵。

利用容积卡尔曼处理非线性系统并提高对非线性的近似程度，获取状态的一步预测估计值和预测协方差P_k|k-1

式中，为容积点，可通过下式生成

其中，S_k-1满足P_k-1|k-1＝S_k-1(S_k-1)^T，ζ_j为下式的第j列

Step4：状态量测更新

所提方法通过将高斯混合形式的似然函数分解到数个高斯滤波器中进行并行运算，并根据量测数据计算各子滤波器的权值，对最终结果进行加权融合以实现对高斯混合形式的似然函数的处理。通常高斯和滤波被用来处理噪声为高斯混合分布的情况，本发明中高斯和滤波被用来处理高斯噪声条件下量测存在多步延迟的情况；

根据量测延迟最大步数d共建立d+1个滤波器，对于延迟i,i＝0,1,...d步所对应的子滤波器，有

计算量测协方差及互协方差/>

其中，延迟i步的量测预测值为

计算增益矩阵

计算延迟i步所对应的后验状态估计值和误差协方差/>

对所有d+1个滤波器按照式(1.13)至式(1.18)进行重复计算，获取到不同延迟步数所对应的d+1个状态估计值和误差协方差。

计算各状态估计值的权值αⁱ

考虑算法实时性，假设状态的估计仍符合高斯分布，将d+1个子滤波器输出的状态估计近似为单高斯分布，则输出状态均值及相应协方差为

至此，完成了算法一个周期的流程。

实施例

有一地面目标做匀速直线运动，则其目标运动模型可用常速度模型描述为

式中，x_k＝[x y v_x v_y]代表被跟踪目标在两个方向的位置和速度，仿真步长为T＝1s，I₂为2维单位矩阵，跟踪滤波器可获得目标的位置信息，则量测方程可以表示为

z_t＝[I₂ 0]x_t+v_t (1.24)

并带有最大三步的随机延迟，即

延迟概率为

μ＝[0.6，0.2，0.1，0.1] (1.26)

目标状态为两方向的位置和速度，其初始值为x₀＝[-10000 10000 30 40]^T，P₀＝diag[10000 10000 100 100]；系统过程噪声为量测噪声为R＝δ×I₂，其中δ＝100m²。

仿真结果如下图2-3所示。虚线为利用未考虑时延的传统容积卡尔曼算法得到的位置和速度跟踪误差，实线为利用本发明提出的算法得到的位置和速度跟踪误差。可以看出，未考虑时延的容积卡尔曼滤波在受到多步时延时跟踪误差明显增大，而本发明的算法可以应用于具有多步随机时延的跟踪系统中，将任意步随机时延系统的似然函数表示为高斯混合的形式，并且将容积卡尔曼滤波、高斯和滤波进行融合，利用高斯和滤波的框架处理带有任意步随机时延的非线性跟踪系统，保证了对目标的准确跟踪。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的装置而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (6)

1.一种基于高斯和滤波的任意步时延目标跟踪方法，其特征在于，包括以下步骤：

步骤1：采集被跟踪目标的状态向量，并进行增广，获得增广矩阵；

步骤2：利用增广矩阵表示被跟踪目标的量测方程，并根据状态向量和量测方程获得量测数据；

步骤3：根据量测方程引入二元随机变量获得被跟踪目标的量测数据被延时接收到的高斯混合似然函数；

高斯混合似然函数p(y_k|X_k)表示为：

其中，X_k为增广矩阵；y_k表示k时刻的量测数据，y_k＝z_k-i表示延迟了i步接收到的量测数据；N(·)代表高斯分布；R_k-i表示k-1时刻的量测噪声方差；μ_i表示量测数据延迟i步的概率，满足h(CⁱX_k)表示第k-i时刻的跟踪目标状态向量的传感器量测向量；X_k为增广矩阵；Cⁱ为m×(d+1)m维矩阵，d表示最大延迟步数，其中第im至(i+1)m列元素为单位矩阵，其余为0；

量测数据延迟i步的概率μⁱ表示为：

为引入的二元随机变量，表示k-i时刻的量测数据在k时刻是否被接收到，1表示被接收到，0表示未被接收到；

步骤4：根据高斯混合似然函数计算状态向量的先验概率密度；

步骤5：根据先验概率密度利用容积卡尔曼滤波，获得状态向量的一步预测估计值和预测协方差；

步骤6：将高斯混合似然函数分解到多个高斯滤波器中进行并行运算，并根据量测数据计算各个高斯滤波器的权值，进行加权融合，获得高斯噪声条件下具有多步延迟的调整量测数据；

步骤7：根据调整量测数据和跟踪滤波器，获得跟踪目标定位信息。

2.根据权利要求1所述的一种基于高斯和滤波的任意步时延目标跟踪方法，其特征在于，步骤1中，增广矩阵表达式为：

X_k＝F(X_k-1)+Bw_k-1

其中，X_k为增广矩阵，为被跟踪目标n维的状态向量；k表示离散的时间序列，表示k时刻；d表示最大延迟步数；表示增广状态下的状态转移矩阵，是利用目标运动方程对当前状态进行一步预测，获取目标下一时刻的状态预测值；f(·)为已知的非线性目标运动方程；/>表示方差为Qk的n维过程噪声；B＝[I_n 0...0]^T，I_n为n维单位矩阵；T表示转置。

3.根据权利要求1所述的一种基于高斯和滤波的任意步时延目标跟踪方法，其特征在于，采用增广矩阵表示量测方程y_k，表示为：

y_k＝h(CⁱX_k)+v_k-i，i＝0,1,...d

式中，Cⁱ为m×(d+1)m维矩阵，d表示最大延迟步数，其中第im至(i+1)m列元素为单位矩阵，其余为0，即CⁱX_k＝x_k-i，x_k-i表示第k-i时刻的被跟踪目标n维的状态向量，i表示延迟了i步；X_k为增广矩阵；h(·)表示传感器量测方程；v_k-i表示第k-i时刻的方差为R_k的m维量测噪声。

4.根据权利要求1所述的一种基于高斯和滤波的任意步时延目标跟踪方法，其特征在于，先验概率密度p(X_k-1|y_1：k-1)表示为：

其中，y_1：k-1表示从开始到k-1时刻所有的量测数据；X_k-1为k-1时刻的增广矩阵；为上一时刻状态后验估计值，P_k-1|k-1为上一时刻状态的估计协方差矩阵。

5.根据权利要求1所述的一种基于高斯和滤波的任意步时延目标跟踪方法，其特征在于，状态向量的一步预测估计值和预测协方差P_k|k-1，表示为：

其中，f(·)为非线性目标运动方程；为先验状态容积点，j表示维数；n表示过程噪声的维度；Q_k-1表示k-1时刻的过程噪声的方差；S_k-1满足P_k-1|k-1＝S_k-1(S_k-1)^T；ζ_j为下式的第j列元素：

6.根据权利要求1所述的一种基于高斯和滤波的任意步时延目标跟踪方法，其特征在于，根据最大延迟步数d构建d+1个高斯滤波器，步骤6的具体实现过程为：

步骤61：计算第i个高斯滤波器的量测协方差及互协方差/>表示为：

其中，为预测状态容积点，j表示维数，S_k满足P_k|k-1＝S_k(S_k)^T，/>表示延迟i步的量测预测值，P_k|k-1表示预测协方差；h(·)表示传感器量测方程；R_k-i表示k-i时刻的量测噪声方差；f(·)为非线性目标运动方程；/>表示状态向量的一步预测估计值；n表示过程噪声的维度；i≤d+1，d表示最大延迟步数；

步骤62：第i个高斯滤波器的增益矩阵表示为：

步骤63：第i个高斯滤波器的延迟i步所对应的后验状态估计值和误差协方差/>表示为：

其中，y_k表示k时刻的量测数据；P_k|k-1表示预测协方差；

步骤64：计算第i个高斯滤波器的状态估计值的权值αⁱ，表示：

其中，μⁱ表示量测数据延迟i步的概率；N(·)代表高斯分布；d表示最大延迟步数；

步骤65：根据每个高斯滤波器的权值进行加权融合，获得输出状态均值及相应协方差，表示为：

其中，状态均值为具有多步延迟的调整量测数据。