一种目标跟踪方法、系统、设备、介质及数据处理终端与流程

1.本发明属于信号处理中状态估计与目标跟踪技术领域，尤其涉及一种目标跟踪方法、系统、设备、介质及数据处理终端。


背景技术：

2.目前，状态估计在信号处理、导航、电池管理、故障诊断和目标跟踪等领域有着广泛的应用。随着技术的突破越来越多，需估计的目标状态越来越容易发生快速变化，例如在目标跟踪中，飞行器等武器能够快速转变速度和加速度，使其能够完成复杂的运动，如转弯，蛇形，加速，翻滚等，使得目标的机动轨迹也越来越复杂。传统的目标跟踪方法难以适应这些不确定性的机动变化，因为这些目标跟踪方法中的大多数滤波器都是基于统计的信息，它们难以处理由运动模式的随机变化引起的不确定性。
3.目前以卡尔曼滤波器为基础的滤波器广泛应用于目标跟踪中，主要有线性卡尔曼滤波器，非线性的扩展卡尔曼滤波器、无迹卡尔曼滤波器和容积卡尔曼滤波器等，此类滤波器都是基于高斯噪声推导，且要求建立精确的状态模型。但实际目标在机动时，由于运动模型可能时刻发生变化，很难给滤波系统建立与运动模型一致的状态模型，如果建立的状态模型跟实际的运动模型不一致，系统很容易发散，导致状态估计效果不理想，且实际工程中噪声也往往不服从高斯分布，这导致该类方法很难满足实际的需求。
4.平滑变结构滤波器(svsf)是一种新型滤波器，其基于滑模控制理论，能够在模型不确定和存在噪声干扰的情况下保持良好的鲁棒性。在目标机动时，即使平滑变结构滤波器的状态模型与实际运动模型存在误差，当该误差导致的不确定性和噪声有上界时，其估计的状态会逐渐靠近真实状态，同时收敛在有界范围内。该滤波器对量测噪声种类没有要求，只要求噪声有上界值，可以应用于非高斯系统中。随着该理论的不断完善，平滑变结构滤波器被广泛应用于导航，无人驾驶，故障诊断等领域。
5.但现有平滑变结构滤波器也存在着问题：一、其一般应用于状态跟量测值有一一对应关系的系统，而现实情况中状态维数一般高于量测维数。比如在雷达目标跟踪中，需要估计的目标状态可能包含位置、速度和加速度等，而雷达只能测量位置信息，不能直接测量速度和加速度等其他状态。平滑变结构滤波器不能直接估计这些没有量测值的状态，针对这类问题现有平滑变结构滤波器有两种解决方案：(1)构造伪量测，例如通过两次的量测值除以采样时间求出速度的伪量测值。(2)构造低阶的luenberger估计器来估计没有量测值的状态。两种方法都主要应用于线性系统中。二、现有方法要求量测方程是线性的，实际的工程领域更多的是非线性系统，状态与量测之间的关系也呈现非线性，这使得该方法在实际应用中严重受限。因此，亟需一种新的目标跟踪方法及系统，以弥补现有技术的缺陷。
6.通过上述分析，现有技术存在的问题及缺陷为：
7.(1)目标的机动轨迹越来越复杂，由于现有目标跟踪方法中的大多数滤波器都是基于统计的信息，因此难以处理由运动模式的随机变化引起的不确定性。
8.(2)现有的卡尔曼类滤波器都是基于高斯噪声情况下，且要求建立精确的状态模
型，但实际目标在机动时，由于运动模型可能时刻发生变化，很难给滤波系统建立与运动模型一致的状态模型，导致系统很容易发散，估计效果不佳，且实际情况中噪声也往往不服从高斯分布，这导致该类方法很难满足实际的需求。
9.(3)现有平滑变结构滤波器主要应用于状态跟量测值有一一对应关系的系统，而现实情况中状态维数一般高于量测维数；在雷达目标跟踪中，平滑变结构滤波器不能直接估计没有量测值的状态。
10.(4)现有平滑变结构滤波器要求量测方程是线性的，实际的工程领域更多的是非线性系统，量测与状态之间关系呈现非线性，即量测方程是非线性的，这大大限制了平滑变结构滤波器的应用。
11.解决以上问题及缺陷的难度为：现有文献基于平滑变结构滤波器的方法中，几乎没有学者在状态方程和量测方程都为非线性的情况下提出有效方法来估计未测量状态，而实际工程中这些往往都是非线性的，本发明方法解决了这一局限。
12.解决以上问题及缺陷的意义：本发明方法使得平滑变结构滤波器既可以用于线性系统又可以用于非线性系统，使其不在受限于量测方程必须为线性的限制，可以像卡尔曼滤波器一样，具有良好的扩展性，针对非线性系统，应用本发明方法和其改进方法，可以用于各类系统中，更易于工程实践。本发明方法针对模型不确定系统，其鲁棒性好，精度高，能够很好的估计没有量测值对应的状态。


技术实现要素：

13.针对现有技术存在的问题，本发明提供了一种目标跟踪方法、系统、设备、介质及数据处理终端，尤其涉及一种基于平滑变结构器滤波器的目标跟踪方法、系统、设备、介质及数据处理终端。
14.本发明是这样实现的，一种目标跟踪方法，所述目标跟踪方法包括以下步骤：
15.步骤一，根据跟踪目标系统，建立离散线性系统或非线性系统，为系统选择合适的目标跟踪运动模型，并对系统进行初始化；
16.步骤二，通过状态方程对目标下一时刻状态进行预测，得到下一时刻状态的预测值，并根据量测方程和状态的预测值求出量测的预测值；
17.步骤三，输入下一时刻传感器的量测数据，根据此量测数据和量测的预测值，求得新息；
18.步骤四，通过新息，根据平滑变结构滤波器求量测增益项；
19.步骤五，通过状态与量测的协方差和量测预测值的协方差，求得状态与量测增益项对应的系数；通过该系数，即各个状态与增益项对应的系数，求得预测状态的修正项，估计出状态最终值。
20.步骤六，根据状态的预测值，量测增益项和状态与量测增益项对应的系数得到最终的状态估计结果，并输出状态；
21.步骤七，输出状态后，返回步骤二，进行迭代，估计后续时刻的状态；
22.进一步，步骤一中，所述离散的系统方程如下：
23.x
k 1
＝f(xk) wk；
24.z
k 1
＝h(x
k 1
) υ
k 1
；
25.式中，xk表示k时刻的状态，f表示状态从k转移到下一时刻k 1的转换关系，wk表示过程噪声，h表示状态与量测之间的对应关系，z
k 1
表示k 1时刻的量测，υ
k 1
表示量测噪声；步骤二中，所述状态的预测值和量测的预测值分别用以下公式计算：
[0026][0027][0028]
步骤三中，所述量测的新息e
k 1|k
由以下公式计算得到：
[0029][0030]
式中，z
k 1
表示k 1时刻系统通过传感器测量到的量测值。
[0031]
步骤四中，所述量测对应的增益项g
k 1
由以下公式计算得到：
[0032][0033]
式中，|e|表示求e的绝对值；γ表示收敛率，取值为0≤γ＜1；表示矩阵的对应元素相乘；sat表示饱和函数；ψ表示平滑有界层宽度；
[0034]
步骤五中，通过系统方程求得状态与量测的协方差p
xz
和量测预测值的协方差p
zz
后，求得状态与量测增益项对应的系数c
k 1
为：
[0035]ck 1
＝p
xz
*(p
zz
)-1
。
[0036]
步骤六中，最终得到的状态估计值由以下公式计算得到：
[0037][0038]
进一步，步骤五中，所述求得状态与量测增益项对应的系数，包括：
[0039]
对于不同的系统有不同的方法，当步骤一中系统方程为线性方程时，步骤二中状态预测值的协方差p
k 1|k
根据以下公式计算得到：
[0040]
p
k 1|k
＝fp
k|kft
qk；
[0041]
式中，状态转移矩阵f为f的具体值，f
t
是矩阵f的转置，p
k|k
表示k时刻的状态的协方差，qk代表过程噪声wk的协方差。
[0042]
则状态与量测的协方差p
xz
由以下公式计算得到：
[0043]
p
xz
＝p
k 1|kht
；
[0044]
式中，h为状态与量测对应的转换矩阵，是公式中h在线性系统中的具体值，h
t
为h的转置。则量测的预测值的协方差p
zz
由以下公式计算得到：
[0045]
p
zz
＝hp
k 1|kht
；
[0046]
则状态与量测增益项对应的系数c
k 1
通过以下公式计算得到：
[0047]ck 1
＝p
xz
*(p
zz
)-1
。
[0048]
进一步，步骤五中，所述求得状态与量测增益项对应的系数，包括：
[0049]
当步骤一中系统方程为非线性方程时，则在求协方差时需要对系统方程进行处理，如对非线性系统进行线性近似处理，求系统方程的一阶泰勒展开，求f和h的雅克比矩阵的公式分别如下：
[0050]
[0051][0052]
步骤二中状态预测值的协方差p
k 1|k
根据以下公式计算得到：
[0053][0054]
则状态与量测的协方差p
xz
和量测预测值的协方差p
zz
分别用以下公式计算：
[0055][0056][0057]
则状态与量测增益项对应的系数c
k 1
通过以下公式计算得到：
[0058]ck 1
＝p
xz
*(p
zz
)-1
。
[0059]
进一步，步骤五中，所述求得状态与量测增益项对应的系数，包括：
[0060]
当步骤一中系统方程为非线性方程时，对于非线性系统方程的处理有多种方法，如求其均值和协方差可采用无迹变换的方法；
[0061]
根据状态和p
k|k
求得2n 1个采样点，采样点x
i,k|k
和其相对应的权值wi由以下公式分别求得：
[0062][0063][0064]
式中，κ是一个尺度参数，是(n κ)p
k|k
的均方根矩阵的第i行或者第i列，n是状态的维数，i为第几个采样点。
[0065]
采样点x
i,k|k
对应的状态的预测值由以下公式计算得到：
[0066][0067]
则步骤二中的状态预测估计值和状态预测协方差p
k 1|k
由以下公式求得：
[0068][0069][0070]
则步骤二中的量测的预测值通过采样点的预测值由以下公式求得：
[0071][0072]
[0073]
式中，是采样点对应的量测预测值，wi是其权值。
[0074]
则状态与量测的协方差p
xz
和量测预测值的协方差p
zz
分别用以下公式计算：
[0075][0076][0077]
则状态与量测增益项对应的系数c
k 1
通过以下公式计算得到：
[0078]ck 1
＝p
xz
*(p
zz
)-1
。
[0079]
本发明的另一目的在于提供一种应用所述的目标跟踪方法的目标跟踪系统，所述目标跟踪系统包括：
[0080]
系统初始化模块，用于根据跟踪目标系统，建立离散线性系统或非线性系统，为系统选择合适的目标跟踪运动模型，并对系统进行初始化；
[0081]
预测值计算模块，用于通过状态方程对目标下一时刻状态进行预测，得到下一时刻状态的预测值，并根据量测方程和状态的预测值求出量测的预测值；
[0082]
量测新息求取模块，用于通过输入下一时刻传感器的量测数据，根据此量测数据和量测的预测值，求得新息；
[0083]
量测增益项求取模块，用于通过新息，根据平滑变结构滤波器求量测增益项；
[0084]
系数计算模块，用于通过状态与量测的协方差和量测预测值的协方差，求得状态与量测增益项对应的系数；
[0085]
状态估计结果获取模块，用于根据状态的预测值，量测增益项和状态与量测增益项对应的系数得到最终的状态估计结果，并输出状态；
[0086]
迭代计算模块，用于在输出状态后，返回预测值计算模块，进行迭代，估计后续时刻的状态。
[0087]
本发明的另一目的在于提供一种计算机设备，所述计算机设备包括存储器和处理器，所述存储器存储有计算机程序，所述计算机程序被所述处理器执行时，使得所述处理器执行如下步骤：
[0088]
(1)根据跟踪目标系统，建立离散线性系统或非线性系统，为系统选择合适的目标跟踪运动模型，并对系统进行初始化；
[0089]
(2)通过状态方程对目标下一时刻状态进行预测，得到下一时刻状态的预测值，并根据量测方程和状态的预测值求出量测的预测值；
[0090]
(3)输入下一时刻传感器的量测数据，根据此量测数据和量测的预测值，求得新息；通过新息，根据平滑变结构滤波器求量测增益项；
[0091]
(4)通过状态与量测的协方差和量测预测值的协方差，求得状态与量测增益项对应的系数；
[0092]
(5)根据状态的预测值，量测增益项和状态与量测增益项对应的系数得到最终的状态估计结果，并输出状态；
[0093]
(6)输出状态后，返回步骤(2)，进行迭代，估计后续时刻的状态。
[0094]
本发明的另一目的在于提供一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，使得所述处理器执行如下步骤：
[0095]
(1)根据跟踪目标系统，建立离散线性系统或非线性系统，为系统选择合适的目标跟踪运动模型，并对系统进行初始化；
[0096]
(2)通过状态方程对目标下一时刻状态进行预测，得到下一时刻状态的预测值，并根据量测方程和状态的预测值求出量测的预测值；
[0097]
(3)输入下一时刻传感器的量测数据，根据此量测数据和量测的预测值，求得新息；通过新息，根据平滑变结构滤波器求量测增益项；
[0098]
(4)通过状态与量测的协方差和量测预测值的协方差，求得状态与量测增益项对应的系数；
[0099]
(5)根据状态的预测值，量测增益项和状态与量测增益项对应的系数得到最终的状态估计结果，并输出状态；
[0100]
(6)输出状态后，返回步骤(2)，进行迭代，估计后续时刻的状态。
[0101]
本发明的另一目的在于提供一种信息数据处理终端，所述信息数据处理终端用于实现所述的目标跟踪系统。
[0102]
本发明的另一目的在于提供一种所述的目标跟踪系统在航空航天、雷达跟踪以及导航定位领域中的应用。
[0103]
结合上述的所有技术方案，本发明所具备的优点及积极效果为：本发明提供的目标跟踪方法，基于平滑变结构滤波器基础原理，利用其协方差求得没有量测值的状态，通过不同方法可以使该系统可应用于线性和非线性的高斯与非高斯系统中。本发明估计出没有量测值的状态的精度高，系统鲁棒性好，突破了原始方法的限制，可以像卡尔曼类滤波器一样应用于各类的状态估计系统中，大大的扩展了该方法的应用范围，具有理论和工程价值，可应用于航空航天，雷达跟踪，导航定位等领域。
[0104]
本发明的目的在于提出新的方法估计没有量测值的状态，同时突破传统方法对量测方程要求必须在线性系统下的限制，扩展该类方法的应用范围，能够使其应用于线性与非线性，高斯与非高斯系统中。
[0105]
本发明提出了一种新的平滑变结构滤波器目标跟踪方法。跟传统方法相比，本发明具有良好的扩展性，能应用线性与非线性系统中，结合不同的非线性处理方法，可以得到不同种类的状态估计器；本发明易于实现且收敛性好，相比传统方法构造伪量测和使用低阶的估计器去估计没有量测值的状态，本发明在使用平滑变结构滤波器理论中不需要为没有量测值的状态设置平滑有界层宽度参数，使得本发明更适宜工程实用。同时，本发明具有良好的鲁棒性和估计精度，能够应用与线性和非线性，高斯与非高斯系统中。在机动目标跟踪中，本发明所提出的方法在鲁棒性更高，在目标机动时其估计精度不会像传统方法一样显著降低，跟踪精度更高。
[0106]
由于本发明完善了传统平滑变结构滤波器在状态维数大于量测维数估计系统存在的缺陷，同时弥补了传统方法要求量测方程必须为线性方程的缺陷。因此本发明中所提出的方法使得平滑变结构滤波器可以像卡尔曼滤波器一样，具有良好的扩展性，可以以本发明为基础，扩展提出不同类型的方法，这将促进本发明在理论上的延伸，同时也可以根据不同的工程实际需要选择不同的方法进行完善，这将大大促进本发明在信号处理，控制工程等领域的应用。
附图说明
[0107]
为了更清楚地说明本发明实施例的技术方案，下面将对本发明实施例中所需要使用的附图做简单的介绍，显而易见地，下面所描述的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下还可以根据这些附图获得其他的附图。
[0108]
图1是本发明实施例提供的目标跟踪方法流程图。
[0109]
图2是本发明实施例提供的目标跟踪方法原理图。
[0110]
图3是本发明实施例提供的目标跟踪系统结构框图；
[0111]
图中：1、系统初始化模块；2、预测值计算模块；3、量测新息求取模块；4、量测增益项求取模块；5、系数计算模块；6、状态估计结果获取模块；7、迭代计算模块。
[0112]
图4是本发明实施例提供的示例1的一次仿真结果的轨迹示意图，包括真实轨迹、量测点和经过不同滤波方法处理的轨迹。
[0113]
图5是本发明实施例提供的示例1的x和y轴上不同方法估计结果的位置绝对误差(m)示意图。
[0114]
图6是本发明实施例提供的示例1的x和y轴上不同方法估计结果的速度绝对误差(m/s)示意图。
具体实施方式
[0115]
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
[0116]
针对现有技术存在的问题，本发明提供了一种目标跟踪方法、系统、设备、介质及数据处理终端，下面结合附图对本发明作详细的描述。
[0117]
如图1所示，本发明实施例提供的目标跟踪方法包括以下步骤：
[0118]
s101，根据跟踪目标系统，建立离散线性系统或非线性系统，为系统选择合适的目标跟踪运动模型，并对系统进行初始化；
[0119]
s102，通过状态方程对目标下一时刻状态进行预测，得到下一时刻状态的预测值，并根据量测方程和状态的预测值求出量测的预测值；
[0120]
s103，输入下一时刻传感器的量测数据，根据此量测数据和量测的预测值，求得新息；通过新息，根据平滑变结构滤波器求量测增益项；
[0121]
s104，通过状态与量测的协方差和量测预测值的协方差，求得状态与量测增益项对应的系数；
[0122]
s105，根据状态的预测值，量测增益项和状态与量测增益项对应的系数得到最终的状态估计结果，并输出状态；
[0123]
s106，输出状态后，返回s102，进行迭代，估计后续时刻的状态。
[0124]
本发明实施例提供的目标跟踪方法原理图如图2所示。
[0125]
如图3所示，本发明实施例提供的目标跟踪系统包括：
[0126]
系统初始化模块1，用于根据跟踪目标系统，建立离散线性系统或非线性系统，为系统选择合适的目标跟踪运动模型，并对系统进行初始化；
[0127]
预测值计算模块2，用于通过状态方程对目标下一时刻状态进行预测，得到下一时刻状态的预测值，并根据量测方程和状态的预测值求出量测的预测值；
[0128]
量测新息求取模块3，用于输入下一时刻传感器的量测数据，根据此量测数据和量测的预测值，求得新息；
[0129]
量测增益项求取模块4，用于通过新息，根据平滑变结构滤波器求量测增益项；
[0130]
系数计算模块5，用于通过状态与量测的协方差和量测预测值的协方差，求得状态与量测增益项对应的系数；
[0131]
状态估计结果获取模块6，用于根据状态的预测值，量测增益项和状态与量测增益项对应的系数得到最终的状态估计结果，并输出状态；
[0132]
迭代计算模块7，用于在输出状态后，返回预测值计算模块，进行迭代，计算后续时刻的状态估计。下面结合具体实施例对本发明的技术方案作进一步描述。
[0133]
实施例1
[0134]
本发明目的提出新的方法估计没有量测值的状态，同时突破传统方法对量测方程要求必须在线性系统下的限制，扩展该类方法的应用范围，能够使其应用于线性与非线性，高斯与非高斯系统中。
[0135]
本发明提供的基于平滑变结构器滤波器的目标跟踪方法，所述方法包括以下步骤：
[0136]
s1，根据跟踪目标系统，建立离散线性系统或非线性系统，为系统选择合适的目标跟踪运动模型，并对系统进行初始化；
[0137]
s2，通过状态方程对目标下一时刻状态进行预测，得到下一时刻状态的预测值，并根据量测方程和状态的预测值求出量测的预测值；
[0138]
s3，输入下一时刻传感器的量测数据，根据此量测数据和量测的预测值，求得新息；
[0139]
s4，通过新息，根据平滑变结构滤波器求量测增益项；
[0140]
s5，通过状态与量测的协方差和量测预测值的协方差，求得状态与量测增益项对应的系数；
[0141]
s6，根据状态的预测值，量测增益项和状态与量测增益项对应的系数得到最终的状态估计结果。输出状态，其包含目标的位置，速度，加速度等等；
[0142]
返回s2，进行迭代，计算后续时刻的状态估计。
[0143]
本发明可以具体化为以下步骤：
[0144]
s1根据跟踪目标系统，建立离散系统方程，根据目标跟踪运动模型建立合适状态方程(1)和量测方程(2)，并对系统进行初始化。
[0145]
所建立的系统可表示为：
[0146]
x
k 1
＝f(xk) wkꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)
[0147]zk 1
＝h(x
k 1
) υ
k 1
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(2)
[0148]
式中各参数定义如下：xk表示k时刻的状态，f表示从状态从k时刻转移到下一k 1时刻的关系，wk表示过程噪声，h表示状态与量测之间的对应关系，z
k 1
表示k 1时刻的量测，υ
k 1
表示量测噪声；
[0149]
s2：通过状态方程和目标当前时刻的估计值对下一时刻进行预测，得到下一时刻
状态的预测值，并根据量测方程和状态的预测值求出量测的预测值。
[0150]
所述下一时刻状态的预测值和量测的预测值可分别用以下公式(3)和(4)计算得到：
[0151][0152][0153]
s3：输入下一时刻传感器的量测数据，根据此量测数据和量测的预测值，求得新息。所述量测的新息e
k 1|k
可由公式(5)计算得到：
[0154][0155]
s4：通过新息，根据平滑变结构滤波器基本理论求得量测对应的增益项。量测对应的增益项可由公式(6)计算得到：
[0156][0157]
式中各参数定义如下：|e|表示求e的绝对值，γ表示收敛率，一般其取值为0≤γ＜1，表示矩阵的对应元素相乘；sat表示饱和函数，可由公式(7)表示：
[0158][0159]
ψ表示平滑有界层宽度；
[0160]
s5：通过状态与量测的协方差和量测预测值的协方差，求得各个状态与量测增益项对应的系数。需要先通过系统方程分别求得状态与量测的协方差p
xz
，和量测预测值的协方差p
zz
，然后求得状态与量测增益项对应的系数c
k 1
。
[0161]
所述s6中最终的得到的状态估计值可由公式(8)计算得到：
[0162][0163]
返回s2，进行迭代，估计后续时刻的状态，直至结束。
[0164]
实施例2
[0165]
当目标跟踪系统是线性系统时，所建立的线性系统方程可表示为：
[0166]
x
k 1
＝fxk wkꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(9)
[0167]zk 1
＝hx
k 1
υ
k 1
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(10)
[0168]
式中各参数定义如下：xk表示k时刻的状态，f表示从状态从k转移到下一时k 1的关系，h表示状态与量测之间的对应关系；通过状态方程对目标当前时刻的状态的估计值值进行预测，得到下一时刻状态的预测值，并根据量测方程和状态的预测值求出量测的预测值。
[0169]
所述状态的预测值和量测的预测值可分别用以下公式计算得到：
[0170][0171][0172]
状态预测值的协方差p
k 1|k
可根据公式(13)计算得到：
[0173]
p
k 1|k
＝fp
k|kft
qkꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(13)
[0174]ft
是矩阵f的转置，p
k|k
表示k时刻的状态的协方差，qk代表过程噪声wk的协方差。
[0175]
量测的预测值的协方差p
zz
可由公式(14)计算得到：
[0176]
p
zz
＝hp
k 1|kht
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(14)
[0177]
状态与量测的协方差p
xz
可由公式(15)计算得到：
[0178]
p
xz
＝p
k 1|kht
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(15)
[0179]
输入下一时刻传感器的量测数据，根据此量测数据和量测的预测值，求得新息。所述量测的新息e
k 1|k
可由公式(16)计算得到：
[0180][0181]
通过新息，根据平滑变结构滤波器求量测对应的增益项。
[0182]
量测对应的增益项g
k 1
，可由公式(17)计算得到：
[0183][0184]
式中各参数定义如下：|e|表示求e的绝对值，γ表示收敛率，一般其取值为0≤γ＜1，表示矩阵的对应元素相乘，sat表示饱和函数，具体为如下公式(18)：
[0185][0186]
ψ表示平滑有界层宽度；
[0187]
通过状态的协方差和量测的预测协方差，求得各个状态与量测增益项对应的系数。需要先通过系统方程求得状态量测的协方差p
xz
，和量测预测值的协方差p
zz
，然后求得状态与量测增益项对应的系数c
k 1
。
[0188]ck 1
＝p
xz
*(p
zz
)-1
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(19)
[0189]
最终的得到的状态估计值可由公式(20)计算得到：
[0190][0191]
计算的协方差p
k 1|k 1
和后验新息e
k 1|k 1
，输出状态，其包含目标的位置，速度，加速度等等，迭代更新下一时刻直至结束。
[0192]
实施例3
[0193]
本发明当目标跟踪系统非线性系统时，所建立的非线性系统的模型可表示为：
[0194]
x
k 1
＝f(xk) wkꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(21)
[0195]zk 1
＝h(x
k 1
) υ
k 1
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(22)
[0196]
对非线性系统进行线性近似处理，如求系统的一阶泰勒截断，求f和h的雅克比矩阵分别如下：
[0197][0198][0199]
状态预测值的协方差p
k 1|k
可根据以下公式(25)计算得到：
[0200]
[0201]
则状态与量测的协方差p
xz
和量测预测值的协方差p
zz
可分别由下面的公式(26)和(27)计算得到：
[0202][0203][0204]
状态与量测对应的c
k 1
可通过下面的公式(28)计算得到：
[0205]ck 1
＝p
xz
*(p
zz
)-1
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(28)
[0206]
量测对应的增益项g
k 1
，可由公式(29)计算得到：
[0207][0208]
最终的得到的状态估计值可由(30)计算得到：
[0209][0210]
计算的协方差p
k 1|k 1
和后验新息e
k 1|k 1
，输出状态，其包含目标的位置，速度，加速度等等，迭代更新下一时刻直至结束。
[0211]
实施例4
[0212]
本发明当目标跟踪系统非线性系统时，所建立的非线性系统可表示为：
[0213]
x
k 1
＝f(xk) wkꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(31)
[0214]zk 1
＝h(x
k 1
) υ
k 1
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(32)
[0215]
对非线性系统进行无迹变换处理，根据状态和p
k|k
求得2n 1个采样点，采样点x
i,k|k
和其相对应的权值wi可由下面公式(33)和(34)求得：
[0216][0217][0218]
式中κ是一个尺度参数，是(n κ)p
k|k
的均方根矩阵的第i行或者第i列，n是状态的维数；采样点x
i,k|k
对应的状态的预测值可由公式(35)计算得到：
[0219][0220]
状态预测估计值和状态预测值的协方差p
k 1|k
可由公式(36)和(37)求得：
[0221][0222]
[0223]
量测的预测值可通过采样点的预测值以及量测的新息e
k 1|k
可由(38)、(39)和(40)求得：
[0224][0225][0226][0227]
则状态与量测的协方差p
xz
和量测预测值的协方差p
zz
可分别由下面公式(41)和(42)计算得到：
[0228][0229][0230]
则状态与量测对应的系数c
k 1
可通过公式(43)计算得到：
[0231]ck 1
＝p
xz
*(p
zz
)-1
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(43)
[0232]
量测对应的增益项g
k 1
，可由公式(44)计算得到：
[0233][0234]
最终的得到的状态估计值可由公式(45)计算得到：
[0235][0236]
计算的协方差p
k 1|k 1
和后验新息e
k 1|k 1
，输出状态，其包含目标的位置，速度，加速度等等，迭代更新下一时刻直至结束。
[0237]
下面结合线在线性系统中使用实施例1的具体的示例1对本发明中基于平滑变结构器滤波器的目标跟踪方法性能进行评估，展示本发明方法的性能优越性。
[0238]
示例一，在对机动目标跟踪时，分别采用卡尔曼滤波(kf)方法、基于平滑变结构滤波器含有低阶luenberger估计器的方法(方法1)、基于平滑变结构滤波器含有伪量测值的方法(方法2)与本发明所提出的方法进行处理，对四种方法的性能进行比较。所有的方法中状态模型都是使用线性的匀速模型f如下(46)所示：
[0239][0240]
上式中t代表采样率，仿真环境为，飞行器初始状态为x0＝[-25000m,30m/s,-10000m,20m/s]
t
；目标先匀速飞行60s，然后目标匀速转弯，以6
°
/s的转弯率飞行30s，接着匀速飞行60s，后按照-3
°
/s的转弯率机动飞行50s，最后匀速飞行100s，直至到达目的地。飞行轨迹如图4所示。
[0241]
雷达提供位置量测，经过坐标系转换后的量测误差服从均值为0标准差为100m的高斯分布；雷达的采样率t为1s。仿真进行了500次蒙特卡罗运算，并给定了不同方法的初始状态协方差矩阵p和系统噪声矩阵q，量测噪声矩阵r，平滑边界层宽度ψ，收敛速度γ等参
数，如表1所示。
[0242]
表1变量定义表
[0243][0244]
图4显示了一次仿真结果的轨迹，包括真实轨迹、量测点和经过不同滤波器处理得到的轨迹。仿真图4表明四种方法都能平滑轨迹，而本发明所提出方法能得到更精确的轨迹。图5～6分别显示了本发明所提出方法和其他方法的在位置和速度上估计的绝对误差结果。表2是估计结果与真实状态在位置或速度上的累积均方根误差armse(500次蒙特卡罗)。
[0245]
表2估计累积均方根误差armse(500次蒙特卡罗)。
[0246][0247]
结合图4～6和表2中的所有实验结果。所提出的方法在精度方面表现出最好的性能(即armse)。值得注意的是，所有的滤波算法建模都只使用匀速模型，而在仿真中，目标飞行轨迹为匀速运动和协调转弯运动。当滤波模型与运动模型相同时，即匀速时，卡尔曼滤波具有较好的性能。然而，当运动从匀速转向协调转向运动时，会出现建模误差，使得卡尔曼滤波器的性能会显著降低，如图4～6所示。由于卡尔曼滤波要求滤波模型与实际运动模型需一致，当模型不匹配时，卡尔曼滤波估计速度和位置的精度会相互影响，同时降低。在与无建模误差的情况相比，本发明所提方法在存在有建模误差的情况下，估计速度的精度有所降低，但位置估计精度的降低并不像卡尔曼滤波中位置误差快速增大。除此之外，从图5和图6可以看出，当机动结束时，例如在90秒和200秒后，卡尔曼滤波器在很长时间内才恢复到良好的滤波状态，而本发明所提出方法的恢复时间更短。如表2所示，卡尔曼滤波器的位置上armse至少是所提出方法的两倍。此外，本发明所提出的方法的速度精度在x轴和y轴上比卡尔曼滤波分别提高了32％和20％。而传统的方法1和方法2的精度低于本发明所述方法。综上所述，本发明所提出的方法的综合性能是上述方法中最好的。
[0248]
在上述实施例中，可以全部或部分地通过软件、硬件、固件或者其任意组合来实现。当使用全部或部分地以计算机程序产品的形式实现，所述计算机程序产品包括一个或多个计算机指令。在计算机上加载或执行所述计算机程序指令时，全部或部分地产生按照本发明实施例所述的流程或功能。所述计算机可以是通用计算机、专用计算机、计算机网络、或者其他可编程装置。所述计算机指令可以存储在计算机可读存储介质中，或者从一个
计算机可读存储介质向另一个计算机可读存储介质传输，例如，所述计算机指令可以从一个网站站点、计算机、服务器或数据中心通过有线(例如同轴电缆、光纤、数字用户线(dsl)或无线(例如红外、无线、微波等)方式向另一个网站站点、计算机、服务器或数据中心进行传输)。所述计算机可读取存储介质可以是计算机能够存取的任何可用介质或者是包含一个或多个可用介质集成的服务器、数据中心等数据存储设备。所述可用介质可以是磁性介质(例如软盘、硬盘、磁带)、光介质(例如dvd)、或者半导体介质(例如固态硬盘solid state disk(ssd))等。
[0249]
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，都应涵盖在本发明的保护范围之内。