基于强跟踪滤波的高阶扩展卡尔曼滤波器设计方法与流程

1.本发明属于非线性动态系统的空间目标跟踪领域，应用于目标状态模型和测量模型均为非线性模型下的状态估计，可用于在雷达目标跟踪过程中，对在地面目标的位置及非线性运动路线进行估计。


背景技术：

2.滤波技术在各个领域受到广泛关注并得到广泛应用，例如弹道导弹防御、多目标攻击、民用飞机和空中交通管制等。对于非线性动态系统，卡尔曼滤波为了提高估计精度，相继出现了扩展卡尔曼滤波(ekf)，无迹卡尔曼滤波(ukf)，强跟踪滤波器(stf)等方法，但是这些方法通常有估计精度低、对非线性系统建模要求难度高等问题，在基于最大相关熵的高阶扩展卡尔曼滤波器设计方法中，采用了扩维的方法对状态模型进行扩维从而提高估计精度，但是在扩维过程中会引入误差，因此，本发明把对状态模型和测量模型进行扩维的方法同强跟踪滤波结合起来，提出了基于强跟踪滤波的高阶扩展卡尔曼滤波器。
3.当状态模型和测量模型具有足够的精度，并且滤波器的初始值选择得当时，上述的扩展卡尔曼滤波可以给出比较准确的状态估计值。然而，通常的情况是，状态及观测模型通常具有模型不确定性，即此模型与其所描述的非线性系统不能完全匹配，造成模型不确定性的主要原因有：
4.1)模型简化。对于比较复杂的系统，若要精确描述其行为，通常需要较高维数的状态变量，甚至无穷维的变量。这对系统状态的重构造成了极大不便。因此，通常人们都要使用模型简化的办法，使用较少的状态变量来描述系统的主要特征，忽略掉实际系统某些较不重要的因素。也就是存在所谓的未建模动态。这些未建模动态在某些特殊条件下有可能被激发起来，造成模型与实际系统之间较大的不匹配。表现在状态转移矩阵和测量矩阵的不准确上。
5.2)噪声统计特性不准确。即所建模型的噪声统计特性与实际过程噪声的统计特性有较大差异。所建模型噪声的统计特性一般过于理想。实际系统在运行过程中，可能会受到强电磁干扰等随机因素的影响，造成实际系统的统计特性发生较大的变动。表现在状态噪声和测量噪声的不准确上
6.3)对实际系统初始状态的统计特性建模不准。表现在状态初始估计值和初始协方差矩阵的取值选择不准确上
7.4)实际系统的参数发生变动。由于实际系统部件老化、损坏等原因，使得系统的参数发生变动(缓变或突变)，造成原模型与实际系统不匹配。也表现在状态转移矩阵和测量矩阵的不准确上。
8.一个很遗憾的事实是，卡尔曼滤波关于模型不确定性的鲁棒性很差，造成卡尔曼滤波会出现状态估计不准，甚至发散等现象。
9.此外，卡尔曼滤波器在系统达到平稳状态时，将丧失对突变状态的跟踪能力。这是卡尔曼滤波类滤波器(包括卡尔曼滤波器在内)的另一大缺陷。造成这种情况的主要原因
是，当系统达平稳状态时，卡尔曼滤波器的增益阵将趋于极小值。这时，若系统状态发生突变，预报残差将随之增大。然而，此时的增益阵仍将保持为极小值，卡尔曼滤波增益矩阵不会随预报误差的增大而相应地增大。


技术实现要素：

10.为了克服上述技术的缺点，本发明的目的在于提供一种将状态模型及测量模型进行扩维的高阶扩展强跟踪滤波器设计方法。
11.本发明根据传感器得到的信号对雷达跟踪目标在地表进行非线性运动的路线进行拟合，得到目标移动的初始非线性运动路线的状态模型函数和测量模型函数，再通过对目标运动的状态及测量非线性函数引入隐变量及相应的权重系数，对其进行扩维展开，从而实现对状态模型和测量模型的线性化表示，再通过本方法提出的模型初始化、滤波器迭代、时间更新和测量更新等步骤对目标真实位置及其运动路线进行最优估计。
12.为了实现上述目的，本发明采用的技术方案是：
13.本发明包括如下步骤：
14.(1)设计状态空间目标跟踪模型，目标在地表做非线性运动的模型如下：
[0015][0016]
x(k 1)∈r
n
为n维系统状态向量，y(k 1)∈r
m
表示m维传感器测量输出向量；k是时间，f(x(k))是目标运动的状态转移函数，h(x(k 1))是传感器测量函数，f(
·
)和h(
·
)都是连续光滑的非线性函数，w(k)和v(k 1)分别为过程噪声和测量噪声，且满足e{w(k)}＝0，e{w(k)w
t
(j)}＝q(k)δ
kj
；e{v(k)}＝0，e{v(k)v
t
(j)}＝r(k)δ
kj
，即w(k)和v(k 1)都是高斯白噪声；q(k)为半正定对称矩阵，r(k)为正定对称矩阵；当k＝j时，δ
kj
＝1；否则，δ
kj
＝0。
[0017]
(2)对非线性运动模型进行线性化表示，包括步骤如下：
[0018]
(2
‑
1)将状态转移函数f(x(k))使用l阶隐变量x
(l)
(k)来表示；
[0019]
(2
‑
2)对(2
‑
1)中的l阶隐变量赋予相对应的权重系数向量
[0020]
(2
‑
3)使用(2
‑
1)和(2
‑
2)中提出的隐变量及权重系数对系统状态向量x(k 1)实现扩维展开，得出与l阶隐变量对应的x
(l)
(k 1)；
[0021]
(2
‑
4)对传感器测量函数h(x(k 1))使用l阶隐变量x
(l)
(k 1)来表示；
[0022]
(2
‑
5)对(2
‑
4)中引入的l阶隐变量赋予相应的权重系数向量
[0023]
(2
‑
6)根据(2
‑
4)和(2
‑
5)中提出的隐变量及权重系数对传感器测量输出向量y(k 1)扩维展开，得出与l阶隐变量对应的y
(l)
(k 1)；
[0024]
(2
‑
7)基于(2
‑
1)和(2
‑
2)中提出的隐变量和权重系数得出线性化表示的状态转移矩阵
[0025]
(2
‑
8)对状态向量中的非线性函数f(x(k))进行线性化表示得出x(k 1)；
[0026]
(2
‑
9)基于(2
‑
4)和(2
‑
5)中提出的隐变量和权重系数得出线性化表示的传感器测量矩阵
[0027]
(2
‑
10)对传感器测量输出向量中的非线性测量函数h(x(k 1))进行线性化表示得出y(k 1)。
[0028]
(3)根据基于强跟踪滤波的高阶扩展卡尔曼滤波器的设计，对地面目标的位置状态函数进行估计，包括步骤如下：
[0029]
(3
‑
1)对系统在k＝1时刻的初始状态矩阵和初始协方差矩阵进行初始化得到x(0)和p0；
[0030]
(3
‑
2)对k 1时刻的状态值和协方差矩阵进行迭代得到和p(k 1|k 1)；
[0031]
(3
‑
3)更新状态预测值和预测误差协方差矩阵得到和p(k 1|k)；
[0032]
(3
‑
4)根据(3
‑
3)更新滤波增益矩阵k(k 1)；
[0033]
(3
‑
5)根据(3
‑
3)和(3
‑
4)进行测量更新，对状态估计值和协方差矩阵进行更新得到更新迭代后新的和p(k 1|k 1)值。
[0034]
至此，基于强跟踪滤波的高阶扩展卡尔曼滤波器设计全部完成。
[0035]
与现有技术相比，本发明具有以下优点：
[0036]
(1)本发明在步骤(3)中的(3
‑
1)到(3
‑
5)中采用了强跟踪滤波的方法对目标进行跟踪，故本发明具有较强的针对模型不确定性的鲁棒性，在干扰噪声相对较大情况下可以有更好的跟踪效果。
[0037]
(2)本发明在步骤(3)中的(3
‑
1)到(3
‑
5)中采用了强跟踪滤波的方法对目标进行跟踪，故本发明具有极强的关于突变状态的跟踪能力，甚至在系统达平稳状态时，仍保持对缓变状态与突变状态的跟踪能力。
[0038]
(3)本发明在步骤(2)中的(2
‑
1)到(2
‑
6)中通过将状态方程和测量方程进行扩维展开，使得本发明的计算复杂性适中，在实现良好跟踪精度情况下节省计算成本。
附图说明
[0039]
图1为实施例1情况下x轴方向上传感器的位置；
[0040]
图2为实施例1情况下y轴方向上传感器的位置；
[0041]
图3为实施例1情况下x轴和y轴方向上位置估计误差；
[0042]
图4为实施例2情况下x轴方向上传感器的位置；
[0043]
图5为实施例2情况下y轴方向上传感器的位置；
[0044]
图6为实施例2情况下x轴和y轴方向上位置估计误差。
具体实施方式
[0045]
下面结合附图1
‑
6和实例详细说明本发明的实施方式。
[0046]
本发明首先将跟踪目标的运动方程使用隐变量和与隐变量对应的权重系数对运动方程进行扩维，从而实现状态模型和测量模型的线性化，并在此基础上对状态模型进行初始化，再通过对运动方程的滤波器迭代过程，时间更新过程和测量更新过程，来实现对目标的位置及其运动轨迹的实时跟踪。
[0047]
本发明是一种应用于空间目标跟踪系统的基于强跟踪滤波的高阶扩展卡尔曼滤
波器设计方法，包括如下步骤：
[0048]
步骤1给定目标在地表做非线性运动模型如下，状态模型和观测模型均为具有强非线性特性复杂动态系统：
[0049][0050]
其中，x(k 1)∈r
n
为n维系统状态向量，y(k 1)∈r
m
表示m维传感器测量输出向量；f(x(k))和h(x(k 1))分别表示状态转移函数和测量函数，w(k)和v(k 1)分别为过程噪声和测量噪声，且满足e{w(k)}＝0，e{w(k)w
t
(j)}＝q(k)δ
kj
；e{v(k)}＝0，e{v(k)v
t
(j)}＝r(k)δ
kj
；q(k)为半正定对称矩阵，r(k)为正定对称矩阵；当k＝j时，δ
kj
＝1；否则，δ
kj
＝0。
[0051]
步骤2对系统非线性模型引入隐变量及与其对应的权重系数进行扩维来完成对模型的线性化。
[0052]
(2
‑
1)为便于描述和理解，令m＝n＝2，则状态转移函数f
i
(x(k))有如下高阶多项式形式
[0053][0054]
其中，为所有l阶张量之和；表示各阶张量对应的权重。其中为l阶隐变量集合。
[0055]
(2
‑
2)在(2
‑
1)中为l阶隐变量对应的权重向量。
[0056]
(2
‑
3)根据(2
‑
1)和(2
‑
2)中引入的隐变量及权重系数对非线性状态转移函数x(k 1)扩维展开，得出与l阶隐变量对应的x
(l)
(k 1)
[0057][0058]
令：
[0059]
则式(3)的矩阵形式为
[0060][0061]
(2
‑
4)同样，假设测量函数h
i
(x(k 1))有如下高阶多项式形式
[0062][0063]
其中，为所有l阶张量之和；表示各阶张量对应的权重。其中为l阶隐变量集合
[0064]
(2
‑
5)在(2
‑
4)中为l阶隐变量对应的权重向量。
[0065]
(2
‑
6)根据(2
‑
4)和(2
‑
5)中引入的隐变量及权重系数对函数y(k 1)扩维展开，得出与l阶隐变量对应的y
(l)
(k 1)如下：
[0066][0067]
(2
‑
7)基于(2
‑
2)引入线性化表示的状态转移矩阵为了将前文建立的伪线性模型转化成真正的线性形式，需要建立l阶隐变量x
(l)
(k 1)和u阶隐变量之间的动态关系。
[0068][0069]
其中，可根据原始状态的输入信息进行辨识。在没有任何先验信息的情况下，对其设置如下
[0070][0071]
(2
‑
8)对非线性状态方程进行线性化表示得出x(k 1)，结合(2
‑
1)和(2
‑
2)、式(3)和式(4)，状态模型式(1)有如下线性矩阵形式
[0072][0073]
令x(k)＝[(x
(1)
(k))
t
，(x
(2)
(k))
t
，
…
，(x
(l)
(k))
t
，
…
，(x
(r)
(k))
t
]
t
[0074][0075][0076]
则式(2.10)有如下线性化形式
[0077]
x(k 1)＝a(k 1，k)x(k) w(k)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(10)
[0078]
其中，w(k)为建模误差。
[0079]
(2
‑
9)基于(2
‑
5)引入线性化表示的传感器测量矩阵
[0080][0081]
(2
‑
10)对非线性测量方程进行线性化表示得出y(k 1)
[0082]
[0083]
进一步，测量模型的线性化形式如下所示
[0084]
x(k 1)＝a(k 1，k)x(k) t
f
(k) w(k)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(13)
[0085]
y(k 1)＝h(k 1)x(k 1) t
h
(k 1) v(k 1)
ꢀꢀꢀꢀꢀꢀꢀ
(14)
[0086]
其中，v(k 1)为建模误差。
[0087]
(3)假设已知如式(12)和(13)所示线性扩维状态模型和测量模型，且e{w(k)w
t
(k)}＝q
w
(k)，e{v(k)v
t
(k)}＝r
v
(k)，则基于强跟踪滤波的高阶扩展卡尔曼滤波器设计如下：
[0088]
(3
‑
1)初始化原始系统的状态矩阵和协方差矩阵初始值得到x(0)和p0：
[0089][0090][0091]
则x(0)有如下特性
[0092]
x(0)＝[(x
(1)
(0))
t
，(x
(2)
(0))
t
，
…
，(x
(r)
(0))
t
]
t
ꢀꢀꢀꢀꢀꢀꢀꢀ
(17)
[0093][0094]
其中，
[0095][0096]
其中，为半正定矩阵。
[0097]
(3
‑
2)对k 1时刻的状态值和协方差矩阵进行迭代得到和p(k 1|k 1)：
[0098]
假设已知y(1)，y(2)，
…
，y(k)，及p(k|k)，则
[0099][0100]
对应的协方差矩阵
[0101][0102]
(3
‑
3)更新状态预测值和预测误差协方差矩阵得到和p(k 1|k)：
[0103][0104]
p(k 1|k)＝a(k)p(k|k)a
t
(k) λ(k 1)q
w
(k)
ꢀꢀꢀꢀꢀꢀꢀ
(23)
[0105]
[0106]
矩阵范数或直接矩阵内所有元素求和，在λ的取值范围内，利用遍历的方法，找到λ和θ。
[0107]
其中
[0108][0109][0110]
n(k 1)＝v0(k 1)
‑
h(k 1)q(k)h
t
(k 1)
‑
β
·
r(k 1)
ꢀꢀꢀꢀꢀꢀ
(27)
[0111]
m(k 1)＝h(k 1)a(k 1，k)p(k|k)a
t
(k 1，k)h
t
(k 1)
ꢀꢀꢀꢀꢀ
(28)
[0112][0113]
β≥1为一个选定的弱化因子。引入此弱化因子的目的是使得状态估计值更加平滑。
[0114]
(3
‑
4)根据(3
‑
3)更新滤波增益矩阵k(k 1)：
[0115]
k(k 1)＝p(k 1|k)h
t
(k 1)[h(k 1)p(k 1|k)h
t
(k 1) r
v
(k 1)]
‑1ꢀꢀꢀ
(30)
[0116]
(3
‑
5)根据(3
‑
3)和(3
‑
4)进行测量更新，得到更新迭代后最优状态估计值和p(k 1|k 1)值：
[0117][0118]
p(k 1|k 1)＝[i
‑
k(k 1)h(k 1)]p(k 1|k)
ꢀꢀꢀꢀꢀꢀ
(32)
[0119]
至此，基于强跟踪滤波的高阶扩展卡尔曼滤波器设计全部完成。
[0120]
为了进一步验证本发明的有效性，采用两个实施例来进行仿真实验，其中两个状态变量x1、x2分别是雷达跟踪目标在地表二维平面的x轴和y轴分别进行的位置变化方程，实施例1为状态模型是强非线性方程，测量模型是线性方程；实施例2为状态模型和测量方程均为强非线性方程。与实施例1相比，实施例2的测量方程为强非线性方程。
[0121]
实施例1系统模型为如下的雷达跟踪目标系统，其中状态模型为非线性模型，测量模型为线性模型。
[0122][0123]
其中，模型噪声有如下特性，w(k)～n(0，0.01)，v(k 1)～n(0，0.01)；x0＝[0 0]
t
，p0＝i2×2。
[0124]
分别用强跟踪滤波器(stf)和本发明提出的基于强跟踪滤波的高阶扩展卡尔曼滤波器(h
‑
stf)对实施例一中的模型进行仿真估计，比较雷达跟踪目标在x轴和y轴的位移跟踪效果的估计值和估计误差，并对两种滤波器的估计误差进行比较，比较结果如表1所示。
[0125]
实施例2系统模型为如下的雷达跟踪目标系统，其中状态模型为非线性模型，测量模型也为非线性模型。
[0126][0127]
其中，模型噪声有如下特性，w1(k)～n(0，0.01)，w2(k)～n(0，0.01)，v(k 1)～n(0，0.01)；x0＝[0 0]
t
，p0＝i2×2。
[0128]
分别用强跟踪滤波器(stf)和本发明提出的基于强跟踪滤波的高阶扩展卡尔曼滤波器(h
‑
stf)对实施例二中模型进行仿真估计，比较雷达跟踪目标在x轴和y轴的位移跟踪效果的估计值和估计误差，并对两种滤波器的估计误差进行比较，比较结果如表2所示。
[0129]
仿真结果分析
[0130]
图1到图6分别给出了在实施例1和实施例2下分别使用stf和本发明提出的h
‑
stf两个滤波器方法对雷达跟踪目标在地表平面x轴和y轴上的位移估计图，以及stf和h
‑
stf的估计误差对比图。表1和表2分别是实施例1模型和实施例2模型下stf和h
‑
stf的估计误差比较及提升
[0131]
表1 实施例1模型下的stf和h
‑
stf的估计误差比较
[0132]
滤波器x1的估计误差x2的估计误差总估计误差stf0.12660.11660.1216h
‑
stf0.10150.10520.1033提升百分比19.83％9.78％15.05％
[0133]
表2 实施例2模型下的stf和h
‑
stf的估计误差比较
[0134]
滤波器x1的估计误差x2的估计误差总估计误差stf0.11320.10160.1074h
‑
stf0.09250.09370.0931提升百分比18.29％7.78％13.31％
[0135]
通过实验数据和仿真结果，可以看到，无论是实施例1中的状态模型为非线性方程，测量模型线性为线性方程，还是实施例2中的状态模型和测量模型均为非线性的情况下，本发明所提出的基于强跟踪滤波的高阶扩展卡尔曼滤波器(h
‑
stf)在对雷达目标跟踪下的估计误差均优于强跟踪滤波器(stf)，且都有较大提升，这是由于本发明采用的使用隐变量扩维方法对系统方程进行扩维可以加强系统的鲁棒性和估计精度，可以在雷达目标跟踪时达到更好的跟踪效果。因此，以上数据分析表明本发明方法的有效性。