本发明属于多目标跟踪技术领域，具体涉及一种高斯测量噪声均值和协方差均未知情形下的自适应估计测量噪声统计量的多目标跟踪方法。
背景技术：
随着传感器技术、计算机技术和自动化控制技术的发展，多目标跟踪技术在移动机器人自主导航、智能视频监控、卫星遥感监测和无线传感器网络等众多领域得到了广泛应用。
一些研究者利用随机有限集理论和概率假设密度滤波器对多目标跟踪问题进行建模和求解，有效克服了各个目标状态量与传感器测量值之间复杂的数据关联难题。在假设测量噪声为零均值高斯分布且其协方差先验已知的条件下，对目标状态采用高斯混合表示法，不需要执行高计算复杂度的簇集运算，并且可以将各种已有的高斯滤波算法直接用于单个高斯分量的预测和更新。
传统的基于高斯混合概率假设密度滤波器在目标状态估计过程中，通常都假设测量噪声的先验均值和协方差是精确已知的，然而在很多实际应用场合中常常会存在上述测量噪声先验统计量未知或者不准确的情况。在以上这些情况下，基于高斯混合概率假设密度滤波器的多目标跟踪方法容易发生估计偏差大、目标丢失和状态估计值不收敛等结果。
技术实现要素：
本发明提供了一种同时估计未知高斯测量噪声统计量的多目标跟踪方法，旨在测量系统的高斯噪声均值和协方差均未知情形下，除了对目标状态进行估计外，同时对测量噪声均值及协方差进行估计。
本发明的目的是通过以下技术方案来实现的：一种同时估计未知高斯测量噪声统计量的多目标跟踪方法，包括以下步骤：
(1)设定用于表征测量噪声统计量的正态逆威沙特分布超参数初始值，并初始化新生目标状态概率假设密度强度值；
(2)利用贝叶斯滤波递归算法和固定点分步迭代优化算法，计算每一个混合项对应参数的预测值和更新值；
(3)根据给定的删减阈值和融合阈值，对更新后的混合项进行删减和融合操作；
(4)从混合项集合中提取当前时刻目标状态估计值，并得到目标个数估计值。
进一步地，步骤(1)具体包括如下步骤：
(1-1)将测量噪声统计量θ的概率值表示为正态逆威沙特分布，并设定其参数初始值：
其中，p(θ0)为测量噪声统计量的初始概率值，θ0＝{r0,r0}表示测量噪声统计量初始值组合，r0和r0分别是测量噪声均值和协方差的初始值，μ0,κ0,ν0,λ0为正态逆威沙特分布
(1-2)将k时刻新生目标状态概率假设密度强度值表示为高斯分布分量与正态逆威沙特分布分量乘积混合项的加权和，并设定其初始值：
其中，γk(xk,θk)为新生目标状态概率假设密度强度值，xk表示k时刻目标状态向量，jγ,k表示上式中混合项的个数，
进一步地，步骤(2)对多目标状态和未知测量噪声统计量的联合概率假设密度强度值进行递归预测及更新计算，具体包括如下步骤：
(2-1)假定k-1时刻的联合后验概率假设密度强度值可表示成为如下乘积混合项的加权累加形式：
其中，υk-1(xk-1,θk-1)为k-1时刻目标状态xk-1和测量噪声统计量θk-1的联合后续概率假设密度强度值，jk-1为混合项个数，
(2-2)利用下式计算联合概率假设密度强度的预测值υk|k-1(xk,θk)：
υk|k-1(xk,θk)＝υs,k|k-1(xk,θk) γk(xk,θk)
其中，υs,k|k-1(xk,θk)表示已存在目标概率假设密度强度预测值，同样也表示成为正态逆威沙特分布和高斯分布的乘积加权累加形式：
上式中，ps,k为给定的目标生存概率，θk|k-1表示测量噪声统计量的预测值，
同时，正态逆威沙特分布分量的参数根据下式进行探索式进化推算：
其中，f(·)为给定的目标运动模型，qk表示已知高斯运动噪声的协方差，ρ∈(0,1]为遗忘因子。
(2-3)通过下式确定联合概率假设密度的后验强度值υk(xk,θk)：
其中，pd,k为已存在目标被成功探测到的概率，zk表示k时刻任一实际测量值，
上式中，jk|k-1为混合项个数，
(2-4)利用固定点分步迭代法计算上式中的相关参数：
给定初始迭代参数值如下：
在第i 1次迭代时，测量噪声的均值
其中，上标(i)＝1,2,…,nmax表示第i次迭代，nmax为最大迭代次数，nz表示测量值向量的维度。
根据下式分别计算测量预测值
其中，h(·)为给定的目标测量模型。
高斯分布分量中的目标状态均值和协方差由下式计算：
其中，新息协方差
正态逆威沙特分布分量的参数由下式计算：
其中，测量残差
一般地，以上步骤经过n次迭代后收敛，或者迭代次数到达给定的最大迭代次数nmax时，可得到混合项参数的集合如下：
其中，jk＝jk|k-1表示混合项个数，混合项的权值计算如下：
上式中ηk(zk)为杂波干扰强度，nfinal取n或nmax。
进一步地，步骤(3)中先将权值小于给定删减阈值的混合项进行剔除，然后将彼此间的mahalanobis距离小于给定融合阈值的混合项进行合并，最后当混合项个数仍然超过给定分量数目上限jmax时，只从中选取权值最大的jmax个混合项等待下一步处理。
进一步地，在步骤(4)中，从经过混合项删减与融合后的集合中，选取权值大于0.5的混合项对应的所有状态均值作为当前时刻的目标位置估计值，符合筛选条件的状态均值个数即为目标个数估计结果。
本发明的有益效果是：本发明首先利用正态逆威沙特分布对未知高斯测量噪声的均值和协方差进行建模，并将目标状态和测量噪声统计量表示为高斯分布与正态逆威沙特分布的联合概率假设密度形式，然后通过概率假设密度滤波算法对联合概率假设密度强度值进行预测，最后结合分步迭代法分别对目标状态和测量噪声统计量进行估计和修正。本发明方法在测量系统的高斯噪声均值和协方差均未知情形下，除了对目标状态进行估计外，同时对测量噪声均值及协方差进行估计，拓展了传统多目标跟踪算法的应用范围。
附图说明
图1为本发明方法的具体流程图；
图2为仿真实验环境下目标的真实运动轨迹；
图3为本发明方法对目标的运动轨迹的估计结果；
图4为本发明方法对未知测量噪声均值的估计结果；
图5为本发明方法对未知测量噪声标准差的估计结果；
图6为本发明方法与其他相关算法关于ospa距离误差的对比结果；
图7为本发明方法与其他相关算法关于估计目标数量的对比结果。
具体实施方式
为了更加具体地描述本发明，下面结合附图及具体实施方式对本发明的技术方案进行详细说明。
本实施例中，假设四个目标在一个长400米、宽300米的二维平面内随机运动，每个目标的状态向量表示为
其中，f为状态转换矩阵，vk表示零均值高斯运动噪声，qk为运动噪声的协方差，具体取值分别如下：
上式中，扫描周期δ＝1s，运动噪声标准差σv＝1。
目标的测量模型同样也为线性高斯模型，但其测量噪声的均值和协方差均未知，即：
实验中为了体现测量噪声的非零均值作用不可忽略(测量噪声均值大于其标准差的3倍)，真实的测量噪声均值和协方差分别设置为rtrue＝[9,9]和rtrue＝diag([1,1])。四个运动目标的初始位置、出现时间及消失时间分别设置如下：1号目标初始坐标位置为(-140m,120m)，其出现时间为时间步1，消失时间为时间步100；2号和3号目标初始坐标位置均为(-180m,80m)，出现时间分别为时间步20和1，消失时间分别为时间步100和80；4号目标初始坐标位置为(-140m,-120m)，出现时间为时间步10，消失时间为时间步90。目标的自发生成概率假设密度强度为：
其中，加权值wγ,k＝0.03，四个高斯分量的协方差均为pγ,k＝diag([25,10,25,10])，均值分别为
如图1所示，本发明涉及的一种同时估计未知高斯测量噪声统计量的多目标跟踪方法，包括以下步骤：
步骤1，将测量噪声统计量θ的概率值表示为正态逆威沙特分布，并设定其参数初始值：
其中，p(θ0)为测量噪声统计量的初始概率值，θ0＝{r0,r0}表示测量噪声统计量初始值组合，r0和r0分别是测量噪声均值和协方差的初始值，μ0,κ0,ν0,λ0为正态逆威沙特分布
步骤2，将k时刻新生目标状态概率假设密度强度值表示为高斯分布分量与正态逆威沙特分布分量乘积混合项的加权和，并设定其初始值：
其中，γk(xk,θk)为新生目标状态概率假设密度强度值，xk表示k时刻目标状态向量，jγ,k表示上式中混合项的个数，
步骤3，假定k-1时刻的联合后验概率假设密度强度值可表示成为如下乘积混合项的加权累加形式：
其中，υk-1(xk-1,θk-1)为k-1时刻目标状态xk-1和测量噪声统计量θk-1的联合后续概率假设密度强度值，jk-1为混合项个数，
步骤4，利用下式计算联合概率假设密度强度的预测值υk|k-1(xk,θk)：
υk|k-1(xk,θk)＝υs,k|k-1(xk,θk) γk(xk,θk)
其中，υs,k|k-1(xk,θk)表示已存在目标概率假设密度强度预测值，同样也表示成为正态逆威沙特分布和高斯分布的乘积加权累加形式：
上式中，ps,k为给定的目标生存概率，θk|k-1表示测量噪声统计量的预测值，
步骤5，正态逆威沙特分布分量的参数根据下式进行探索式进化推算：
其中，f(·)为给定的目标运动模型，qk表示已知高斯运动噪声的协方差，ρ∈(0,1]为遗忘因子。
步骤6，通过下式确定联合概率假设密度的后验强度值υk(xk,θk)：
其中，pd,k为已存在目标被成功探测到的概率，zk表示k时刻任一实际测量值，
上式中，jk|k-1为混合项个数，
步骤7，利用固定点分步迭代法计算上式中的相关参数：
给定初始迭代参数值如下：
在第i 1次迭代时，测量噪声的均值
其中，上标(i)＝1,2,…,nmax表示第i次迭代，nmax为最大迭代次数，nz表示测量值向量的维度。
步骤8，根据下式分别计算测量预测值
其中，h(·)为给定的目标测量模型。
高斯分布分量中的目标状态均值和协方差由下式计算：
其中，新息协方差
步骤9，正态逆威沙特分布分量的参数由下式计算：
其中，测量残差
一般地，以上步骤经过n次迭代后收敛，或者迭代次数到达给定的最大迭代次数nmax时，可得到混合项参数的集合如下：
其中，jk＝jk|k-1表示混合项个数，混合项的权值计算如下：
上式中ηk(zk)为杂波干扰强度，nfinal取n或nmax。
步骤10，先将权值小于给定删减阈值的混合项进行剔除，然后将彼此间的mahalanobis距离小于给定融合阈值的混合项进行合并，最后当混合项个数仍然超过给定分量数目上限jmax时，只从中选取权值最大的jmax个混合项等待下一步处理。
步骤11，从经过混合项删减与融合后的集合中，选取权值大于0.5的混合项对应的所有状态均值作为当前时刻的目标位置估计值，符合筛选条件的状态均值个数即为目标个数估计结果。
如图2所示为对本发明方法性能评估的测试环境，菱形表示目标的运动起始位置，正方形表示目标的运动结束位置，黑色实线表示目标的实际运动轨迹。利用本发明方法得到的目标跟踪整体效果如图3所示，其中×表示带有杂波干扰和一定漏检概率的目标位置测量值，o表示目标位置估计值。图4和图5分别是对未知的测量噪声均值和标准差的估计效果，实线表示均值和方差的真实值，虚线表示相应的两个维度上的估计值。从图中可以看出，随着时间步的向前推进，测量噪声均值和标准差的估计值渐渐收敛至相应的真实值附近。
为了对比测试本发明方法与其他已有算法的目标跟踪效果，同时实现了另外三种基于概率假设密度滤波器的多目标跟踪算法，分别是gmts-phd算法(标准的高斯混合概率假设密度滤波算法，且测量噪声均值设为真实值9.0，标准差设为真实值1.0)、gmtm-phd算法(标准的高斯混合概率假设密度滤波算法，测量噪声均值设为真实值9.0，但标准差设为2.0)和gm-vbphd算法(基于变分贝叶斯推断的自适应高斯混合概率假设密度滤波算法，测量噪声均值设为8.0，标准差自适应估计)。对所有上述算法均运行100次蒙特卡洛试验后，不同算法关于目标位置估计误差和目标个数统计估计结果对比分别如图6和图7所示。由图可知，在大约第10个时间步之后，由于本发明方法同时具备自适应估计未知测量噪声均值和协方差的能力，其目标位置估计误差和目标个数估计结果与gmts-phd算法相当，两者均明显优于其他对比算法。
以上所述的具体实施方式对本发明的技术方案和有益效果进行了详细说明，应理解的是以上所述仅为本发明的最优选实施例，并不用于限制本发明，凡在本发明的原则范围内所做的任何修改、补充和等同替换等，均应包含在本发明的保护范围之内。
1.一种同时估计未知高斯测量噪声统计量的多目标跟踪方法，其特征在于，包括：
(1)设定用于表征测量噪声统计量的正态逆威沙特分布超参数初始值，并初始化新生目标状态概率假设密度强度值；
(2)利用贝叶斯滤波递归算法和固定点分步迭代优化算法，计算每一个混合项对应参数的预测值和更新值；
(3)根据给定的删减阈值和融合阈值，对更新后的混合项进行删减和融合操作；
(4)从混合项集合中提取当前时刻目标状态估计值，并得到目标个数估计值。
2.如权利要求1所述的同时估计未知高斯测量噪声统计量的多目标跟踪方法，其特征在于，步骤(1)具体包括如下步骤：
(1-1)将测量噪声统计量θ的概率值表示为正态逆威沙特分布，并设定其参数初始值：
其中，p(θ0)为测量噪声统计量的初始概率值，θ0＝{r0,r0}表示测量噪声统计量初始值组合，r0和r0分别是测量噪声均值和协方差的初始值，μ0,κ0,ν0,λ0为正态逆威沙特分布
(1-2)将k时刻新生目标状态概率假设密度强度值表示为高斯分布分量与正态逆威沙特分布分量乘积混合项的加权和，并设定其初始值：
其中，γk(xk,θk)为新生目标状态概率假设密度强度值，xk表示k时刻目标状态向量，jγ,k表示上式中混合项的个数，
3.如权利要求1所述的同时估计未知高斯测量噪声统计量的多目标跟踪方法，其特征在于，步骤(2)对多目标状态和未知测量噪声统计量的联合概率假设密度强度值进行递归预测及更新计算，具体包括如下步骤：
(2-1)假定k-1时刻的联合后验概率假设密度强度值可表示成为如下乘积混合项的加权累加形式：
其中，υk-1(xk-1,θk-1)为k-1时刻目标状态xk-1和测量噪声统计量θk-1的联合后续概率假设密度强度值，jk-1为混合项个数，
(2-2)利用下式计算联合概率假设密度强度的预测值υk|k-1(xk,θk)：
υk|k-1(xk,θk)＝υs,k|k-1(xk,θk) γk(xk,θk)
其中，υs,k|k-1(xk,θk)表示已存在目标概率假设密度强度预测值，同样也表示成为正态逆威沙特分布和高斯分布的乘积加权累加形式：
上式中，ps,k为给定的目标生存概率，θk|k-1表示测量噪声统计量的预测值，
同时，正态逆威沙特分布分量的参数根据下式进行探索式进化推算：
其中，f(·)为给定的目标运动模型，qk表示已知高斯运动噪声的协方差，ρ∈(0,1]为遗忘因子。
(2-3)通过下式确定联合概率假设密度的后验强度值υk(xk,θk)：
其中，pd,k为已存在目标被成功探测到的概率，zk表示k时刻任一实际测量值，
上式中，jk|k-1为混合项个数，
(2-4)利用固定点分步迭代法计算上式中的相关参数：
给定初始迭代参数值如下：
在第i 1次迭代时，测量噪声的均值
其中，上标(i)＝1,2,…,nmax表示第i次迭代，nmax为最大迭代次数，nz表示测量值向量的维度。
根据下式分别计算测量预测值
其中，h(·)为给定的目标测量模型。
高斯分布分量中的目标状态均值和协方差由下式计算：
其中，新息协方差
正态逆威沙特分布分量的参数由下式计算：
其中，测量残差
以上步骤经过n次迭代后收敛，或者迭代次数到达给定的最大迭代次数nmax时，可得到混合项参数的集合如下：
其中，jk＝jk|k-1表示混合项个数，混合项的权值计算如下：
上式中ηk(zk)为杂波干扰强度，nfinal取n或nmax。
4.如权利要求1所述的同时估计未知高斯测量噪声统计量的多目标跟踪方法，其特征在于，步骤(3)中先将权值小于给定删减阈值的混合项进行剔除，然后将彼此间的mahalanobis距离小于给定融合阈值的混合项进行合并，最后当混合项个数仍然超过给定分量数目上限jmax时，只从中选取权值最大的jmax个混合项等待下一步处理。
5.如权利要求1所述的同时估计未知高斯测量噪声统计量的多目标跟踪方法，其特征在于：在步骤(4)中，从经过混合项删减与融合后的集合中，选取权值大于0.5的混合项对应的所有状态均值作为当前时刻的目标位置估计值，符合筛选条件的状态均值个数即为目标个数估计结果。
技术总结