一种带有色量测噪声和变分贝叶斯自适应卡尔曼滤波的目标跟踪方法与流程

本发明属于船舶、飞机、车辆等运载体导航技术领域，具体涉及一种带有色量测噪声和变分贝叶斯自适应卡尔曼滤波的目标跟踪方法。

背景技术：

卡尔曼滤波器已广泛用于许多工程应用中，其中卡尔曼滤波器假设噪声协方差矩阵是已知精确的。然而，在许多具有不精确的噪声协方差矩阵的应用中，卡尔曼滤波器的性能可能会下降。基于变分贝叶斯自适应卡尔曼滤波器是解决这一问题的绝佳解决方案。通过选择适当的共轭先验分布，现有的变分贝叶斯自适应卡尔曼滤波器联合估计出状态向量、不精确且缓慢变化的系统噪声协方差矩阵和测量噪声协方差矩阵。由于现有的基于变分贝叶斯自适应卡尔曼滤波器专门针对白色测量噪声而设计，因此对于有色测量噪声，现有方法的性能可能会降低。

尽管使用现有的测量差分方法可以将带有有色测量噪声和不精确的系统噪声协方差矩阵和量测噪声协方差矩阵的问题转换为具有一步延迟状态和不精确的系统噪声协方差矩阵和量测噪声协方差矩阵的问题，但它将带来两个问题。首先，在当前时刻测量差分之后构造的状态空间模型的测量不仅取决于在当前的状态，同时也依赖于前一时刻的状态，而现有的变分贝叶斯自适应卡尔曼滤波器不能用于处理具有一步延迟状态的线性状态空间模型的问题；其次，在更新扩展状态向量的一步预测误差协方差矩阵的过程中，不仅需要估计一步预测误差协方差矩阵，还需要来自上一时刻的估计误差协方差矩阵，故现有的变分贝叶斯自适应卡尔曼滤波器无法估计扩展状态向量的一步预测误差协方差矩阵。

技术实现要素：

本发明的目的在于提供一种带有色量测噪声和变分贝叶斯自适应卡尔曼滤波的目标跟踪方法。

一种带有色量测噪声和变分贝叶斯自适应卡尔曼滤波的目标跟踪方法，该方法包括以下步骤：

步骤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⁽ⁱ⁺¹⁾(θ)，其中使用近似后验概率密度函数q⁽ⁱ⁾(ξ^(-θ))求出q⁽ⁱ⁺¹⁾(θ)，迭代收敛到上式的局部最优。

步骤5所述的通过变分贝叶斯方法联合估计扩展状态向量及其相应的一步预测协方差矩阵和量测协方差矩阵包括：联合概率密度p(ξ,y1:k)表示为：

令则有：

其中q⁽ⁱ⁺¹⁾(·)是概率密度函数q(·)在第i+1次迭代的近似，表示为：

其中，e⁽ⁱ⁾[ρ]是变量ρ在第i次迭代的期望值；

将更新为自由参数为和逆尺度矩阵为的逆wishart概率密度函数，

即：

其中，自由参数和逆尺度矩阵表示为：

令θ＝mk，则有：

其中，表示为下式：

将q⁽ⁱ⁺¹⁾(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⁽ⁱ⁺¹⁾(θ)，其中使用近似后验概率密度函数q⁽ⁱ⁾(ξ^(-θ))求出q⁽ⁱ⁺¹⁾(θ)。迭代收敛到(28)的局部最优。

步骤五：通过变分贝叶斯方法联合估计扩展状态向量及其相应的一步预测协方差矩阵和量测协方差矩阵。

利用(1)-(9)、(15)和(16)中的高斯-逆-wishart状态空间模型的条件独立特性，联合概率密度p(ξ,y1:k)可以表示为：

将(8)-(9)和(15)-(16)代入(30)得出：

利用式(31)，p(ξ,y1:k)可以表示为：

令再将式(32)代入式(28)中，我们可以得到：

其中q⁽ⁱ⁺¹⁾(·)是概率密度函数q(·)在第i+1次迭代的近似，可以表示为：

其中，e⁽ⁱ⁾[ρ]是变量ρ在第i次迭代的期望值。

通过方程(33)，可以更新为自由参数为和逆尺度矩阵为的逆wishart概率密度函数，即：

其中，自由参数和逆尺度矩阵可以被表示如下：

令θ＝mk，再将式(32)代入式(28)中，我们可以得到：

其中，被表示如下：

通过方程(38)，q⁽ⁱ⁺¹⁾(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＝1m²/s³，r＝100m²。真实的量测噪声协方差矩阵rk和真实的白色量测噪声协方差矩阵mk分别由下式给出：

新的观测矩阵gk由下式给出：

gk＝[hk-ψk-1hk-1]＝[i20-0.8i20](73)

步骤二：在目标跟踪中同时运行有色卡尔曼滤波器、现有的基于变分贝叶斯卡尔曼滤波器和所提出的滤波器。

有色卡尔曼滤波方程为：

在此实施例，名义上的系统噪声协方差矩阵和量测噪声协方差矩阵分别被选择为和其中，i4是四维单位矩阵。对噪声协方差矩阵为名义值和的有色卡尔曼滤波器(名义卡尔曼滤波)、噪声协方差矩阵为真实qk和rk的有色卡尔曼滤波器(真实卡尔曼滤波)、现有的估计pk和rk基于变分贝叶斯卡尔曼滤波器和所提出同时估计一步预测误差协方差矩阵和量测协方差矩阵mk的基于变分贝叶斯卡尔曼滤波器进行了对比。算法参数设置为：参数α＝1，参数β＝100，调整参数τ＝3，遗忘因子ρ＝1-exp(-4)和迭代次数n＝10。

步骤三：选择位置和速度的均方误差根和均方误差根均值作为性能指标。

为了评估状态估计精度，选择位置和速度的均方误差根和均方误差根均值作为性能指标，其定义如下：

其中，和分别是是在第s次蒙特卡洛仿真后的真实的位置和估计的位置，m＝1000代表了蒙特卡洛仿真的总次数。类似于位置的均方误差根和均方误差根均值方程，我们也可以写出速度的均方误差根和均方误差根均值方程。

步骤四：选择归一化f范数的平方根和归一化f范数的平方根均值作为性能指标。

为了评估一步预测误差协方差矩阵和量测协方差矩阵的估计精度，选择归一化f范数的平方根和归一化f范数的平方根均值作为性能指标，其定义如下：

其中，‖d‖²＝tr(dd^t)，和分别代表了在第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时估计性能都比现有的方法具有更高的估计精度。

从以上实施例不难看出，当噪声协方差矩阵不精确且量测噪声有色时，相对于有色卡尔曼滤波方法和现有的基于变分贝叶斯自适应卡尔曼滤波方法，本方法能够提高目标跟踪的性能。