CN109508445A - 一种带有色量测噪声和变分贝叶斯自适应卡尔曼滤波的目标跟踪方法 - Google Patents
一种带有色量测噪声和变分贝叶斯自适应卡尔曼滤波的目标跟踪方法 Download PDFInfo
- Publication number
- CN109508445A CN109508445A CN201910032588.1A CN201910032588A CN109508445A CN 109508445 A CN109508445 A CN 109508445A CN 201910032588 A CN201910032588 A CN 201910032588A CN 109508445 A CN109508445 A CN 109508445A
- Authority
- CN
- China
- Prior art keywords
- covariance matrix
- probability density
- measurement
- expressed
- matrix
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 67
- 238000001914 filtration Methods 0.000 title claims abstract description 20
- 230000006978 adaptation Effects 0.000 title abstract 2
- 239000011159 matrix material Substances 0.000 claims abstract description 170
- 238000005259 measurement Methods 0.000 claims abstract description 86
- 230000003044 adaptive effect Effects 0.000 claims description 17
- 230000007704 transition Effects 0.000 claims description 7
- 238000000354 decomposition reaction Methods 0.000 claims description 3
- 230000017105 transposition Effects 0.000 claims description 3
- 230000001131 transforming effect Effects 0.000 claims 1
- 239000000969 carrier Substances 0.000 abstract description 2
- 238000013398 bayesian method Methods 0.000 abstract 1
- 238000000342 Monte Carlo simulation Methods 0.000 description 3
- 238000004088 simulation Methods 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000015556 catabolic process Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000006731 degradation reaction Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000010606 normalization Methods 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
Classifications
-
- 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/18—Complex mathematical operations for evaluating statistical data, e.g. average values, frequency distributions, probability functions, regression analysis
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Data Mining & Analysis (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Mathematical Physics (AREA)
- Mathematical Analysis (AREA)
- Theoretical Computer Science (AREA)
- Computational Mathematics (AREA)
- Pure & Applied Mathematics (AREA)
- Mathematical Optimization (AREA)
- Probability & Statistics with Applications (AREA)
- Evolutionary Biology (AREA)
- Operations Research (AREA)
- Bioinformatics & Cheminformatics (AREA)
- Life Sciences & Earth Sciences (AREA)
- Algebra (AREA)
- Automation & Control Theory (AREA)
- Databases & Information Systems (AREA)
- Software Systems (AREA)
- General Engineering & Computer Science (AREA)
- Bioinformatics & Computational Biology (AREA)
- Measurement Of Mechanical Vibrations Or Ultrasonic Waves (AREA)
- Indication And Recording Devices For Special Purposes And Tariff Metering Devices (AREA)
Abstract
本发明属于船舶、飞机、车辆等运载体导航技术领域,具体涉及一种带有色量测噪声和变分贝叶斯自适应卡尔曼滤波的目标跟踪方法。包括1、建立目标跟踪的状态方程和量测方程。2、采用量测差分方法将有色量测噪声转化为白色量测噪声。3、将状态扩展向量的一步预测协方差矩阵和量测协方差矩阵的先验分布选择为逆Wishart分布。4、联合后验概率密度函数的变分近似。5、通过变分贝叶斯方法联合估计扩展状态向量及其相应的一步预测协方差矩阵和量测协方差矩阵。本发明的方法在带有不精确的噪声协方差矩阵和有色量测噪声情况下完成目标跟踪过程中的状态估计任务,其跟踪精度高于现有的基于其它滤波器的目标跟踪方法。
Description
技术领域
本发明属于船舶、飞机、车辆等运载体导航技术领域,具体涉及一种带有色量测噪声和变分贝叶斯自适应卡尔曼滤波的目标跟踪方法。
背景技术
卡尔曼滤波器已广泛用于许多工程应用中,其中卡尔曼滤波器假设噪声协方差矩阵是已知精确的。然而,在许多具有不精确的噪声协方差矩阵的应用中,卡尔曼滤波器的性能可能会下降。基于变分贝叶斯自适应卡尔曼滤波器是解决这一问题的绝佳解决方案。通过选择适当的共轭先验分布,现有的变分贝叶斯自适应卡尔曼滤波器联合估计出状态向量、不精确且缓慢变化的系统噪声协方差矩阵和测量噪声协方差矩阵。由于现有的基于变分贝叶斯自适应卡尔曼滤波器专门针对白色测量噪声而设计,因此对于有色测量噪声,现有方法的性能可能会降低。
尽管使用现有的测量差分方法可以将带有有色测量噪声和不精确的系统噪声协方差矩阵和量测噪声协方差矩阵的问题转换为具有一步延迟状态和不精确的系统噪声协方差矩阵和量测噪声协方差矩阵的问题,但它将带来两个问题。首先,在当前时刻测量差分之后构造的状态空间模型的测量不仅取决于在当前的状态,同时也依赖于前一时刻的状态,而现有的变分贝叶斯自适应卡尔曼滤波器不能用于处理具有一步延迟状态的线性状态空间模型的问题;其次,在更新扩展状态向量的一步预测误差协方差矩阵的过程中,不仅需要估计一步预测误差协方差矩阵,还需要来自上一时刻的估计误差协方差矩阵,故现有的变分贝叶斯自适应卡尔曼滤波器无法估计扩展状态向量的一步预测误差协方差矩阵。
发明内容
本发明的目的在于提供一种带有色量测噪声和变分贝叶斯自适应卡尔曼滤波的目标跟踪方法。
一种带有色量测噪声和变分贝叶斯自适应卡尔曼滤波的目标跟踪方法,该方法包括以下步骤:
步骤1:建立目标跟踪的状态方程和量测方程;
步骤2:采用量测差分方法将有色量测噪声转化为白色量测噪声;
步骤3:将状态扩展向量的一步预测协方差矩阵和量测协方差矩阵的先验分布选择为逆Wishart分布;
步骤4:联合后验概率密度函数的变分近似;
步骤5:通过变分贝叶斯方法联合估计扩展状态向量及其相应的一步预测协方差矩阵和量测协方差矩阵。
步骤1所述的建立目标跟踪的状态方程和量测方程,状态方程表示为:
其中,k是离散时间,表示状态向量,表示状态转移矩阵,表示均值为零协方差矩阵为Qk的系统高斯白噪声,量测方程表示为下式:
其中,表示量测向量,表示观测矩阵,表示均值为零协方差矩阵为Rk的量测高斯有色噪声。
步骤2所述的采用量测差分方法将有色量测噪声转化为白色量测噪声,其重建的具有白色量测噪声的量测方程yk表示为下式:
yk=Gkζk+ξk
其中,Gkζk=Hkxk-ψk-1Hk-1xk-1,且Gk=[Hk-ψHk-1],(·)T表示为矩阵转置运算;Gk表示为新的观测矩阵,ζk表示为k时刻扩展状态向量。
步骤3所述的将状态扩展向量的一步预测协方差矩阵和量测协方差矩阵的先验分布选择为逆Wishart分布包括:在卡尔曼滤波的框架下,一步预测概率密度函数和似然概率密度函数服从以下高斯分布,一步预测概率密度函数表示为下式:
似然概率密度函数表示为下式:
其中N(·;μ,Σ)代表了均值向量为μ协方差矩阵为Σ的高斯概率密度函数,表示k时刻的状态一步预测矩阵,表示k时刻的相应的预测误差协方差矩阵,表示为:
表示为下式:
其中(·)T代表转置运算,是在k-1时刻的状态向量估计,是在k-1时刻的相应的估计误差协方差矩阵,表示k时刻的状态一步预测矩阵,表示k时刻的相应的预测误差协方差矩阵,k时刻的状态一步预测矩阵表示为下式:
k时刻的相应的预测误差协方差矩阵表示为:
互协方差矩阵表示为:
由于和Mk都是服从高斯概率密度函数的协方差矩阵,其先验分布和p(Mk|y1:k-1)均选为逆Wishart概率密度函数,即:
其中,IW(·;μk,Σk)表示为自由度参数为μk,逆尺度矩阵为Σk的逆Wishart概率密度函数,为的自由度参数,为的逆尺度矩阵,为的自由度参数,为p(Mk|y1:k-1)的逆尺度矩阵。
步骤4所述的联合后验概率密度函数的变分近似包括:利用变分贝叶斯方法得到联合后验概率密度函数的自由分解近似,即:
其中,q(·)代表p(·)的近似后验概率密度函数,通过对因子分解的近似后验概率密度函数进行最小化Kullback-Leibler散度得出和q(Mk),联合后验概率密度函数即:
其中,表示为q(x)和p(x)之间的Kullback-Leibler散度,上式中的最优解满足下列等式:
其中,E[·]代表期望运算,log(·)代表对数函数,θ是Ξ中的任意元素,Ξ(-θ)是Ξ中除了θ以外所有元素的集合,cθ表示与变量θ相关的常量;和q(Mk)的变分参数是耦合的,采用定点迭代来求解,Ξ中任意元素θ的近似后验概率密度函数q(θ)更新为第i+1次迭代q(i+1)(θ),其中使用近似后验概率密度函数q(i)(Ξ(-θ))求出q(i+1)(θ),迭代收敛到上式的局部最优。
步骤5所述的通过变分贝叶斯方法联合估计扩展状态向量及其相应的一步预测协方差矩阵和量测协方差矩阵包括:联合概率密度p(Ξ,y1:k)表示为:
令则有:
其中q(i+1)(·)是概率密度函数q(·)在第i+1次迭代的近似,表示为:
其中,E(i)[ρ]是变量ρ在第i次迭代的期望值;
将更新为自由参数为和逆尺度矩阵为的逆Wishart概率密度函数,
即:
其中,自由参数和逆尺度矩阵表示为:
令θ=Mk,则有:
其中,表示为下式:
将q(i+1)(Mk)更新为自由参数和逆尺度矩阵的逆Wishart概率密度函数,即:
其中,自由参数和逆尺度矩阵表示为:
令则有:
其中,在i+1次迭代,均值向量和系统噪声协方差矩阵表示为:
其中,为第一状态向量估计值,为第二状态向量估计值,为第一协方差矩阵,为第二协方差矩阵,为第三协方差矩阵;
第一状态向量估计值表示为:
其中,
第二状态向量估计值表示为:
其中,
第一协方差矩阵表示为:
其中,
第二协方差矩阵表示为:
其中,
第三协方差矩阵表示为:
其中,
经过定点迭代N次后,后验概率密度函数和q(Mk)的变分近似被表示为:
本发明的有益效果在于:
(1)通过量测差分方法将带有不精确的噪声协方差矩阵和有色量测噪声状态空间模型转变为带有不精确的噪声协方差矩阵和白色量测噪声状态空间模型。
(2)利用变分贝叶斯方法联合估计扩展状态向量及其一步预测协方差矩阵和量测噪声协方差矩阵,从而在有色量测噪声的情况下,提升了自适应滤波精度。
附图说明
图1是基于带有色量测噪声和变分贝叶斯自适应卡尔曼滤波流程图。
图2(a)是目标跟踪的位置均方误差根。
图2(b)是目标跟踪的速度均方误差根。
图3(a)是目标跟踪的一步预测误差协方差矩阵的归一化F范数的平方根。
图3(b)是目标跟踪的量测协方差矩阵的归一化F范数的平方根。
图4(a)是当N=1,2,...,20时目标跟踪的位置的均方误差根均值。
图4(b)是当N=1,2,...,20时目标跟踪的速度的均方误差根均值。
图5(a)是当N=1,2,...,20时目标跟踪的一步预测误差协方差矩阵的归一化F范数的平方根均值。
图5(b)是当N=1,2,...,20时目标跟踪的量测噪声协方差矩阵的归一化F范数的平方根均值。
图6是当τ=2,3,4时目标跟踪的位置的均方误差根。
图7是当τ=2,3,4时目标跟踪的速度的均方误差根。
具体实施方式
下面将结合附图和实施例对本发明作进一步的详细说明。
本发明的目的是为了解决上述问题,提供了一种基于带有色量测噪声和变分贝叶斯自适应卡尔曼滤波的目标跟踪方法。在该方法中,基于量测差分方法将带有不精确噪声协方差矩阵和有色量测噪声的自适应状态估计问题,转化为带有不精确噪声协方差矩阵和白色量测噪声的自适应状态估计问题。将扩展状态向量的后验概率密度更新为高斯分布,由于已知均值高斯分布的协方差矩阵共轭可以保证后验分布与先验分布具有相同的函数形式,利用变分贝叶斯方法将扩展状态的一步预测协方差矩阵和量测协方差矩阵更新为逆Wishart分布。选择合适的滤波参数,利用变分贝叶斯方法联合估计扩展状态向量及其相应的一步预测协方差矩阵和量测协方差矩阵,最终提高目标跟踪精度。
在标准的变分贝叶斯方法中,选择KLD作为近似概率密度函数乘积和真实联合后验概率密度函数之间的距离度量,最优解通过最小化KLD获得。变分贝叶斯方法可以为近似后验概率密度函数提供封闭形式的解,并保证定点迭代的局部收敛。
本发明的一种基于带有色量测噪声和变分贝叶斯自适应卡尔曼滤波目标跟踪方法,流程图如图1所示,包括以下几个步骤:
步骤一:建立目标跟踪的状态方程和观测方程。
具体为,考虑如下离散时间线性随机状态空间模型:
其中(1)和(2)分别表示系统方程和量测方程,k是离散时间,表示状态向量,表示量测向量,表示状态转移矩阵,表示观测矩阵,表示均值为零协方差矩阵为Qk的系统高斯白噪声,表示均值为零协方差矩阵为Rk的量测高斯有色噪声。假设初始状态向量x0,其均值向量为协方差矩阵为P0|0的高斯分布。假设对任何j和k时刻x0,wk和vj互不相关。vk的一阶自回归模型可以表示如下:
vk=ψk-1vk-1+ξk (3)
其中,ψk-1是已知的相关参数。ξk是白色高斯概率密度函数,可以建模如下的高斯分布:
p(ξk)=N(ξk;0,Mk) (4)
其中N(·;μ,Σ)代表了均值向量为μ协方差矩阵为Σ的高斯概率密度函数。
步骤二:用量测差分方法将有色量测噪声转化为白色量测噪声。
具体为:
通过量测差分方法将有色量测噪声转化为白色量测噪声,被表示如下:
yk=zk-ψk-1zk-1 (5)
其中,yk是重建的具有白色量测噪声的量测方程。
yk=Gkζk+ξk (6)
Gkζk=Hkxk-ψk-1Hk-1xk-1 (7)
其中,Gk=[Hk-ψHk-1],(·)T表示为矩阵转置运算。Gk表示为新的观测矩阵,ζk表示为k时刻扩展状态向量。
步骤三:将状态扩展向量的一步预测协方差矩阵和量测协方差矩阵的先验分布选择为逆Wishart分布。
具体为:
在卡尔曼滤波的框架下,一步预测概率密度函数和似然概率密度函数服从以下高斯分布,即:
其中N(·;μ,Σ)代表了均值向量为μ协方差矩阵为Σ的高斯概率密度函数,和分别表示k时刻的状态一步预测和相应的预测误差协方差矩阵,和分别被表示如下:
其中(·)T代表转置运算,和分别是在k-1时刻的状态向量估计和相应的估计误差协方差矩阵,和分别表示k时刻的状态一步预测和相应的预测误差协方差矩阵,和互协方差矩阵分别被表示如下:
由于和Mk都是服从高斯概率密度函数的协方差矩阵,其先验分布和p(Mk|y1:k-1)均选为逆Wishart概率密度函数,即:
其中,IW(·;μk,Σk)表示为自由度参数为μk,逆尺度矩阵为Σk的逆Wishart概率密度函数,和分别为的自由度参数和逆尺度矩阵,和分别为p(Mk|y1:k-1)的自由度参数和逆尺度矩阵。
的均值设为名义上的一步预测误差协方差矩阵即:
其中,表示名义的系统协方差矩阵,且是所提出的基于变分贝叶斯卡尔曼滤波算法参数。令:
其中,τc≥0是调整参数。
根据贝叶斯准则,先验分布p(Mk|y1:k-1)表示为
p(Mk|y1:k-1)=∫p(Mk|Mk-1)p(Mk-1|y1:k-1)dMk-1 (21)
其中,p(Mk-1|y1:k-1)是量测噪声协方差矩阵Mk-1的后验概率密度函数。
后验概率密度函数p(Mk-1|y1:k-1)更新为逆Wishart概率密度函数,即:
其中,ρc∈(01]是遗忘因子。
初始量测协方差矩阵M0也被假定为逆Wishart概率密度函数,即为了获得初始量测协方差矩阵M0的先验信息,其均值被设为名义初始量测协方差矩阵即:
其中,名义初始量测噪声协方差矩阵是提出的基于变分贝叶斯卡尔曼滤波算法参数。
步骤四:联合后验概率密度函数的变分近似。
利用变分贝叶斯方法得到后验概率密度函数的自由分解近似,即:
其中,q(·)代表p(·)的近似后验概率密度函数,通过对因子分解的近似后验概率密度函数进行最小化Kullback-Leibler散度得出和q(Mk),联合后验概率密度函数即:
其中,表示为q(x)和p(x)之间的Kullback-Leibler散度。式(27)中的最优解满足下列等式:
其中,E[·]代表期望运算,log(·)代表对数函数,θ是Ξ中的任意元素,Ξ(-θ)是Ξ中除了θ以外所有元素的集合,cθ表示与变量θ相关的常量。由于和q(Mk)的变分参数是耦合的,我们需要采用定点迭代来求解(28),Ξ中任意元素θ的近似后验概率密度函数q(θ)更新为第i+1次迭代q(i+1)(θ),其中使用近似后验概率密度函数q(i)(Ξ(-θ))求出q(i +1)(θ)。迭代收敛到(28)的局部最优。
步骤五:通过变分贝叶斯方法联合估计扩展状态向量及其相应的一步预测协方差矩阵和量测协方差矩阵。
利用(1)-(9)、(15)和(16)中的高斯-逆-Wishart状态空间模型的条件独立特性,联合概率密度p(Ξ,y1:k)可以表示为:
将(8)-(9)和(15)-(16)代入(30)得出:
利用式(31),p(Ξ,y1:k)可以表示为:
令再将式(32)代入式(28)中,我们可以得到:
其中q(i+1)(·)是概率密度函数q(·)在第i+1次迭代的近似,可以表示为:
其中,E(i)[ρ]是变量ρ在第i次迭代的期望值。
通过方程(33),可以更新为自由参数为和逆尺度矩阵为的逆Wishart概率密度函数,即:
其中,自由参数和逆尺度矩阵可以被表示如下:
令θ=Mk,再将式(32)代入式(28)中,我们可以得到:
其中,被表示如下:
通过方程(38),q(i+1)(Mk)可以更新为自由参数和逆尺度矩阵的逆Wishart概率密度函数,即:
其中,自由参数和逆尺度矩阵可以被表示如下:
令再将式(32)代入式(28)中,我们可以得到:
其中,和可以被表示如下:
在i+1次迭代,定义修正后的一步预测概率密度函数和似然概率密度函数如下:
其中修正后的系统噪声协方差矩阵和量测噪声协方差矩阵被表示如下:
将(46)、(47)、(49)和(50)代入(43)中得出:
其中,归一化常数被表示如下:
根据式(45)-(51),可以更新为均值向量为和协方差矩阵为的高斯概率密度函数,即:
其中,在i+1次迭代,均值向量和协方差矩阵被分别表示如下:
其中,状态向量估计值和协方差矩阵和分别表示如下:
经过定点迭代N次后,后验概率密度函数和q(Mk)的变分近似被表示如下:
在实际工程应用中,应选足够大的迭代次数N,从而保证定点迭代局部收敛。
实施例:在缓慢变化的系统噪声协方差矩阵和量测协方差矩阵的目标跟踪问题中,目标根据二维笛卡尔坐标中的连续加速度运动模型进行移动,而且目标的位置由传感器收集。建立目标跟踪模型时,目标跟踪的有色量测噪声导致有色卡尔曼滤波器和现有的基于变分贝叶斯自适应卡尔曼滤波方法性能下降,而本发明的方法能获得更加优越的性能。下面以具体实施例子来说明本发明的优越性。具体如下:
步骤一:建立目标跟踪的状态方程和观测方程。
状态被定义为其中,xk,yk,和表示笛卡尔坐标和相应的速度。状态转移矩阵Fk-1和观测矩阵Hk分别由下式给出:
其中,参数Δt=1s是采样间隔,I2是二维的单位矩阵。类似于文献[8]真实的系统噪声协方差矩阵和量测噪声协方差矩阵由下式给出:
其中,T=1000s代表仿真时间,q=1m2/s3,r=100m2。真实的量测噪声协方差矩阵Rk和真实的白色量测噪声协方差矩阵Mk分别由下式给出:
新的观测矩阵Gk由下式给出:
Gk=[Hk -ψk-1Hk-1]=[I2 0 -0.8I2 0] (73)
步骤二:在目标跟踪中同时运行有色卡尔曼滤波器、现有的基于变分贝叶斯卡尔曼滤波器和所提出的滤波器。
有色卡尔曼滤波方程为:
在此实施例,名义上的系统噪声协方差矩阵和量测噪声协方差矩阵分别被选择为和其中,I4是四维单位矩阵。对噪声协方差矩阵为名义值和的有色卡尔曼滤波器(名义卡尔曼滤波)、噪声协方差矩阵为真实Qk和Rk的有色卡尔曼滤波器(真实卡尔曼滤波)、现有的估计Pk和Rk基于变分贝叶斯卡尔曼滤波器和所提出同时估计一步预测误差协方差矩阵和量测协方差矩阵Mk的基于变分贝叶斯卡尔曼滤波器进行了对比。算法参数设置为:参数α=1,参数β=100,调整参数τ=3,遗忘因子ρ=1-exp(-4)和迭代次数N=10。
步骤三:选择位置和速度的均方误差根和均方误差根均值作为性能指标。
为了评估状态估计精度,选择位置和速度的均方误差根和均方误差根均值作为性能指标,其定义如下:
其中,和分别是是在第s次蒙特卡洛仿真后的真实的位置和估计的位置,M=1000代表了蒙特卡洛仿真的总次数。类似于位置的均方误差根和均方误差根均值方程,我们也可以写出速度的均方误差根和均方误差根均值方程。
步骤四:选择归一化F范数的平方根和归一化F范数的平方根均值作为性能指标。
为了评估一步预测误差协方差矩阵和量测协方差矩阵的估计精度,选择归一化F范数的平方根和归一化F范数的平方根均值作为性能指标,其定义如下:
其中,‖D‖2=tr(DDT),和分别代表了在第s次蒙特卡洛仿真后估计的一步预测误差协方差矩阵和利用噪声协方差矩阵为真实Qk和Rk的有色卡尔曼滤波器得出精确的一步预测误差协方差矩阵。类似于一步预测误差协方差矩阵的归一化F范数的平方根和归一化F范数的平方根均值方程,我们也可以写出量测协方差矩阵的归一化F范数的平方根和归一化F范数的平方根均值方程。
实施效果:现有的滤波器和所提出的滤波器的位置的均方误差根以及一步预测误差协方差矩阵的归一化F范数的平方根分别如图2(a)和图3(a)所示。现有的滤波器和所提出的滤波器的速度的均方误差根以及量测协方差矩阵的归一化F范数的平方根分别如图2(b)和图3(b)所示。从图2(a)和图2(b)可以看出,所提出的方法比现有的有色卡尔曼滤波器和现有的变分贝叶斯卡尔曼滤波器具有更小的均方误差根。从图3(a)和图3(b)我们可以看出,所提出的方法比现有的方法具有更小的归一化F范数的平方根。因此,所提出的方法比现有最先进的滤波器具有更好的估计精度。
图4(a)和图5(a)分别显示了当N=1,2,...,20时,现有方法和所提出的方法的位置的均方误差根均值以及一步预测误差协方差矩阵的归一化F范数的平方根均值。图4(b)和图5(b)分别显示了当N=1,2,...,20时,现有方法和所提出的方法的速度的均方误差根均值以及量测噪声协方差矩阵的归一化F范数的平方根均值。可以从图4(a)、图4(b)、图5(a)和图5(b)中看出,当N≥5时所提出滤波器比现有的滤波器具有更小的均方误差根均值和归一化F范数的平方根均值,当N≥7时所提出的滤波器收敛。因此,所提出的滤波器相对于迭代次数展现出令人满意的收敛速度。
图6和图7显示了当τ=2,3,4时,现有方法和所提出的方法的位置和速度的均方误差根。由于当τ=4时,现有的变分贝叶斯滤波器发散,其仿真结果不在图6和图7中显示。可以从图6和图7看出,所提出的滤波器在τ=2,3,4时估计性能都比现有的方法具有更高的估计精度。
从以上实施例不难看出,当噪声协方差矩阵不精确且量测噪声有色时,相对于有色卡尔曼滤波方法和现有的基于变分贝叶斯自适应卡尔曼滤波方法,本方法能够提高目标跟踪的性能。
Claims (6)
1.一种带有色量测噪声和变分贝叶斯自适应卡尔曼滤波的目标跟踪方法,其特征在于,该方法包括以下步骤:
步骤1:建立目标跟踪的状态方程和量测方程;
步骤2:采用量测差分方法将有色量测噪声转化为白色量测噪声;
步骤3:将状态扩展向量的一步预测协方差矩阵和量测协方差矩阵的先验分布选择为逆Wishart分布;
步骤4:联合后验概率密度函数的变分近似;
步骤5:通过变分贝叶斯方法联合估计扩展状态向量及其相应的一步预测协方差矩阵和量测协方差矩阵。
2.根据权利要求1所述的一种带有色量测噪声和变分贝叶斯自适应卡尔曼滤波的目标跟踪方法,其特征在于,步骤1所述的建立目标跟踪的状态方程和量测方程,状态方程表示为:
其中,k是离散时间,表示状态向量,表示状态转移矩阵,表示均值为零协方差矩阵为Qk的系统高斯白噪声,量测方程表示为下式:
其中,表示量测向量,表示观测矩阵,表示均值为零协方差矩阵为Rk的量测高斯有色噪声。
3.根据权利要求2所述的一种带有色量测噪声和变分贝叶斯自适应卡尔曼滤波的目标跟踪方法,其特征在于,步骤2所述的采用量测差分方法将有色量测噪声转化为白色量测噪声,其重建的具有白色量测噪声的量测方程yk表示为下式:
yk=Gkζk+ξk
其中,Gkζk=Hkxk-ψk-1Hk-1xk-1,且Gk=[Hk-ψHk-1],(·)T表示为矩阵转置运算;Gk表示为新的观测矩阵,ζk表示为k时刻扩展状态向量。
4.根据权利要求3所述的一种带有色量测噪声和变分贝叶斯自适应卡尔曼滤波的目标跟踪方法,其特征在于,步骤3所述的将状态扩展向量的一步预测协方差矩阵和量测协方差矩阵的先验分布选择为逆Wishart分布包括:在卡尔曼滤波的框架下,一步预测概率密度函数和似然概率密度函数服从以下高斯分布,一步预测概率密度函数表示为下式:
似然概率密度函数表示为下式:
其中N(·;μ,Σ)代表了均值向量为μ协方差矩阵为Σ的高斯概率密度函数,表示k时刻的状态一步预测矩阵,表示k时刻的相应的预测误差协方差矩阵,表示为:
表示为下式:
其中(·)T代表转置运算,是在k-1时刻的状态向量估计,是在k-1时刻的相应的估计误差协方差矩阵,表示k时刻的状态一步预测矩阵,表示k时刻的相应的预测误差协方差矩阵,k时刻的状态一步预测矩阵表示为下式:
k时刻的相应的预测误差协方差矩阵表示为:
互协方差矩阵表示为:
由于和Mk都是服从高斯概率密度函数的协方差矩阵,其先验分布和p(Mk|y1:k-1)均选为逆Wishart概率密度函数,即:
其中,IW(·;μk,Σk)表示为自由度参数为μk,逆尺度矩阵为Σk的逆Wishart概率密度函数,为的自由度参数,为的逆尺度矩阵,为p(Mk|y1:k-1)的自由度参数,为p(Mk|y1:k-1)的逆尺度矩阵。
5.根据权利要求4所述的一种带有色量测噪声和变分贝叶斯自适应卡尔曼滤波的目标跟踪方法,其特征在于,步骤4所述的联合后验概率密度函数的变分近似包括:利用变分贝叶斯方法找出联合后验概率密度函数的自由分解,即:
其中,q(·)代表p(·)的相仿的后验概率密度函数,通过对因子分解的相仿的后验概率密度函数进行最小化Kullback-Leibler散度得出和q(Mk),联合后验概率密度函数即:
其中,表示为q(x)和p(x)之间的Kullback-Leibler散度,上式中的最优解满足下式:
其中,E[·]代表期望运算,log(·)代表对数函数,θ是Ξ中的任意元素,Ξ(-θ)是Ξ中除了θ以外所有元素的集合,cθ表示与变量θ相关的常量;和q(Mk)的变分参数是耦合的,采用定点迭代来求解,Ξ中任意元素θ的相仿的后验概率密度函数q(θ)更新为第i+1次迭代q(i+1)(θ),其中使用相仿的后验概率密度函数q(i)(Ξ(-θ))求出q(i+1)(θ),迭代收敛到上式的局部最优。
6.根据权利要求5所述的一种带有色量测噪声和变分贝叶斯自适应卡尔曼滤波的目标跟踪方法,其特征在于,步骤5所述的通过变分贝叶斯方法联合估计扩展状态向量及其相应的一步预测协方差矩阵和量测协方差矩阵包括:联合概率密度p(Ξ,y1:k)表示为:
令则有:
其中q(i+1)(·)是概率密度函数q(·)的第i+1次迭代,表示为:
其中,E(i)[ρ]是变量ρ在第i次迭代的期望值;
将更新为第一自由参数为和第一逆尺度矩阵为的逆Wishart概率密度函数,即:
其中,第一自由参数和第一逆尺度矩阵表示为:
令θ=Mk,则有:
其中,表示为下式:
将q(i+1)(Mk)更新为第二自由参数和第二逆尺度矩阵的逆Wishart概率密度函数,即:
其中,第二自由参数和第二逆尺度矩阵表示为:
令则有:
其中,在i+1次迭代,均值向量和系统噪声协方差矩阵表示为:
其中,为第一状态向量估计值,为第二状态向量估计值,为第一协方差矩阵,为第二协方差矩阵,为第三协方差矩阵;
第一状态向量估计值表示为:
其中,
第二状态向量估计值表示为:
其中,
第一协方差矩阵表示为:
其中,
第二协方差矩阵表示为:
其中,
第三协方差矩阵表示为:
其中,
经过定点迭代N次后,后验概率密度函数和q(Mk)的变分近似被表示为:
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910032588.1A CN109508445B (zh) | 2019-01-14 | 2019-01-14 | 一种带有色量测噪声和变分贝叶斯自适应卡尔曼滤波的目标跟踪方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910032588.1A CN109508445B (zh) | 2019-01-14 | 2019-01-14 | 一种带有色量测噪声和变分贝叶斯自适应卡尔曼滤波的目标跟踪方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN109508445A true CN109508445A (zh) | 2019-03-22 |
CN109508445B CN109508445B (zh) | 2023-05-05 |
Family
ID=65757801
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910032588.1A Active CN109508445B (zh) | 2019-01-14 | 2019-01-14 | 一种带有色量测噪声和变分贝叶斯自适应卡尔曼滤波的目标跟踪方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109508445B (zh) |
Cited By (22)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110034746A (zh) * | 2019-04-02 | 2019-07-19 | 汕头大学 | 一种基于最大协同熵卡尔曼滤波方法 |
CN110120066A (zh) * | 2019-04-11 | 2019-08-13 | 上海交通大学 | 面向监视系统的鲁棒群目标跟踪方法及跟踪系统 |
CN110503071A (zh) * | 2019-08-29 | 2019-11-26 | 电子科技大学 | 基于变分贝叶斯标签多伯努利叠加模型的多目标跟踪方法 |
CN110501686A (zh) * | 2019-09-19 | 2019-11-26 | 哈尔滨工程大学 | 基于一种新型自适应高阶无迹卡尔曼滤波的状态估计方法 |
CN110649911A (zh) * | 2019-07-17 | 2020-01-03 | 电子科技大学 | 一种基于α散度的分布式非线性卡尔曼滤波方法 |
CN110794409A (zh) * | 2019-10-21 | 2020-02-14 | 哈尔滨工程大学 | 一种可估计未知有效声速的水下单信标定位方法 |
CN111160266A (zh) * | 2019-12-30 | 2020-05-15 | 三一重工股份有限公司 | 物体跟踪方法和装置 |
CN111328015A (zh) * | 2020-01-28 | 2020-06-23 | 浙江大学 | 一种基于Fisher信息距离的无线传感器网络目标跟踪方法 |
CN111783358A (zh) * | 2020-07-02 | 2020-10-16 | 哈尔滨工业大学 | 一种基于贝叶斯估计的高超速飞行器长期轨迹预报方法 |
CN112468116A (zh) * | 2020-12-01 | 2021-03-09 | 哈尔滨工程大学 | 一种基于Gibbs采样器的自适应平滑方法 |
CN113008235A (zh) * | 2021-02-07 | 2021-06-22 | 西北工业大学 | 基于矩阵k-l散度的多源导航信息融合方法 |
CN113534128A (zh) * | 2020-10-21 | 2021-10-22 | 中国人民解放军空军预警学院 | 一种机载预警雷达海面机动舰船目标自适应跟踪方法 |
CN113779497A (zh) * | 2021-10-13 | 2021-12-10 | 东南大学 | 一种解决量测信息存在随机时延和丢包的目标跟踪方法 |
CN113919223A (zh) * | 2021-10-09 | 2022-01-11 | 福州大学 | 结合贝叶斯网络和卡尔曼滤波的结构系统噪声误差评估方法 |
CN113971752A (zh) * | 2021-09-18 | 2022-01-25 | 江苏大学 | 一种抗观测数据干扰的多车协同状态估计方法 |
CN114567288A (zh) * | 2022-01-25 | 2022-05-31 | 河南大学 | 基于变分贝叶斯的分布协同非线性系统状态估计方法 |
CN115379560A (zh) * | 2022-08-22 | 2022-11-22 | 昆明理工大学 | 一种无线传感网络中仅有距离量测信息下的目标定位与跟踪方法 |
CN115930971A (zh) * | 2023-02-01 | 2023-04-07 | 七腾机器人有限公司 | 一种机器人定位与建图的数据融合处理方法 |
CN116338573A (zh) * | 2023-03-30 | 2023-06-27 | 中国矿业大学 | 一种封闭空间考虑噪声误差特性的无人系统定位方法 |
CN116500575A (zh) * | 2023-05-11 | 2023-07-28 | 兰州理工大学 | 一种基于变分贝叶斯理论的扩展目标跟踪方法和装置 |
CN117095314A (zh) * | 2023-08-22 | 2023-11-21 | 中国电子科技集团公司第五十四研究所 | 跨域多维空地环境下的目标检测与重识别方法 |
CN117351042A (zh) * | 2023-10-08 | 2024-01-05 | 哈尔滨工业大学 | 一种基于高斯和滤波的任意步时延目标跟踪方法 |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106291645A (zh) * | 2016-07-19 | 2017-01-04 | 东南大学 | 适于高维gnss/ins深耦合的容积卡尔曼滤波方法 |
CN108599737A (zh) * | 2018-04-10 | 2018-09-28 | 西北工业大学 | 一种变分贝叶斯的非线性卡尔曼滤波器的设计方法 |
CN108763167A (zh) * | 2018-05-07 | 2018-11-06 | 西北工业大学 | 一种变分贝叶斯的自适应滤波方法 |
-
2019
- 2019-01-14 CN CN201910032588.1A patent/CN109508445B/zh active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106291645A (zh) * | 2016-07-19 | 2017-01-04 | 东南大学 | 适于高维gnss/ins深耦合的容积卡尔曼滤波方法 |
WO2018014602A1 (zh) * | 2016-07-19 | 2018-01-25 | 东南大学 | 适于高维gnss/ins深耦合的容积卡尔曼滤波方法 |
CN108599737A (zh) * | 2018-04-10 | 2018-09-28 | 西北工业大学 | 一种变分贝叶斯的非线性卡尔曼滤波器的设计方法 |
CN108763167A (zh) * | 2018-05-07 | 2018-11-06 | 西北工业大学 | 一种变分贝叶斯的自适应滤波方法 |
Non-Patent Citations (2)
Title |
---|
王荣等: "一种新的基于闪烁噪声的扩展目标跟踪方法", 《商洛学院学报》 * |
黄玉龙等: "带有色厚尾量测噪声的鲁棒高斯近似滤波器和平滑器", 《自动化学报》 * |
Cited By (38)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110034746B (zh) * | 2019-04-02 | 2023-10-13 | 汕头大学 | 一种基于最大协同熵卡尔曼滤波方法 |
CN110034746A (zh) * | 2019-04-02 | 2019-07-19 | 汕头大学 | 一种基于最大协同熵卡尔曼滤波方法 |
CN110120066A (zh) * | 2019-04-11 | 2019-08-13 | 上海交通大学 | 面向监视系统的鲁棒群目标跟踪方法及跟踪系统 |
CN110649911A (zh) * | 2019-07-17 | 2020-01-03 | 电子科技大学 | 一种基于α散度的分布式非线性卡尔曼滤波方法 |
CN110649911B (zh) * | 2019-07-17 | 2023-10-27 | 电子科技大学 | 一种基于α散度的分布式非线性卡尔曼滤波方法 |
CN110503071A (zh) * | 2019-08-29 | 2019-11-26 | 电子科技大学 | 基于变分贝叶斯标签多伯努利叠加模型的多目标跟踪方法 |
CN110503071B (zh) * | 2019-08-29 | 2021-11-23 | 电子科技大学 | 基于变分贝叶斯标签多伯努利叠加模型的多目标跟踪方法 |
CN110501686A (zh) * | 2019-09-19 | 2019-11-26 | 哈尔滨工程大学 | 基于一种新型自适应高阶无迹卡尔曼滤波的状态估计方法 |
CN110794409B (zh) * | 2019-10-21 | 2021-09-21 | 哈尔滨工程大学 | 一种可估计未知有效声速的水下单信标定位方法 |
CN110794409A (zh) * | 2019-10-21 | 2020-02-14 | 哈尔滨工程大学 | 一种可估计未知有效声速的水下单信标定位方法 |
CN111160266A (zh) * | 2019-12-30 | 2020-05-15 | 三一重工股份有限公司 | 物体跟踪方法和装置 |
CN111160266B (zh) * | 2019-12-30 | 2023-04-18 | 三一重工股份有限公司 | 物体跟踪方法和装置 |
CN111328015B (zh) * | 2020-01-28 | 2021-02-26 | 浙江大学 | 一种基于Fisher信息距离的无线传感器网络目标跟踪方法 |
CN111328015A (zh) * | 2020-01-28 | 2020-06-23 | 浙江大学 | 一种基于Fisher信息距离的无线传感器网络目标跟踪方法 |
CN111783358A (zh) * | 2020-07-02 | 2020-10-16 | 哈尔滨工业大学 | 一种基于贝叶斯估计的高超速飞行器长期轨迹预报方法 |
CN113534128A (zh) * | 2020-10-21 | 2021-10-22 | 中国人民解放军空军预警学院 | 一种机载预警雷达海面机动舰船目标自适应跟踪方法 |
CN112468116A (zh) * | 2020-12-01 | 2021-03-09 | 哈尔滨工程大学 | 一种基于Gibbs采样器的自适应平滑方法 |
CN112468116B (zh) * | 2020-12-01 | 2023-06-16 | 哈尔滨工程大学 | 一种基于Gibbs采样器的自适应平滑方法 |
CN113008235A (zh) * | 2021-02-07 | 2021-06-22 | 西北工业大学 | 基于矩阵k-l散度的多源导航信息融合方法 |
CN113971752B (zh) * | 2021-09-18 | 2024-03-19 | 江苏大学 | 一种抗观测数据干扰的多车协同状态估计方法 |
CN113971752A (zh) * | 2021-09-18 | 2022-01-25 | 江苏大学 | 一种抗观测数据干扰的多车协同状态估计方法 |
CN113919223A (zh) * | 2021-10-09 | 2022-01-11 | 福州大学 | 结合贝叶斯网络和卡尔曼滤波的结构系统噪声误差评估方法 |
CN113779497B (zh) * | 2021-10-13 | 2022-11-18 | 东南大学 | 一种解决量测信息存在随机时延和丢包的目标跟踪方法 |
CN113779497A (zh) * | 2021-10-13 | 2021-12-10 | 东南大学 | 一种解决量测信息存在随机时延和丢包的目标跟踪方法 |
CN114567288A (zh) * | 2022-01-25 | 2022-05-31 | 河南大学 | 基于变分贝叶斯的分布协同非线性系统状态估计方法 |
CN114567288B (zh) * | 2022-01-25 | 2024-04-26 | 河南大学 | 基于变分贝叶斯的分布协同非线性系统状态估计方法 |
CN115379560A (zh) * | 2022-08-22 | 2022-11-22 | 昆明理工大学 | 一种无线传感网络中仅有距离量测信息下的目标定位与跟踪方法 |
CN115379560B (zh) * | 2022-08-22 | 2024-03-08 | 昆明理工大学 | 一种无线传感网络中仅有距离量测信息下的目标定位与跟踪方法 |
CN115930971A (zh) * | 2023-02-01 | 2023-04-07 | 七腾机器人有限公司 | 一种机器人定位与建图的数据融合处理方法 |
CN115930971B (zh) * | 2023-02-01 | 2023-09-19 | 七腾机器人有限公司 | 一种机器人定位与建图的数据融合处理方法 |
CN116338573B (zh) * | 2023-03-30 | 2023-12-22 | 中国矿业大学 | 一种封闭空间考虑噪声误差特性的无人系统定位方法 |
CN116338573A (zh) * | 2023-03-30 | 2023-06-27 | 中国矿业大学 | 一种封闭空间考虑噪声误差特性的无人系统定位方法 |
CN116500575B (zh) * | 2023-05-11 | 2023-12-22 | 兰州理工大学 | 一种基于变分贝叶斯理论的扩展目标跟踪方法和装置 |
CN116500575A (zh) * | 2023-05-11 | 2023-07-28 | 兰州理工大学 | 一种基于变分贝叶斯理论的扩展目标跟踪方法和装置 |
CN117095314A (zh) * | 2023-08-22 | 2023-11-21 | 中国电子科技集团公司第五十四研究所 | 跨域多维空地环境下的目标检测与重识别方法 |
CN117095314B (zh) * | 2023-08-22 | 2024-03-26 | 中国电子科技集团公司第五十四研究所 | 跨域多维空地环境下的目标检测与重识别方法 |
CN117351042A (zh) * | 2023-10-08 | 2024-01-05 | 哈尔滨工业大学 | 一种基于高斯和滤波的任意步时延目标跟踪方法 |
CN117351042B (zh) * | 2023-10-08 | 2024-05-14 | 哈尔滨工业大学 | 一种基于高斯和滤波的任意步时延目标跟踪方法 |
Also Published As
Publication number | Publication date |
---|---|
CN109508445B (zh) | 2023-05-05 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109508445A (zh) | 一种带有色量测噪声和变分贝叶斯自适应卡尔曼滤波的目标跟踪方法 | |
CN106352876B (zh) | 一种基于h∞和ckf混合滤波的机载分布式pos传递对准方法 | |
Chang et al. | Kernel particle filter for visual tracking | |
AU2009289008B2 (en) | Estimating a state of at least one target | |
CN108717174B (zh) | 基于信息论的预测快速协方差交互融合无源协同定位方法 | |
CN111985093A (zh) | 一种带噪声估计器的自适应无迹卡尔曼滤波状态估计方法 | |
CN110501696B (zh) | 一种基于多普勒量测自适应处理的雷达目标跟踪方法 | |
Kalandros et al. | Tutorial on multisensor management and fusion algorithms for target tracking | |
CN110244303B (zh) | 基于sbl-admm的稀疏孔径isar成像方法 | |
CN108734725B (zh) | 基于高斯过程的概率数据关联滤波扩展目标跟踪方法 | |
CN109581281B (zh) | 基于到达时间差和到达频率差的移动目标定位方法 | |
CN108267731B (zh) | 无人机目标跟踪系统的构建方法及应用 | |
CN108871365B (zh) | 一种航向约束下的状态估计方法及系统 | |
CN111220980B (zh) | 一种地基sar非线性大气相位补偿方法 | |
Malleswaran et al. | IMM-UKF-TFS model-based approach for intelligent navigation | |
CN112083457A (zh) | 一种神经网络优化的imm卫星定位导航方法 | |
CN112346010B (zh) | 基于尺度差和时差的双机无源定位方法 | |
CN110136167A (zh) | 面向监视系统的多群目标跟踪方法及跟踪系统 | |
CN112034445A (zh) | 基于毫米波雷达的车辆运动轨迹跟踪方法和系统 | |
CN107886058B (zh) | 噪声相关的两阶段容积Kalman滤波估计方法及系统 | |
CN113219406B (zh) | 一种基于扩展卡尔曼滤波的直接跟踪方法 | |
CN115578425A (zh) | 一种应用在鱼苗计数器中基于无际卡尔曼滤波的动态追踪的方法 | |
CN113008235B (zh) | 基于矩阵k-l散度的多源导航信息融合方法 | |
CN114415157A (zh) | 一种基于水声传感器网络的水下目标多模型跟踪方法 | |
Han et al. | Linear recursive automotive target tracking filter for advanced collision warning systems |
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 |