CN113189578B - 一种扩展目标跟踪方法 - Google Patents

一种扩展目标跟踪方法 Download PDF

Info

Publication number
CN113189578B
CN113189578B CN202110425063.1A CN202110425063A CN113189578B CN 113189578 B CN113189578 B CN 113189578B CN 202110425063 A CN202110425063 A CN 202110425063A CN 113189578 B CN113189578 B CN 113189578B
Authority
CN
China
Prior art keywords
state
target
time
estimation
parameter
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202110425063.1A
Other languages
English (en)
Other versions
CN113189578A (zh
Inventor
李春光
陈紫燕
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Zhejiang University ZJU
Original Assignee
Zhejiang University ZJU
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Zhejiang University ZJU filed Critical Zhejiang University ZJU
Priority to CN202110425063.1A priority Critical patent/CN113189578B/zh
Publication of CN113189578A publication Critical patent/CN113189578A/zh
Application granted granted Critical
Publication of CN113189578B publication Critical patent/CN113189578B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/66—Radar-tracking systems; Analogous systems
    • G01S13/72—Radar-tracking systems; Analogous systems for two-dimensional tracking, e.g. combination of angle and range tracking, track-while-scan radar
    • G01S13/723—Radar-tracking systems; Analogous systems for two-dimensional tracking, e.g. combination of angle and range tracking, track-while-scan radar by using numerical data
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/15—Correlation function computation including computation of convolution operations
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization

Abstract

本发明提供一种扩展目标跟踪方法。在诸多军事、民用相关领域中，目标跟踪技术都被广泛应用并发挥着关键的作用。这些应用场景在激发目标跟踪理论巨大潜力的同时，也对目标跟踪技术提出了更高的要求。本发明针对在道路网络上行进、受到道路约束的地面目标，通过对道路进行建模，引入与道路有关的参数向量，来模拟这种约束；利用高分辨率传感器所提供的对于目标范围的观测，进一步完善跟踪模型；基于最大相关熵准则推导一种新的卡尔曼滤波器，并使用两个这样的卡尔曼滤波器分别对系统中的状态和参数进行估计，提高了非高斯噪声环境中扩展目标跟踪系统的性能。

Description

一种扩展目标跟踪方法
技术领域
本发明属于信号处理和目标跟踪领域，具体涉及一种扩展目标跟踪方法。
背景技术
在传统的跟踪场景中，通常将被跟踪对象建模为不占据任何空间范围的几何点，即假设当某物体被传感器感知到时，其最多只占用传感器的一个分辨单元。现今这种假设在很多应用中依然有其合理性，但在另一些场景中则显示出局限性。比如，在自动驾驶过程中，要实现对于障碍物及其他道路使用者的智能闪避，势必不能忽视这些对象实际所占据的空间范围。在这类情况下对物体进行跟踪引发了新的需求，称为扩展目标跟踪(extendedobject tracking, EOT)。
在扩展目标跟踪中，物体的形状通常是未知的，甚至可能随时间变化。伴随着雷达、红外等传感器分辨率的不断提高，传感器通常会返回对单个物体的多个检测结果，这些检测结果共同提供了物体空间结构的相关信息，即一个高分辨率的传感器能够在合理的信噪比内提供对于目标范围一个或多个维度的观测，例如目标的距离向范围或是方位向范围。本发明通过将目标的范围量合并到跟踪系统中，来完善传统的点目标跟踪模型，此时目标的状态由位置向量、运动向量和范围向量共同组成。
目标跟踪问题的实质是利用所获取的观测信息，实现对目标状态的最优估计，在这方面卡尔曼滤波(kalman filtering,KF)是研究较久较为成熟、应用较广较有成效的一种算法。传统的卡尔曼滤波器是高斯假设下的最优估计器，但在处理包含非高斯噪声的目标跟踪系统时其性能会显著下降。主要原因就在于卡尔曼滤波是基于最小均方误差(minimum mean square error,MMSE)准则开发的，该准则只捕获误差信号的二阶统计量，对较大的离群值很敏感。本发明在许多跟踪场景，尤其是地面目标跟踪中，跟踪目标通常可以假设为在已有的道路网络上行进，道路的朝向等属性是缓慢变化的，此时目标的状态受到道路的约束。然而，由于运动模型和观测的不确定性，状态的预测和估计可能不属于道路，这样就会造成估计的不准确性。
发明内容
本发明针对扩展目标跟踪问题，提出了一种扩展目标跟踪方法，采用最大相关熵(maximum correntropy,MC)准则来推导一种新的卡尔曼滤波器，由于相关熵包含二阶及更高阶的误差矩，该滤波器在非高斯噪声环境中有更好的表现。此外，本发明通过对道路进行建模，引入与道路有关的参数向量，来利用这种约束，进一步完善跟踪模型。此时扩展目标跟踪系统中同时存在未知的状态和参数向量，本发明使用两个最大相关熵卡尔曼滤波器来分别对系统中的状态和参数进行估计，即为最大相关熵对偶卡尔曼滤波(maximumcorrentropy-dual extended kalman filter,MC-DEKF)方法，提高了非高斯噪声环境中跟踪系统的性能。
本发明解决其问题所采用的技术方案如下：
一种扩展目标跟踪方法，包括以下步骤：
(1)将目标形状建模为椭圆，定义目标的状态向量x包含其x、y方向的位置和速度以及椭圆的长轴长度：
x＝[px py vx vy l]T
式中，x为目标的状态向量，px和py分别为目标在x方向和y方向上的位置，vx和vy分别为目标在x方向和y方向上的线速度幅值，l为椭圆的长轴长度，上角标T表示转置；
观测向量y包含目标质心的距离和方位观测，以及目标沿传感器—目标视线的距离向范围观测：
y＝[ρ ζ L]T
式中，y为观测向量，ρ为目标相对于传感器的距离，ζ为目标相对于传感器的方位，L为目标沿传感器—目标视线的距离向范围；
(2)通过参数向量θ来表示各路段，描述目标在道路网络上行进的受约束运动，建立扩展目标跟踪模型：
xk+1＝f(xk，θk)+ωk
yk＝h(xk)+υk
式中，xk+1为k+1时刻的目标状态，xk为k时刻的目标状态，θk为k时刻系统的参数向量，ωk为k时刻的过程噪声，vk为k时刻的观测噪声，yk为k时刻系统的观测值，f(·)为状态转移方程，h(·)为观测方程；
(3)设置核宽σ、收敛阈值ε和遗忘因子λ；
(4)为了获取初始系统的初始状态向量，取前N次测量数据，对位置观测进行线性拟合，从拟合的线性方程中提取得到参数的初始值：
θ0＝[a0 b0 c0]T
式中，θ0为系统的初始参数向量，a0、b0、c0共同描述了路段s的线性方程，即
a0 sps,x+b0 sps,y+c0 s＝0，其中ps,x和ps,y分别为位于路段s上的点的横纵坐标；
将位置观测在拟合结果上的投影作为目标的初始位置状态，并据此计算得到目标的初始速度状态和范围状态；初始化误差协方差矩阵
Figure BDA0003029018580000031
和
Figure BDA0003029018580000032
利用最大相关熵对偶卡尔曼滤波算法，分别对扩展目标跟踪模型中的各时刻的状态和参数进行估计。
进一步的，所述的对扩展目标跟踪模型中的各时刻的状态和参数进行估计的步骤包括：
步骤ⅰ：计算参数的先验估计及其误差协方差矩阵：
Figure BDA0003029018580000033
Figure BDA0003029018580000034
式中，
Figure BDA0003029018580000035
为k-1时刻参数的后验估计，
Figure BDA0003029018580000036
为k时刻参数的先验估计，λ为遗忘因子，
Figure BDA0003029018580000037
为k-1时刻参数误差协方差矩阵的后验估计，
Figure BDA0003029018580000038
为k时刻参数误差协方差矩阵的先验估计；
步骤ⅱ：计算状态的先验估计及其误差协方差矩阵：
Figure BDA0003029018580000039
Figure BDA00030290185800000310
Figure BDA00030290185800000311
式中：
Figure BDA00030290185800000312
为k时刻状态的先验估计，
Figure BDA00030290185800000313
为k-1时刻状态的后验估计，
Figure BDA00030290185800000314
为k时刻参数的先验估计，Fk为k时刻状态转移方程f(·)关于系统状态的雅可比矩阵，
Figure BDA00030290185800000315
为k-1时刻状态误差协方差矩阵的后验估计，
Figure BDA00030290185800000316
为Fk的转置，
Figure BDA00030290185800000317
为k时刻过程噪声的协方差矩阵；
步骤ⅲ：通过不动点迭代法计算状态的后验估计及其误差协方差矩阵：
Figure BDA00030290185800000318
Figure BDA00030290185800000319
Figure BDA00030290185800000320
式中：
Figure BDA00030290185800000321
为k时刻状态的后验估计，
Figure BDA00030290185800000322
为k时刻状态的卡尔曼增益，yk为k时刻系统的观测值，
Figure BDA00030290185800000323
为k时刻状态误差协方差矩阵的后验估计，I为单位矩阵，
Figure BDA00030290185800000324
为k时刻观测方程h(·)关于系统状态的雅可比矩阵；
步骤ⅳ：通过不动点迭代法计算参数的后验估计及其误差协方差矩阵：
Figure BDA00030290185800000325
Figure BDA00030290185800000326
Figure BDA00030290185800000327
式中：
Figure BDA0003029018580000041
为k时刻参数的后验估计，
Figure BDA0003029018580000042
为k时刻参数的卡尔曼增益，
Figure BDA0003029018580000043
为k时刻观测方程 h(·)关于系统参数的雅可比矩阵；
Figure BDA0003029018580000044
为k时刻参数误差协方差矩阵的先验估计，
Figure BDA0003029018580000045
为k时刻参数误差协方差矩阵的后验估计；
步骤v：重复步骤ⅰ-ⅳ。
进一步的，最大相关熵对偶卡尔曼滤波算法，其目标函数表示为：
Figure BDA0003029018580000046
式中，Gσ(·)为高斯核，σ>0为核宽度，M为向量的维度，zk,m为k时刻状态先验估计和观测值所组合向量的第m行，dm(xk)为k时刻状态值和观测函数所组合向量的第m行。
进一步的，所述的对偶卡尔曼滤波算法是将两个卡尔曼滤波器结合起来交替运算，令状态估计和参数估计结果互相参与彼此的预测和更新过程。
与现有技术相比，本发明的优势在于：
(1)考虑到现有对扩展目标跟踪的研究多集中于目标的外形建模这一现实，本发明的方法除用椭圆形模型对目标进行描述外，还增加了道路网络对于目标运动约束的考量，用一系列线性方程来对道路进行描述，在原本只包含未知状态的模型中引入未知参数来表达这种约束，从而将扩展目标跟踪问题建模为一对偶估计问题。通过这样的形式，减轻了已有的运动模型与目标实际的运动状态不匹配对状态估计造成的影响。
(2)在实际的观测环境中往往存在非高斯噪声，这是基于最小均方误差准则的卡尔曼滤波所不能处理的，现有的扩展目标跟踪方法对此的研究也较为稀少。鉴于最大相关熵准则在非高斯噪声环境中的有效应用，本发明基于该准则对对偶扩展卡尔曼滤波方法进行改进，提出了最大相关熵对偶扩展卡尔曼滤波方法。仿真实验验证了该方法在非高斯噪声环境中仍能表现出较好的跟踪性能。
附图说明
图1为传感器对椭圆形目标的观测；
图2为最大相关熵卡尔曼滤波算法流程；
图3为用于参数估计的卡尔曼滤波；
图4为DKEF的迭代(a)和序贯形式(b)；
图5为对偶扩展卡尔曼滤波算法流程；
图6为算法总体流程；
图7为高斯噪声环境中的估计结果；
图8为非高斯噪声环境中的估计结果。
具体实施方式
下面结合附图对本发明的技术方案做进一步的阐述。
基于最大相关熵对偶卡尔曼滤波的扩展目标跟踪方法的具体实现步骤如下(结合图6所示)：
(1)针对在道路网络上行进、受到道路约束的地面目标，通过对道路进行建模，引入与道路有关的参数向量，来模拟这种约束；
(2)将目标的范围量结合进目标的状态向量中，利用高分辨率传感器所提供的对于目标范围的观测，进一步完善跟踪模型；
(3)基于最大相关熵准则推导一种新的卡尔曼滤波器；
(4)使用两个最大相关熵卡尔曼滤波器分别对扩展目标跟踪模型中的状态和参数进行估计。
步骤(1)的具体过程如下：
在目标跟踪滤波技术中，一般首先需要对目标运动规律进行合理的数学建模，然后才能准确估计目标的状态(如位置、速度、加速度和角速度等)。由于目标运动模式的不确定性，使用自行建立的运动模型对目标状态进行预测时，会出现目标预测状态和目标真实状态的不确定性。当建立的目标运动模型能比较完美地描述目标的运动时，目标的预测状态与真实状态匹配，跟踪性能较好。反之目标的预测状态与真实状态不匹配，可能会导致跟踪性能严重下降甚至丢失目标。
最简单的运动模型基于牛顿运动定律，假定运动为匀速运动，即恒定速度(constant velocity,CV)模型：
Figure BDA0003029018580000051
式中：
px和py分别为目标的横纵坐标；
vx和vy分别为目标在相应方向的线速度幅值。
在本发明的假设中，地面目标在道路网络上不断行进，只利用已有的运动模型难以充分描述目标的这种受约束运动，根据模型所得到的状态估计很可能不属于道路。因此利用路段描述，将目标的动力学状态建模为：
xk+1＝P(M(xk)，wk)
式中：
M(·)为无道路约束下物体的运动模型，由于地面目标的机动性通常不会太强，此处使用 CV模型是一个较为合理的选择；
P(·)为上述运动模型所预测的状态在相应道路上的投影。
为了简单起见，本发明考虑一个没有地形特征的二维道路网。同样地，本发明考虑一条没有宽度的道路，它可能代表道路左车道或右车道的中心线。一般来说，一条道路可以被近似视为由一组线性段或曲线段组成。
当将各道路段视为线性段时，可将各段建模为：
asps，x+bsps，y+cs＝O
式中：ps，x和ps，y分别为位于路段s上的点的横纵坐标。
此时本发明可以通过参数向量θ＝[a b c]T来描述各路段。
相对应地，目标的运动模型为：
Figure BDA0003029018580000061
步骤(2)的具体过程如下：
本发明通过定义一个简单的通用目标模型来利用目标的范围信息。本发明将目标形状建模为椭圆(见图1)，此时目标的距离向和方位向的范围随着传感器—目标之间的视线方向平滑地变化(若为矩形模型，则目标范围的一阶导数不连续)。
可以看出，椭圆形目标的距离向范围为：
Figure BDA0003029018580000062
式中：
l为椭圆长轴的长度；
φ为椭圆长轴和传感器—目标视线之间的夹角；
μ为椭圆短轴和长轴长度之比(μ＜1)。
目标的方位向范围也可通过类似的表达式得到。本发明假定椭圆的宽长轴比μ是固定并且已知的，则L(φ)只取决于目标长度l及其相对于传感器—目标视线的方向，本发明将目标长度l这一未知量包含在待估的状态向量中，得到目标的状态向量为：
x＝[px py vx vy l]T
假设该运动目标的长轴平行于其速度矢量，一固定在原点的高分辨率传感器提供目标质心的距离和方位观测，以及目标沿传感器—目标视线的距离范围观测，形成一个三维观测向量，记为：
y＝[ρ ζ L]T
式中：
ρ和ζ分别为目标相对于传感器的距离和方位。
考虑噪声，得到完整的扩展目标跟踪模型为：
Figure BDA0003029018580000071
Figure BDA0003029018580000072
式中：
Figure BDA0003029018580000081
ωk的协方差矩阵
Figure BDA0003029018580000082
R为一2×2的旋转矩阵，沿路段的标准偏差σar远大于与路段正交的标准偏差σcr，即σar＞＞σcr；
vk的协方差矩阵
Figure BDA0003029018580000083
步骤(3)的具体过程如下：
对于离散线性动力系统，本发明有
Figure BDA0003029018580000084
式中：
Figure BDA0003029018580000085
为根据前一时刻的状态估计值得到的当前时刻的先验状态估计值。
nk的协方差矩阵
Figure BDA0003029018580000086
式中：
Figure BDA0003029018580000087
为误差协方差矩阵的先验估计；
Figure BDA0003029018580000088
为观测噪声的协方差矩阵；
Figure BDA0003029018580000089
和
Figure BDA00030290185800000810
可分别通过对
Figure BDA00030290185800000811
和
Figure BDA00030290185800000812
进行Cholesky分解获得。
将式(3-9)等号两边同时左乘
Figure BDA00030290185800000813
可得：
Zk＝d(xk)+ek
式中：
Figure BDA00030290185800000814
Figure BDA00030290185800000815
Figure BDA00030290185800000816
其协方差矩阵
Figure BDA00030290185800000817
基于最大相关熵准则的卡尔曼滤波算法，其目标函数可表示为：
Figure BDA0003029018580000091
式中：
Gσ(·)为高斯核，σ>0为核宽度；
ek,m、zk,m和dm(xk)分别为ek、zk和d(xk)的第m个元素；
M＝Mx+My为zk的维度，Mx、My分别为xk、yk的维度。
此时xk的最优估计为：
Figure BDA0003029018580000092
将目标函数关于xk求偏导并使其结果为零：
Figure BDA0003029018580000093
式中：Dk,m为Dk的第m行，
Figure BDA0003029018580000094
可以求得：
Figure BDA0003029018580000095
上式右侧是关于
Figure BDA0003029018580000096
的函数，即可将上式重写为
Figure BDA0003029018580000097
该式可通过不动点迭代法计算求解，在迭代的第t步，
Figure BDA0003029018580000098
令
Figure BDA0003029018580000099
式中：
Figure BDA00030290185800000910
Figure BDA00030290185800000911
则：
Figure BDA00030290185800000912
进一步推导可得：
Figure BDA00030290185800000913
式中：
Figure BDA0003029018580000101
Figure BDA0003029018580000102
Figure BDA0003029018580000103
因为
Figure BDA0003029018580000104
和
Figure BDA0003029018580000105
均为
Figure BDA0003029018580000106
的函数，故
Figure BDA0003029018580000107
通过
Figure BDA0003029018580000108
和
Figure BDA0003029018580000109
与
Figure BDA00030290185800001010
相关，上式依然为
Figure BDA00030290185800001011
的不动点方程。
通过上述推导，结合卡尔曼滤波算法流程，将最大相关熵卡尔曼滤波算法总结如图2所示。
其中：
Figure BDA00030290185800001012
为状态转移方程f(·)关于系统状态的雅可比矩阵；
Figure BDA00030290185800001013
为观测方程h(·)关于系统状态的雅可比矩阵。
步骤(4)的具体过程如下：
卡尔曼滤波器基于最小均方误差准则提供了一种有效的方法来生成离散时间非线性动力系统状态的近似最优估计。如果已知不含噪声的系统状态，KF的第二个用途是根据状态值和输出值估计系统的未知参数。
对于包含未知参数θ的动力系统，假设该系统的参数是缓慢变化的，本发明定义该系统的参数方程和观测方程为：
Figure BDA00030290185800001019
yk＝h(x，θk)+vk
式中：
θk为k时刻系统的参数向量；
yk为系统的观测值；
Figure BDA00030290185800001014
和υk分别为过程噪声和观测噪声，其协方差矩阵分别为
Figure BDA00030290185800001015
和
Figure BDA00030290185800001016
其中，过程噪声协方差矩阵
Figure BDA00030290185800001017
会影响到算法的收敛速度和跟踪性能，其值越大，历史数据对结果的影响衰减越快。可通过引入遗忘因子λ∈(0，1]，使得
Figure BDA00030290185800001018
以提供对历史数据对当前估计的近似指数衰减加权，衰减速度可通过λ的大小来进行调节。
类似于状态估计，KF应用于参数估计时的一般算法流程如图3所示。
结合扩展卡尔曼滤波在状态估计和参数估计中的应用，可设计对偶扩展卡尔曼滤波(dual extended kalman filter,DEKF)来解决对偶估计问题。
对偶扩展卡尔曼滤波算法将两个EKF结合起来交替运算，令状态估计器和参数估计器的输出结果互相参与彼此的预测和更新过程。它的形式可以是迭代的，也可以是序贯的。迭代形式的DEKF使用当前模型和所有观测值对系统状态进行估计，然后使用估计值和所有观测值估计系统参数(见图4(a))。该方案必须限制在离线应用程序中，应用之前应已收集了一批数据进行处理。序贯形式的DEKF在每次监测到数据时都将更新系统状态与参数(见图4(b))，这种特性使得它能够进行在线估算，故本发明采用这种形式，算法流程如图5所示。
其中：
Figure BDA0003029018580000111
为状态转移方程f(·)关于系统状态的雅可比矩阵；
Figure BDA0003029018580000112
为观测方程h(·)关于系统状态的雅可比矩阵；
Figure BDA0003029018580000113
为观测方程h(·)关于系统参数的雅可比矩阵。
需要指出的是，DEKF并不是状态估计EKF和参数估计EKF的简单组合。由于状态估计器的递归结构，
Figure BDA0003029018580000114
同时是
Figure BDA0003029018580000115
和
Figure BDA0003029018580000116
的函数，因此参数估计器中对于
Figure BDA0003029018580000117
的求取会引入两部分偏导，可以通过如下的迭代过程求解：
Figure BDA0003029018580000118
Figure BDA0003029018580000119
因为参数并不是状态的函数，状态估计器中的线性化可通过直接简单计算得到。
通过假设卡尔曼增益
Figure BDA00030290185800001110
独立于系统参数θ，本发明可以舍去这一项来大大简化算法，这是计算成本与性能改善之间的权衡问题。
以步骤(3)中的MC-EKF替换EKF，用于步骤(2)中跟踪模型的状态和参数估计，即为扩展目标跟踪的最大相关熵对偶卡尔曼滤波方法。
实施例
在2个仿真的场景中对本发明描述的过滤器进行评估，场景1和场景2分别为高斯噪声和非高斯噪声环境中的扩展目标跟踪，表1和表2中总结了这2个场景中所采用的主要参数。
表1 高斯噪声环境设置
Figure BDA00030290185800001111
Figure BDA0003029018580000121
表2 非高斯噪声环境设置
Figure BDA0003029018580000122
即长4.7m、宽1.8m的车辆在曲形道路上行驶，车辆从道路起点出发行至道路终点，全程速度恒定在30m/s。位于(0,0)的传感器以10Hz的频率测量目标的距离、方位和距离向范围，每个测量值都有独立的误差。
为了比较，本发明在仿真生成数据上运行了DEKF和MC-DEKF算法，以及相对应的EKF 和MC-EKF算法，在后二者的应用场景中，本发明不引入包含道路状况的参数向量而只使用目标的状态向量来独立地建立运动模型。
均方根误差(root mean squared error,RMSE)常用来衡量估计值同真值之间的偏差，本发明选择扩展目标的位置估计的RMSE作为性能指标对上述4种算法进行对比分析：
Figure BDA0003029018580000131
图7和图8分别为高斯和非高斯噪声环境中的估计结果，表3总结了滤波算法的位置估计RMSE。
表3 滤波算法的位置估计RMSE
EKF DEKF MC-EKF MC-DEKF
高斯噪声 6.2984 4.4920 6.3432 4.4916
非高斯噪声 8.5492 4.9156 6.8121 4.6174
仿真结果显示，传统的卡尔曼滤波器和基于最大相关熵的卡尔曼滤波器在高斯噪声环境中的滤波效果近似相同。但在非高斯噪声环境中，EKF和DEKF方法都多次出现受被非高斯噪声污染的观测数据影响较大而跟踪性能明显降低的情况，而MC-DEKF方法仍旧能够表现出较好的跟踪性能。

Claims (4)

1.一种扩展目标跟踪方法，其特征在于，包括以下步骤：
(1)将目标形状建模为椭圆，定义目标的状态向量x包含其x、y方向的位置和速度以及椭圆的长轴长度：
x＝[px py vx vy l]T
式中，x为目标的状态向量，px和py分别为目标在x方向和y方向上的位置，vx和vy分别为目标在x方向和y方向上的线速度幅值，l为椭圆的长轴长度，上角标T表示转置；
观测向量y包含目标质心的距离和方位观测，以及目标沿传感器—目标视线的距离向范围观测：
y＝[ρ ζ L]T
式中，y为观测向量，ρ为目标相对于传感器的距离，ζ为目标相对于传感器的方位，L为目标沿传感器—目标视线的距离向范围；
(2)通过参数向量θ来表示各路段，描述目标在道路网络上行进的受约束运动，建立扩展目标跟踪模型：
xk+1＝f(xk，θk)+ωk
yk＝h(xk)+vk
式中，xk+1为k+1时刻的目标状态，xk为k时刻的目标状态，θk为k时刻系统的参数向量，ωk为k时刻的过程噪声，υk为k时刻的观测噪声，yk为k时刻系统的观测值，f(·)为状态转移方程，h(·)为观测方程；
(3)设置核宽σ、收敛阈值ε和遗忘因子λ；
(4)为了获取初始系统的初始状态向量，取前N次测量数据，对位置观测进行线性拟合，从拟合的线性方程中提取得到参数的初始值：
θ0＝[a0 b0 c0]T
式中，θ0为系统的初始参数向量，a0、b0、c0共同描述了路段s的线性方程，即a0 sps,x+b0 sps ,y+c0 s＝0，其中ps,x和ps,y分别为位于路段s上的点的横纵坐标；
将位置观测在拟合结果上的投影作为目标的初始位置状态，并据此计算得到目标的初始速度状态和范围状态；初始化误差协方差矩阵
Figure FDA0003029018570000011
和
Figure FDA0003029018570000012
利用最大相关熵对偶卡尔曼滤波算法，分别对扩展目标跟踪模型中的各时刻的状态和参数进行估计。
2.根据权利要求1所述的一种扩展目标跟踪方法，其特征在于，所述的对扩展目标跟踪模型中的各时刻的状态和参数进行估计的步骤包括：
步骤ⅰ：计算参数的先验估计及其误差协方差矩阵：
Figure FDA0003029018570000021
Figure FDA0003029018570000022
式中，
Figure FDA0003029018570000023
为k-1时刻参数的后验估计，
Figure FDA0003029018570000024
为k时刻参数的先验估计，λ为遗忘因子，
Figure FDA0003029018570000025
为k-1时刻参数误差协方差矩阵的后验估计，
Figure FDA0003029018570000026
为k时刻参数误差协方差矩阵的先验估计；
步骤ⅱ：计算状态的先验估计及其误差协方差矩阵：
Figure FDA0003029018570000027
Figure FDA0003029018570000028
Figure FDA0003029018570000029
式中：
Figure FDA00030290185700000210
为k时刻状态的先验估计，
Figure FDA00030290185700000211
为k-1时刻状态的后验估计，
Figure FDA00030290185700000212
为k时刻参数的先验估计，Fk为k时刻状态转移方程f(·)关于系统状态的雅可比矩阵，
Figure FDA00030290185700000213
为k-1时刻状态误差协方差矩阵的后验估计，
Figure FDA00030290185700000214
为Fk的转置，
Figure FDA00030290185700000215
为k时刻过程噪声的协方差矩阵；
步骤ⅲ：通过不动点迭代法计算状态的后验估计及其误差协方差矩阵：
Figure FDA00030290185700000216
Figure FDA00030290185700000217
Figure FDA00030290185700000218
式中：
Figure FDA00030290185700000219
为k时刻状态的后验估计，
Figure FDA00030290185700000220
为k时刻状态的卡尔曼增益，yk为k时刻系统的观测值，
Figure FDA00030290185700000221
为k时刻状态误差协方差矩阵的后验估计，I为单位矩阵，
Figure FDA00030290185700000222
为k时刻观测方程h(·)关于系统状态的雅可比矩阵；
步骤ⅳ：通过不动点迭代法计算参数的后验估计及其误差协方差矩阵：
Figure FDA00030290185700000223
Figure FDA00030290185700000224
Figure FDA00030290185700000225
式中：
Figure FDA00030290185700000226
为k时刻参数的后验估计，
Figure FDA00030290185700000227
为k时刻参数的卡尔曼增益，
Figure FDA00030290185700000228
为k时刻观测方程h(·)关于系统参数的雅可比矩阵；
Figure FDA00030290185700000229
为k时刻参数误差协方差矩阵的先验估计，
Figure FDA00030290185700000230
为k时刻参数误差协方差矩阵的后验估计；
步骤v：重复步骤ⅰ-ⅳ。
3.根据权利要求2所述的一种扩展目标跟踪方法，其特征在于，最大相关熵对偶卡尔曼滤波算法，其目标函数表示为：
Figure FDA0003029018570000031
式中，Gσ(·)为高斯核，σ>0为核宽度，M为向量的维度，zk,m为k时刻状态先验估计和观测值所组合向量的第m行，dm(xk)为k时刻状态值和观测函数所组合向量的第m行。
4.根据权利要求2所述的一种扩展目标跟踪方法，其特征在于，所述的对偶卡尔曼滤波算法是将两个卡尔曼滤波器结合起来交替运算，令状态估计和参数估计结果互相参与彼此的预测和更新过程。
CN202110425063.1A 2021-04-20 2021-04-20 一种扩展目标跟踪方法 Active CN113189578B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110425063.1A CN113189578B (zh) 2021-04-20 2021-04-20 一种扩展目标跟踪方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110425063.1A CN113189578B (zh) 2021-04-20 2021-04-20 一种扩展目标跟踪方法

Publications (2)

Publication Number Publication Date
CN113189578A CN113189578A (zh) 2021-07-30
CN113189578B true CN113189578B (zh) 2022-09-16

Family

ID=76977611

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110425063.1A Active CN113189578B (zh) 2021-04-20 2021-04-20 一种扩展目标跟踪方法

Country Status (1)

Country Link
CN (1) CN113189578B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114609912B (zh) * 2022-03-18 2023-10-03 电子科技大学 基于伪线性最大相关熵卡尔曼滤波的仅测角目标追踪方法

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101614817A (zh) * 2009-06-24 2009-12-30 北京航空航天大学 一种基于地面动目标指示雷达系统的多目标跟踪方法
JP2010281782A (ja) * 2009-06-08 2010-12-16 Mitsubishi Electric Corp 目標追尾装置
CN108802692A (zh) * 2018-05-25 2018-11-13 哈尔滨工程大学 一种基于最大互相关熵容积粒子滤波的目标跟踪方法
CN108802721A (zh) * 2018-08-22 2018-11-13 哈尔滨工业大学 一种任意直线约束下目标跟踪方法
CN108983215A (zh) * 2018-05-25 2018-12-11 哈尔滨工程大学 一种基于最大互相关熵自适应无迹粒子滤波器的目标跟踪方法
CN109671099A (zh) * 2017-11-26 2019-04-23 电子科技大学 一种基于路网结构和目标特性的目标跟踪算法
CN110208792A (zh) * 2019-06-26 2019-09-06 哈尔滨工业大学 同时估计目标状态和轨迹参数的任意直线约束跟踪方法
WO2020087846A1 (zh) * 2018-10-31 2020-05-07 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
CN111596290A (zh) * 2020-06-01 2020-08-28 中国电子科技集团公司信息科学研究院 一种基于最大相关熵扩展卡尔曼滤波的雷达目标跟踪方法

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10175348B2 (en) * 2014-10-08 2019-01-08 Src, Inc. Use of range-rate measurements in a fusion tracking system via projections
CN106291645B (zh) * 2016-07-19 2018-08-21 东南大学 适于高维gnss/ins深耦合的容积卡尔曼滤波方法
DE102020123585A1 (de) * 2019-09-13 2021-08-19 Motional AD LLC (n.d.Ges.d. Staates Delaware) Erweiterte objektverfolgung mittels radar

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2010281782A (ja) * 2009-06-08 2010-12-16 Mitsubishi Electric Corp 目標追尾装置
CN101614817A (zh) * 2009-06-24 2009-12-30 北京航空航天大学 一种基于地面动目标指示雷达系统的多目标跟踪方法
CN109671099A (zh) * 2017-11-26 2019-04-23 电子科技大学 一种基于路网结构和目标特性的目标跟踪算法
CN108802692A (zh) * 2018-05-25 2018-11-13 哈尔滨工程大学 一种基于最大互相关熵容积粒子滤波的目标跟踪方法
CN108983215A (zh) * 2018-05-25 2018-12-11 哈尔滨工程大学 一种基于最大互相关熵自适应无迹粒子滤波器的目标跟踪方法
CN108802721A (zh) * 2018-08-22 2018-11-13 哈尔滨工业大学 一种任意直线约束下目标跟踪方法
WO2020087846A1 (zh) * 2018-10-31 2020-05-07 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
CN110208792A (zh) * 2019-06-26 2019-09-06 哈尔滨工业大学 同时估计目标状态和轨迹参数的任意直线约束跟踪方法
CN111596290A (zh) * 2020-06-01 2020-08-28 中国电子科技集团公司信息科学研究院 一种基于最大相关熵扩展卡尔曼滤波的雷达目标跟踪方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
Tracking Power System State Evolution with Maximum-correntropy-based Extended Kalman Filter;Julio A. D. Massignan et al.;《Journal of Modern Power Systems and Clean Energy》;20201231;全文 *
一种约束扩展卡尔曼粒子滤波器;张宏伟;《东莞理工学院学报》;20181106(第05期);全文 *
基于最大相关熵的雷达扩展卡尔曼滤波算法研究;王恒等;《中国传媒大学学报(自然科学版)》;20200625(第03期);全文 *
基于机载雷达的地面目标跟踪算法研究;王忠强等;《中国电子科学研究院学报》;20100820(第04期);全文 *

Also Published As

Publication number Publication date
CN113189578A (zh) 2021-07-30

Similar Documents

Publication Publication Date Title
Li et al. Kalman filter and its application
Agamennoni et al. An outlier-robust Kalman filter
Bilik et al. Maneuvering target tracking in the presence of glint using the nonlinear Gaussian mixture Kalman filter
CN108536171A (zh) 一种多约束下多无人机协同跟踪的路径规划方法
CN112154481B (zh) 基于多个测量假设的目标追踪
CN110361744A (zh) 基于密度聚类的rbmcda水下多目标跟踪方法
Leung et al. An improved weighting strategy for rao-blackwellized probability hypothesis density simultaneous localization and mapping
Malleswaran et al. IMM-UKF-TFS model-based approach for intelligent navigation
Lundgren et al. Variational Bayesian expectation maximization for radar map estimation
CN108871365B (zh) 一种航向约束下的状态估计方法及系统
CN113189578B (zh) 一种扩展目标跟踪方法
CN116047498A (zh) 基于最大相关熵扩展卡尔曼滤波的机动目标跟踪方法
Xia et al. Extended object tracking using hierarchical truncation measurement model with automotive radar
Leung et al. Evaluating set measurement likelihoods in random-finite-set slam
Havangi et al. A novel adaptive neuro-fuzzy unscented Kalman filter for SLAM
Havangi An adaptive particle filter based on PSO and fuzzy inference system for nonlinear state systems
CN104268597A (zh) 基于ahcif的集中式测量值扩维融合方法
CN111273279B (zh) 一种基于加速度噪声参数的多雷达数据处理方法
CN109474892B (zh) 基于信息形式的强鲁棒传感器网络目标跟踪方法
CN112800889A (zh) 一种基于分布式矩阵加权融合高斯滤波的目标跟踪方法
Zhang et al. The Unscented Kalman Filter Based Fusion Tracking Method with Multi-error Model
Nielsen et al. Relative target estimation using a cascade of extended Kalman filters
Allam et al. Stereo vision as a sensor for SLAM based smooth variable structure filter with an adaptive boundary layer width
Liu et al. A multiplicative noises and additive correlated noises cubature Kalman filter and its application in quadruped robot
Roumeliotis et al. Vision-aided inertial navigation

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant