法律状态公告日
法律状态信息
法律状态
2023-05-02
实质审查的生效 IPC(主分类):G06F17/16 专利申请号:2023100980051 申请日:20230210
实质审查的生效
技术领域
本发明涉及数字信号处理及最优估计领域,具体涉及一种基于改进Kalman滤波的机动目标跟踪方法。
背景技术
机动目标跟踪一直是数字信号处理领域的难点问题,其关键在于建立目标动态模型和状态估计算法。由于目标的机动性不可预测,通常难以建立精确的目标动态模型。传统方法则是通过引入目标状态的过程噪声来表征动态模型的精度,利用Kalman滤波结合历史状态和观测数据进行递推估计,从而得到目标状态的最优估计结果。然而,在实际应用中,传感器的观测噪声和系统状态的过程噪声统计特性难以先验已知,这将造成目标状态估计结果次优甚至发散。此外,目标机动也会影响当前时刻及后续一段时间的滤波估计结果,严重时会造成跟踪目标的丢失。
针对先验噪声统计特性未知的问题,可以利用噪声协方差估计方法解决,然而目标机动会造成当前时刻动态模型异常,使得新息被污染,异常的新息统计信息将严重影响噪声协方差的估计精度。针对目标机动的问题,可以利用自适应渐消Kalman滤波方法解决,然而先验噪声统计不精确会影响动态模型异常的判别及渐消因子的构造。因此,现有技术在先验噪声统计特性未知的情况下,无法有效跟踪机动目标的状态。
发明内容
针对现有技术中的上述不足,本发明提供的一种基于改进Kalman滤波的机动目标跟踪方法,能够在先验噪声统计特性不精确的前提下,有效跟踪机动目标的状态。
为了达到上述发明目的,本发明采用的技术方案为:
一种基于改进Kalman滤波的机动目标跟踪方法,包括以下步骤:
S1、根据机动目标运动特征、传感器测量值和机动目标状态的物理关系,构建包括机动目标的动态模型和感器测量模型的状态空间模型;
S2、对步骤S1中机动目标的动态模型异常进行检验与处理;
S3、对步骤S2中处理动态模型异常后的机动目标的状态空间模型进行异常新息的修正,得到稳健新息;
S4、根据步骤S3中的稳健新息估计并迭代修正噪声协方差;
S5、根据步骤S1中机动目标的状态空间模型和步骤S4中修正后的噪声协方差,得到机动目标的跟踪结果。
进一步地,步骤S1包括以下分步骤:
S11、根据机动目标运动特征,建立机动目标的动态模型:
其中:
S12、根据传感器测量值和机动目标状态的物理关系,建立机动目标的传感器测量模型:
其中:
S13、根据分步骤S11中建立的机动目标的动态模型和分步骤S12中建立的机动目标的传感器测量模型,构建机动目标的状态空间模型:
。
进一步地,步骤S2包括以下分步骤:
S21、对步骤S1中机动目标的状态空间模型输入传感器测量值、先验噪声协方差、状态初始值及其协方差;
S22、对机动目标的状态进行时间更新;
S23、从第二时刻开始根据卡方检验临界值判断机动目标的动态模型是否异常;若是则进入分步骤S24,否则进行测量更新并输出当前时刻机动目标的跟踪结果;
S24、根据卡方检验临界值和牛顿迭代法确认自适应渐消因子;
S25、根据分步骤S24中的自适应渐消因子修正预测协方差并进行测量更新。
进一步地,步骤S22包括以下分步骤:
S221、根据步骤S1中构建的机动目标的状态空间模型和上一时刻机动目标的状态向量,计算当前时刻机动目标的状态预测向量,表示为:
其中:
S222、根据上一时刻机动目标的状态向量的协方差,计算当前时刻机动目标的状态预测向量的协方差,表示为:
其中:
S223、根据当前时刻传感器的测量向量和分步骤S221中当前时刻机动目标的状态预测向量,计算当前时刻的新息,表示为:
其中:
S224、根据分步骤S222中机动目标的状态预测向量的协方差,计算当前时刻新息的协方差,计算式为:
其中:
进一步地,在步骤S23中,进行测量更新包括以下分步骤:
S231、根据分步骤S222中当前时刻机动目标的状态预测向量的协方差和分步骤S224中当前时刻新息的协方差,计算当前时刻的滤波增益,表示为:
其中:
S232、根据分步骤S221中当前时刻机动目标的状态预测向量、分步骤S223中当前时刻的新息和分步骤S231中当前时刻的滤波增益,计算当前时刻机动目标的状态向量,表示为:
其中:
S233、根据分步骤S222中当前时刻机动目标的状态预测向量的协方差和分步骤S231中当前时刻的滤波增益,计算当前时刻机动目标的状态向量的协方差,表示为:
其中:
进一步地,步骤S24包括以下分步骤:
S241、根据卡方检验临界值建立自适应渐消因子的等式,表示为
其中:
S242、采用牛顿法迭代计算分步骤S241中自适应渐消因子的等式,表示为:
其中:
进一步地,步骤S25包括以下分步骤:
S251、根据步骤S242中的自适应渐消因子修正状态预测协方差,表示为:
其中:
S252、根据步骤S242中的自适应渐消因子修正新息协方差,表示为:
其中:
S253、根据分步骤S251中修正后的状态预测协方差和分步骤S252中修正后的新息协方差,计算自适应渐消因子修正后的自适应增益,表示为:
其中:
S254、根据分步骤S253中自适应渐消因子修正后的自适应增益,计算机动目标状态的自适应估值,表示为:
其中:
S255、根据分步骤S251中修正后的状态预测协方差和分步骤S253中自适应渐消因子修正后的自适应增益,计算机动目标状态的协方差,表示为:
其中:
进一步地,步骤S3包括以下分步骤:
S31、根据分步骤S254中机动目标状态的自适应估值确定受污染前机动目标状态的预测向量,表示为:
其中:
S32、根据分步骤S31中受污染前机动目标状态的预测向量计算异常新息的初步修正值,表示为:
其中:
S33、判断当前时刻是否大于或等于传感器的最终测量时刻;若是则进入分步骤S34,否则跳转到分步骤S22;
S34、根据分步骤S32中异常新息的初步修正值和稳健估计方法,计算稳健新息,表示为:
其中:
进一步地,步骤S4包括以下分步骤:
S41、根据步骤S1中机动目标的状态空间模型,构造新息的状态空间模型,表示为:
其中:
S42、根据分步骤S41中新息的状态空间模型和S33中的稳健新息,建立线性的噪声协方差估计模型;
S43、计算分步骤S42中线性的噪声协方差估计模型的法方程,得到噪声协方差估值,表示为:
其中:
S44、判断分步骤S43中的噪声协方差估值是否收敛;若是则进入步骤S5,否则迭代修正噪声协方差估值并跳转到步骤S2。
进一步地,步骤S42包括以下分步骤:
S421、根据分步骤S41中新息的状态空间模型,得到新息自协方差的递推形式,表示为:
其中:
S422、根据分步骤S421中新息自协方差的递推形式和定义的窗口长度,确定新息自协方差序列,表示为:
其中:
S423、将分步骤S421中新息自协方差的递推形式代入到分步骤S422中的新息自协方差序列,可得新息自协方差序列的等式:
其中:
S424、利用时间平均从稳健新息中近似期望计算新息自协方差序列中的各元素,表示为:
其中:
S425、根据滤波稳态确定预测协方差的等式,表示为:
其中:
S426、联立分步骤S423中的新息自协方差序列的等式和分步骤S425中的预测协方差的等式,并利用克罗内克积运算将矩阵拉伸为向量,建立线性的噪声协方差估计模型,表示为:
其中:
本发明的有益效果为:
(1)本发明利用自适应估值和稳健估计的思想推导出了稳健新息,抵制了动态模型异常对噪声协方差估值的影响,提高了噪声协方差的估计精度,进而实现了机动目标状态的有效跟踪;
(2)本发明通过估计并迭代更新噪声协方差,抵制了噪声协方差不精确对动态模型异常的探测和自适应渐消因子的构造的影响,提升了滤波器的状态估计精度与自适应跟踪性能;
(3)本发明能够适用于任何具有系统状态不连续变化且先验噪声统计特性未知的线性时不变系统,以获得系统状态的最优跟踪估计。
附图说明
图1为一种基于改进Kalman滤波的机动目标跟踪方法流程图。
具体实施方式
下面对本发明的具体实施方式进行描述,以便于本技术领域的技术人员理解本发明,但应该清楚,本发明不限于具体实施方式的范围,对本技术领域的普通技术人员来讲,只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内,这些变化是显而易见的,一切利用本发明构思的发明创造均在保护之列。
如图1所示,一种基于改进Kalman滤波的机动目标跟踪方法,包括步骤S1-S5:
S1、根据机动目标运动特征、传感器测量值和机动目标状态的物理关系,构建包括机动目标的动态模型和感器测量模型的状态空间模型。
在本发明的一个可选实施例中,本发明根据机动目标运动的特征建立机动目标的常速度或常加速度动态模型,引入特定矩阵解构形式的机动目标的过程噪声协方差矩阵,初值未知则任意给定。根据机动目标的传感器测量值与机动目标状态的物理关系,建立机动目标的传感器测量模型,测量噪声协方差初值未知则根据传感器的精度经验给定。最后,组合机动目标的动态模型和传感器测量模型建立机动目标的状态空间模型。
步骤S1包括以下分步骤:
S11、根据机动目标运动特征,建立机动目标的动态模型:
其中:
具体地,本发明根据机动目标的运动特征,在低机动环境下采用机动目标的常速度动态模型,在高机动环境下采用机动目标的常加速度模型。
机动目标的常速度动态模型为:
其中:
机动目标的常加速度模型为:
其中:
具体地,本发明引入特定矩阵解构形式的过程噪声协方差矩阵Q。
对于机动目标的常速度动态模型,过程噪声协方差矩阵Q满足:
其中:
对于机动目标的常加速度动态模型,过程噪声协方差矩阵Q满足:
。
因此,可将机动目标的常速度动态模型和机动目标的常加速度动态模型简化为如下形式:
其中:
S12、根据传感器测量值和机动目标状态的物理关系,建立机动目标的传感器测量模型:
其中:
S13、根据分步骤S11中建立的机动目标的动态模型和分步骤S12中建立的机动目标的传感器测量模型,构建机动目标的状态空间模型:
。
S2、对步骤S1中机动目标的动态模型异常进行检验与处理。
在本发明的一个可选实施例中,本发明将机动目标的初始位置、速度或者加速度设置为0,并设置较大的机动目标的协方差提高初始状态的不确定性,第一时刻执行标准Kalman滤波。本发明从第二时刻开始构造卡方检验探测机动目标的动态模型的异常,通过卡方检验则执行标准Kalman滤波,即进行测量更新,否则利用牛顿迭代法求解自适应渐消因子,更新状态预测协方差后执行标准Kalman滤波,即进行自适应渐消因子修正后的测量更新,获得机动目标的状态的自适应估计结果。
步骤S2包括以下分步骤:
S21、对步骤S1中机动目标的状态空间模型输入传感器测量值、先验噪声协方差、状态初始值及其协方差。
具体地,本发明令初始机动目标的状态向量
S22、对机动目标的状态进行时间更新。
具体地,本发明从第一个时刻开始,对机动目标的状态进行时间更新。
步骤S22包括以下分步骤:
S221、根据步骤S1中构建的机动目标的状态空间模型和上一时刻机动目标的状态向量,计算当前时刻机动目标的状态预测向量,表示为:
其中:
S222、根据上一时刻机动目标的状态向量的协方差,计算当前时刻机动目标的状态预测向量的协方差,表示为:
其中:
S223、根据当前时刻传感器的测量向量和分步骤S221中当前时刻机动目标的状态预测向量,计算当前时刻的新息,表示为:
其中:
S224、根据分步骤S222中机动目标的状态预测向量的协方差,计算当前时刻新息的协方差,计算式为:
其中:
S23、从第二时刻开始根据卡方检验临界值判断机动目标的动态模型是否异常;若是则进入分步骤S24,否则进行测量更新并输出当前时刻机动目标的跟踪结果。
具体地,新息为零均值高斯分布,那么新息的马氏距离的平方服从卡方分布,表示为:
其中:
若卡方检验未通过,即:
在步骤S23中,进行测量更新包括以下分步骤:
S231、根据分步骤S222中当前时刻机动目标的状态预测向量的协方差和分步骤S224中当前时刻新息的协方差,计算当前时刻的滤波增益,表示为:
其中:
S232、根据分步骤S221中当前时刻机动目标的状态预测向量、分步骤S223中当前时刻的新息和分步骤S231中当前时刻的滤波增益,计算当前时刻机动目标的状态向量,表示为:
其中:
S233、根据分步骤S222中当前时刻机动目标的状态预测向量的协方差和分步骤S231中当前时刻的滤波增益,计算当前时刻机动目标的状态向量的协方差,表示为:
其中:
S24、根据卡方检验临界值和牛顿迭代法确认自适应渐消因子。
步骤S24包括以下分步骤:
S241、根据卡方检验临界值建立自适应渐消因子的等式,表示为
其中:
S242、采用牛顿法迭代计算分步骤S241中自适应渐消因子的等式,表示为:
其中:
S25、根据分步骤S24中的自适应渐消因子修正预测协方差并进行测量更新。
步骤S25包括以下分步骤:
S251、根据步骤S242中的自适应渐消因子修正状态预测协方差,表示为:
其中:
S252、根据步骤S242中的自适应渐消因子修正新息协方差,表示为:
其中:
S253、根据分步骤S251中修正后的状态预测协方差和分步骤S252中修正后的新息协方差,计算自适应渐消因子修正后的自适应增益,表示为:
其中:
S254、根据分步骤S253中自适应渐消因子修正后的自适应增益,计算机动目标状态的自适应估值,表示为:
其中:
S255、根据分步骤S251中修正后的状态预测协方差和分步骤S253中自适应渐消因子修正后的自适应增益,计算机动目标状态的协方差,表示为:
其中:
S3、对步骤S2中处理动态模型异常后的机动目标的状态空间模型进行异常新息的修正,得到稳健新息。
在本发明的一个可选实施例中,本发明为抵制机动目标的动态模型的异常对噪声协方差估值的影响,利用机动目标状态的自适应估值推导受污染前的机动目标状态的预测向量,获得初步修正后的新息。然后本发明基于稳健估计的思想,进一步修正残余的异常新息。
步骤S3包括以下分步骤:
S31、根据分步骤S254中机动目标状态的自适应估值确定受污染前机动目标状态的预测向量,表示为:
其中:
具体地,本发明根据自适应模型可得到上述计算式。自适应修正模型可表示为:
。
S32、根据分步骤S31中受污染前机动目标状态的预测向量计算异常新息的初步修正值,表示为:
其中:
S33、判断当前时刻是否大于或等于传感器的最终测量时刻;若是则进入分步骤S34,否则跳转到分步骤S22。
具体地,本发明在该分步骤判断当前时刻是否大于或等于传感器的最终测量时刻;若当前时刻大于或等于传感器的最终测量时刻,即
S34、根据分步骤S32中异常新息的初步修正值和稳健估计方法,计算稳健新息,表示为:
其中:
计算稳健因子包括以下分步骤:
S341、采用中位数法统计异常新息的初步修正值的中误差,表示为:
其中:
S342、根据分步骤S342中异常新息的初步修正值的中误差,计算标准化新息,表示为:
其中:
S343、利用IGGⅢ方案并根据分步骤S342中的标准化新息,计算稳健因子,表示为:
其中:
S4、根据步骤S3中的稳健新息估计并迭代修正噪声协方差。
在本发明的一个可选实施例中,本发明为抵制先验噪声协方差不精确对机动目标动态模型异常处理的影响,利用稳健的新息二阶统计信息建立过程噪声协方差和测量噪声协方差的线性估计模型,得到并将噪声协方差的估计结果作为先验噪声协方差回代给步骤2,重新执行步骤2–4,直到噪声协方差估值收敛后停止迭代,根据收敛的噪声协方差估值输出所有时刻机动目标的状态向量。
步骤S4包括以下分步骤:
S41、根据步骤S1中机动目标的状态空间模型,构造新息的状态空间模型,表示为:
其中:
S42、根据分步骤S41中新息的状态空间模型和S33中的稳健新息,建立线性的噪声协方差估计模型;
步骤S42包括以下分步骤:
S421、根据分步骤S41中新息的状态空间模型,得到新息自协方差的递推形式,表示为:
其中:
S422、根据分步骤S421中新息自协方差的递推形式和定义的窗口长度,确定新息自协方差序列,表示为:
其中:
S423、将分步骤S421中新息自协方差的递推形式代入到分步骤S422中的新息自协方差序列,可得新息自协方差序列的等式:
其中:
S424、利用时间平均从稳健新息中近似期望计算新息自协方差序列中的各元素,表示为:
其中:
S425、根据滤波稳态确定预测协方差的等式,表示为:
其中:
S426、联立分步骤S423中的新息自协方差序列的等式和分步骤S425中的预测协方差的等式,并利用克罗内克积运算将矩阵拉伸为向量,建立线性的噪声协方差估计模型,表示为:
其中:
具体地,由于机动目标常速度或常加速度的动态模型的过程噪声满足特定结构矩阵形式,因此可将上式转换为过程噪声参数和测量噪声协方差元素的估计形式,表达如下:
。
其中,
S43、计算分步骤S42中线性的噪声协方差估计模型的法方程,得到噪声协方差估值,表示为:
其中:
具体地,当
S44、判断分步骤S43中的噪声协方差估值是否收敛;若是则进入步骤S5,否则迭代修正噪声协方差估值并跳转到步骤S2。
S5、根据步骤S1中机动目标的状态空间模型和步骤S4中修正后的噪声协方差,得到机动目标的跟踪结果。
在本发明的一个可选实施例中,本发明将修正后的噪声协方差带回到机动目标的状态空间模型,可以得到更加可靠的所有时刻的机动目标的状态向量。
本领域的普通技术人员将会意识到,这里所述的实施例是为了帮助读者理解本发明的原理,应被理解为本发明的保护范围并不局限于这样的特别陈述和实施例。本领域的普通技术人员可以根据本发明公开的这些技术启示做出各种不脱离本发明实质的其它各种具体变形和组合,这些变形和组合仍然在本发明的保护范围内。
机译: 基于码本的方法基于Kalman滤波的语音增强方法
机译: 一种基于粒子滤波的鲁棒目标跟踪方法及系统
机译: 一种基于粒子滤波框架的鲁棒目标跟踪方法及系统