一种调整网格间距的改进AGIMM跟踪方法与流程

本发明属于机动目标跟踪领域，特别涉及一种调整网格间距的改进AGIMM跟踪方法，即一种调整网格间距的改进自适应网格交互多模型跟踪方法，适用于通过机动判别调整网格间距以及搜索相对最佳的角速度自适应跟踪高速大机动目标，以取得很好地跟踪滤波效果。

背景技术：

一直以来，目标跟踪在军事上都是重点研究领域，而对高速大机动目标跟踪则是重点研究中的难点；高速大机动目标因其速度大、机动迅速且难以预测的特点使得跟踪滤波建模有很大的难度。

针对目标的机动性，学者们提出了一些机动模型，在一定程度上提高了机动目标的跟踪精度，但仍然无法很好的适应强机动目标的跟踪；而交互多模型的提出则解决了单模型跟踪适应性差的问题，具有很好的鲁棒特性；研究人员也相继在交互多模型的基础上提出了一系列的改进算法，主要改进方向是模型集的选择、模型概率的计算以及跟踪滤波算法的改进等。

目前根据交互多模型算法结合图论思想的自适应网格交互多模型(AGIMM)算法是最有效的跟踪算法之一，AGIMM算法主要的特点是将有不同转弯速率的转弯模型作为跟踪的模型集，借鉴图论的思想自适应更新转弯速率使模型集一直处于动态更新中，相对于模型固定的交互多模型算法，具有更好的自适应能力；但是同样地，AGIMM算法存在着模型网格间距不能自适应调整以及角速度估计速度慢的问题。

技术实现要素：

针对上述现有技术存在的问题，本发明的目的在于提出一种调整网格间距的改进AGIMM方法，该种调整网格间距的改进AGIMM方法是一种根据残差信息进行机动判别进而自适应调整网格间距的改进AGIMM方法，并且在非机动时刻利用一维搜索的思想搜索最佳角速度以适应真实角速度，不仅能够改善对机动时刻的自适应能力，而且也能够提高对非机动时刻的跟踪精度和对角速度的估计速度。

为达到上述技术目的，本发明采用如下技术方案予以实现。

一种调整网格间距的改进AGIMM方法，包括以下步骤：

步骤1，确定雷达，所述雷达扫描范围内存在目标，雷达对目标进行扫描探测；建立雷达直角坐标系和雷达极坐标系，确定在雷达直角坐标系下雷达对目标的量测值，分别得到雷达对目标的初始状态向量以及雷达对目标的初始误差协方差矩阵P0；分别确定交互模型集及交互模型集初始值，交互模型集包括L个机动转弯子模型

步骤2，初始化：令k′表示k'采样时刻，k′＝3～N，k'的初始值为3；根据雷达对目标的初始状态向量以及雷达对目标的初始误差协方差矩阵P0确定交互模型集的卡尔曼滤波初始值；

步骤3，根据k'-1采样时刻交互模型集的卡尔曼滤波值，计算k'采样时刻第j个机动转弯子模型的新息值Vj(k')和k'采样时刻第j个机动转弯子模型的新息协方差矩阵Sj(k')，以及k'采样时刻第j个机动转弯子模型的状态向量滤波输出值和k'采样时刻第j个机动转弯子模型的误差协方差滤波输出值Pj(k'|k')；j＝1,2,…,L；

步骤4，根据k'采样时刻第j个机动转弯子模型的新息值Vj(k')、k'采样时刻第j个机动转弯子模型的新息协方差矩阵Sj(k')，计算得到k'采样时刻交互模型集概率pk'；

步骤5，根据k'采样时刻交互模型集概率pk'，分别计算得到k'采样时刻第1个机动转弯子模型的转弯角速度ω1(k')、k'采样时刻第2个机动转弯子模型的转弯角速度ω2(k')和k'采样时刻第3个机动转弯子模型的转弯角速度ω3(k')；

步骤6，令k'加1，重复执行步骤3至步骤5，直到得到3采样时刻第j个机动转弯子模型的状态向量滤波输出值至N采样时刻第j个机动转弯子模型的状态向量滤波输出值和3采样时刻第j个机动转弯子模型的误差协方差滤波输出值Pj(3|3)至N采样时刻第j个机动转弯子模型的误差协方差滤波输出值Pj(N|N)，并记为一种调整网格间距的改进AGIMM跟踪结果。

本发明与现有技术相比主要优点体现在两方面：

(1)解决了模型网格间距不能自适应调整的缺点，能让模型根据目标机动大小实时地在线调整网格间距，增强了对目标机动变化的适应性，提高了对机动时刻的跟踪性能。

(2)在非机动时刻，利用一维搜索的思想使模型集向最佳角速度的方向收敛，加快了对角速度的估计，提高了非机动时刻的跟踪精度。

附图说明

下面结合附图说明和具体实施方式对本发明作进一步详细说明。

图1是本发明的一种调整网格间距的改进AGIMM方法流程图；

图2是建立的雷达直角坐标系和雷达极坐标系示意图；

图3是目标为转弯机动，设置量测噪声1时，使用AGIMM算法和本发明方法的距离RMSE对比图；

图4是目标为转弯机动，设置量测噪声1时，使用AGIMM算法和本发明方法的速度RMSE对比图；

图5a是目标为转弯机动，设置量测噪声1时，使用AGIMM算法对应的角速度变化图；

图5b是目标为转弯机动，设置量测噪声1时，使用本发明方法对应的角速度变化图；

图6是目标为转弯机动，设置量测噪声2时，使用AGIMM算法和本发明方法的距离RMSE对比图；

图7是目标为转弯机动，设置量测噪声2时，使用AGIMM算法和本发明方法的速度RMSE对比图；

图8是目标为临近空间飞行器，设置初始模型集1时，使用AGIMM算法和本发明方法的距离RMSE对比图；

图9是目标为临近空间飞行器，设置初始模型集1时，使用AGIMM算法和本发明方法的速度RMSE对比图；

图10是目标为临近空间飞行器，设置初始模型集1和初始模型集2时，使用AGIMM算法的距离RMSE对比图；

图11是目标为临近空间飞行器，设置初始模型集1和初始模型集2时，使用本发明方法的距离RMSE对比图；

图12a是目标为临近空间飞行器，设置初始模型集2时使用AGIMM算法的角速度变化图；

图12b是目标为临近空间飞行器，设置初始模型集2时使用本发明方法的角速度变化图。

具体实施方式

参照图1，本发明的一种调整网格间距的改进AGIMM方法流程图；其中所述调整网格间距的改进AGIMM方法，包括以下步骤：

步骤1，确定雷达，所述雷达扫描范围内存在目标，雷达对目标进行扫描探测；建立雷达直角坐标系和雷达极坐标系，确定在雷达直角坐标系下雷达对目标的量测值，分别得到雷达对目标的初始状态向量以及雷达对目标的初始误差协方差矩阵P0；分别确定交互模型集及交互模型集初始值。

步骤1的子步骤为：

(1a)确定雷达，所述雷达扫描范围内存在目标，雷达对目标进行扫描探测；将目标所在轨迹中的任意一点记为点目标P，然后以雷达所在中心位置为原点o、以雷达所指正东方向为x轴、以雷达所指正北方向为y轴、根据右手定则确定的z轴建立雷达直角坐标系，其中确定z轴过程为：设定右手的拇指和食指分别指向x轴和y轴，则中指指向为z轴。

以雷达所在中心位置为原点o、以点目标P与雷达之间的径向距离为ρ、以点目标P相对于雷达的方位角为θ、以点目标P相对于雷达的俯仰角ε建立雷达极坐标系，其中雷达直角坐标系和雷达极坐标系示意图如图2所示。

(1b)雷达扫描范围内存在目标，雷达对目标进行扫描探测，通过雷达传感器可以获得目标在雷达极坐标系(ρ-θ-ε)下的量测信息，ρ表示点目标P与雷达之间的径向距离，θ表示点目标P相对于雷达的方位角，ε表示点目标P相对于雷达的俯仰角。

然后将雷达极坐标系下的量测值转换为雷达直角坐标系(x-y-z)下的量测值，假设雷达扫描周期为T'，雷达对目标总的探测时长为t，则相当于雷达对目标轨迹进行了周期为T'的N次采样，N＝t/T'。

雷达使用自身携带的传感器获取目标在雷达极坐标系下的量测值，具体包括k采样时刻在雷达极坐标系下雷达获取的目标径向距离量测值ρ(k)、k采样时刻在雷达极坐标系下雷达获取的目标方位角量测值θ(k)和k采样时刻在雷达极坐标系下雷达获取的目标俯仰角量测值ε(k)；并将获取的目标在雷达极坐标系下的量测值转化为k采样时刻在雷达直角坐标系下雷达对目标的量测值：

Zx(k)＝ρ(k)cosε(k)cosθ(k),Zy(k)＝ρ(k)cosε(k)sinθ(k),Zz(k)＝ρ(k)sinε(k)

其中，k＝1,2,…,N，Zx(k)表示k采样时刻在雷达直角坐标系下雷达对目标x方向的量测值，Zy(k)表示k采样时刻在雷达直角坐标系下雷达对目标y方向的量测值，Zz(k)表示k采样时刻在雷达直角坐标系下雷达对目标z方向的量测值，ρ(k)表示k采样时刻在雷达极坐标系下雷达获取的目标径向距离量测值，θ(k)表示k采样时刻在雷达极坐标系下雷达获取的目标方位角量测值，ε(k)表示k采样时刻在雷达极坐标系下雷达获取的目标俯仰角量测值，cos表示余弦函数，sin表示正弦函数。

(1c)利用1采样时刻和2采样时刻在雷达直角坐标系下雷达对目标的位置量测值，计算雷达对目标的初始状态向量

其中，Zx(2)表示2采样时刻在雷达直角坐标系下雷达对目标x方向的量测值，即2采样时刻目标在雷达直角坐标系下x方向的距离；Zx(1)表示1采样时刻在雷达直角坐标系下雷达对目标x方向的量测值；Zy(2)表示2采样时刻在雷达直角坐标系下雷达对目标y方向的量测值，即2采样时刻目标在雷达直角坐标系下y方向的距离；Zy(1)表示1采样时刻在雷达直角坐标系下雷达对目标y方向的量测值；Zz(2)表示2采样时刻在雷达直角坐标系下雷达对目标z方向的量测值，即2采样时刻目标在雷达直角坐标系下z方向的距离；Zz(1)表示1采样时刻在雷达直角坐标系下雷达对目标z方向的量测值，T'表示雷达扫描周期，上标T表示转置。

(Zx(2)-Zx(1))/T'对应2采样时刻目标在雷达直角坐标系下x方向的速度，(Zy(2)-Zy(1))/T'对应2采样时刻目标在雷达直角坐标系下y方向的速度，

(Zz(2)-Zz(1))/T'对应2采样时刻目标在雷达直角坐标系下z方向的速度。

(1d)根据2采样时刻在雷达极坐标系下雷达获取的目标径向距离量测值ρ(2)、2采样时刻在雷达极坐标系下雷达获取的目标方位角量测值θ(2)和2采样时刻在雷达极坐标系下雷达获取的俯仰角量测值ε(2)，计算2采样时刻在雷达直角坐标系下雷达对目标的量测噪声协方差矩阵R(2)，其表达式为：

其中，r11(2)表示2采样时刻在雷达直角坐标系下雷达对目标的量测噪声协方差矩阵R(2)中第1行、第1列元素，r12(2)表示2采样时刻在雷达直角坐标系下雷达对目标的量测噪声协方差矩阵R(2)中第1行、第2列元素，r13(2)表示2采样时刻在雷达直角坐标系下雷达对目标的量测噪声协方差矩阵R(2)中第1行、第3列元素，r22(2)表示2采样时刻在雷达直角坐标系下雷达对目标的量测噪声协方差矩阵R(2)中第2行、第2列元素，r23(2)表示2采样时刻在雷达直角坐标系下雷达对目标的量测噪声协方差矩阵R(2)中第2行、第3列元素，r33(2)表示2采样时刻在雷达直角坐标系下雷达对目标的量测噪声协方差矩阵R(2)中第3行、第3列元素，A(2)表示2采样时刻雷达极坐标系到雷达直角坐标系的噪声转换矩阵，表示点目标P与雷达之间的径向距离ρ的量测噪声方差值，表示点目标P相对于雷达的方位角θ的量测噪声方差值，表示点目标P相对于雷达的俯仰角ε的量测噪声方差值，点目标P为雷达扫描范围内目标所在轨迹中的任意一点。

(1e)根据2采样时刻在雷达直角坐标系下雷达对目标的量测噪声协方差矩阵R(2)，计算雷达对目标的初始误差协方差矩阵P0，其表达式为：

其中

其中，rij(2)表示2采样时刻在雷达直角坐标系下雷达对目标的量测噪声协方差矩阵R(2)中第i行、第j列元素，Pij表示初始误差协方差矩阵P0中第i行、第j列元素，i＝1,2,3，j＝1,2,3。

(1f)设置L个转弯角速度不同的机动转弯子模型，其中机动转弯子模型为自适应网格交互多模型(AGIMM)算法中提到的协同转弯模型，AGIMM算法主要是将具有不同转弯速率的协同转弯模型作为跟踪模型集，借鉴图论的思想自适应更新协同转弯模型的转弯速率，进而使得跟踪模型集一直处于动态更新中；每个机动转弯子模型分别对应1个卡尔曼Kalman滤波器。

将L个机动转弯子模型作为交互模型集，L＝3，并确定交互模型集初始值，所述交互模型集初始值为2采样时刻第1个机动转弯子模型的转弯角速度ω1(2)、2采样时刻第2个机动转弯子模型的转弯角速度ω2(2)和2采样时刻第3个机动转弯子模型的转弯角速度ω3(2)，ω1(2)＝-ωmax,ω2(2)＝0,ω3(2)＝ωmax，ωmax表示预设的转弯角速度跳跃变化的最大值，ωmax的取值范围在-180°到180°之间，具体的ωmax的取值可以根据实际跟踪的目标运动特性做经验性的设定。

步骤2，根据转弯角速度确定L个机动转弯子模型的状态转移矩阵，设置过程噪声矩阵；确定交互模型集的初始概率以及L个机动转弯子模型之间的马尔科夫状态转移概率。

步骤2的子步骤为：

(2a)设定k采样时刻交互模型集对应的转弯角速度为Mk，

Mk＝{ω1(k),ω2(k),ω3(k)}，ω1(k)表示k采样时刻第1个机动转弯子模型的转弯角速度，ω2(k)表示k采样时刻第2个机动转弯子模型的转弯角速度，ω3(k)表示k采样时刻第3个机动转弯子模型的转弯角速度。

设定k采样时刻交互模型集对应概率为pk，pk＝{μ1(k),μ2(k),μ3(k)}，μ1(k)表示k采样时刻第1个机动转弯子模型概率，μ2(k)表示k采样时刻第2个机动转弯子模型概率，μ3(k)表示k采样时刻第3个机动转弯子模型概率。

(2b)，确定第i个机动转弯子模型到第j个机动转弯子模型的马尔科夫模型转移概率pij，其表达式为：

其中，i＝1,2,3，j＝1,2,3。

(2c)，交互模型集包括3个机动转弯子模型，计算k采样时刻第j个机动转弯子模型的状态转移矩阵Fj(k)。

具体地，每个机动转弯子模型的状态转移矩阵在雷达直角坐标系中可以分解为水平方向和垂直方向两部分：水平方向上看作匀速转弯运动，与x轴、y轴方向的变化都有关；垂直方向看作匀速运动，只与z轴方向变化有关，则得到t时刻第j个机动转弯子模型在雷达直角坐标系下的连续状态方程如下：

其中，x(t)表示t时刻目标在x轴方向的距离，y(t)表示t时刻目标在y轴方向的距离，z(t)表示t时刻目标在z轴方向的距离；表示t时刻目标在x轴方向的速度，表示t时刻目标在y轴方向的速度，表示t时刻目标在z轴方向的速度；表示t时刻目标在x轴方向的加速度，表示t时刻目标在y轴方向的加速度，表示t时刻目标在z轴方向的加速度；ωj(t)表示t时刻第j个机动转弯子模型的转弯角速度，表示t时刻第j个机动转弯子模型水平方向的状态转移矩阵，表示t时刻第j个机动转弯子模型垂直方向的状态转移矩阵，t表示时间变量。

对所述水平方向的状态转移矩阵和所述垂直方向的状态转移矩阵分别进行采样周期为T′的离散化处理，分别得到k采样时刻第j个机动转弯子模型在水平方向上的状态转移矩阵和k采样时刻第j个机动转弯子模型在垂直方向上的状态转移矩阵其表达式为：

然后计算得到k采样时刻第j个机动转弯子模型的状态转移矩阵Fj(k)，其表达式为：

其中，j＝1,2,3，02×4表示2×4维全0矩阵，04×2表示4×2维全0矩阵，ωj(k)表示k采样时刻第j个机动转弯子模型的转弯角速度，T'表示雷达扫描周期，

(2d)，计算k采样时刻第j个机动转弯子模型的过程噪声协方差矩阵Qj(k)，其表达式为：

其中，

其中，j＝1,2,3，qj表示第j个机动转弯子模型的过程噪声方差，过程噪声方差qj根据量测噪声以及机动转弯子模型属性设置，一般取在σρ到之间，σρ表示点目标P与雷达之间的径向距离ρ的量测噪声标准差，表示点目标P与雷达之间的径向距离ρ的量测噪声方差值，点目标P为雷达扫描范围内目标所在轨迹中的任意一点；过程噪声方差qj设置越大表示模型在其他参数不变的情况下越能适应大机动或大噪声背景下的目标；反之，过程噪声方差qj设置越小，则表示模型在其他参数不变的情况下对弱机动或低噪声背景下的目标有更好的跟踪精度；表示k采样时刻第j个机动转弯子模型在水平方向上的过程噪声协方差矩阵，表示k采样时刻第j个机动转弯子模型在垂直方向上的过程噪声协方差矩阵。

初始化：令k′表示k'采样时刻，k′＝3～N，k'的初始值为3；根据雷达对目标的初始状态向量以及雷达对目标的初始误差协方差矩阵P0确定交互模型集的卡尔曼滤波初始值。

步骤3，利用模型集概率和模型转移概率对L个机动转弯子模型k-1采样时刻的状态向量以及误差协方差矩阵进行加权交互计算作为L个机动转弯子模型k采样时刻的滤波输入，然后利用机动目标在直角坐标系下的状态转移框架和量测框架对各个机动转弯子模型分别进行卡尔曼Kalman滤波；因为卡尔曼滤波器的初始采样时刻是2采样时刻，所以卡尔曼滤波从3采样时刻开始；设定参与卡尔曼滤波器的采样时刻为k′，k′＝3～N，k'的初始值为3。

步骤3的子步骤为：

(3a)确定k'-1采样时刻交互模型集的卡尔曼滤波值，当k'为3时所述k'-1采样时刻交互模型集的卡尔曼滤波值为交互模型集的卡尔曼滤波初始值，所述交互模型集的卡尔曼滤波初始值包括2采样时刻第1个机动转弯子模型的状态滤波输出值2采样时刻第2个机动转弯子模型的状态滤波输出值2采样时刻第3个机动转弯子模型的状态滤波输出值2采样时刻第1个机动转弯子模型的误差协方差滤波输出值P1(2|2)、2采样时刻第2个机动转弯子模型的误差协方差滤波输出值P2(2|2)、2采样时刻第3个机动转弯子模型的误差协方差滤波输出值P3(2|2)，其表达式分别为：

P1(2|2)＝P0，P2(2|2)＝P0，P3(2|2)＝P0

其中，表示雷达对目标的初始状态向量，P0表示雷达对目标的初始误差协方差矩阵。

(3b)计算k'采样时刻第j个机动转弯子模型的交互状态向量输入值其表达式为：

其中，μi|j(k'-1|k'-1)表示k'-1采样时刻第i个机动转弯子模型到第j个机动转弯子模型的一步状态转移概率，表示k'-1采样时刻第i个机动转弯子模型的状态滤波输出值，μi(k'-1)表示k'-1采样时刻第i个机动转弯子模型概率，pij表示第i个机动转弯子模型到第j个机动转弯子模型的马尔科夫模型转移概率。

(3c)计算k'采样时刻第j个机动转弯子模型的交互误差协方差矩阵输入值Poj(k'-1|k'-1)，其表达式为：

其中，μi|j(k'-1|k'-1)表示k'-1采样时刻第i个机动转弯子模型到第j个机动转弯子模型的一步状态转移概率，pij表示第i个机动转弯子模型到第j个机动转弯子模型的马尔科夫模型转移概率，Pj(k'-1|k'-1)表示k'-1采样时刻第j个机动转弯子模型的误差协方差滤波输出值，表示k'-1采样时刻第j个机动转弯子模型的状态滤波输出值与k'采样时刻第j个机动转弯子模型的交互状态向量输入值之间的差值，表示k'-1采样时刻第j个机动转弯子模型的状态滤波输出值，表示k'采样时刻第j个机动转弯子模型的交互状态向量输入值。

(3d)将k'采样时刻第j个机动转弯子模型的交互状态向量输入值和k'采样时刻第j个机动转弯子模型的交互误差协方差矩阵输入值Poj(k'-1|k'-1)作为k采样时刻第j个机动转弯子模型的卡尔曼滤波器输入值，然后进行卡尔曼滤波。

分别计算k'-1采样时刻到k'采样时刻第j个机动转弯子模型的状态向量的预测值和k-1采样时刻到k'采样时刻第j个机动转弯子模型的误差协方差矩阵预测值Pj(k'|k'-1)，其表达式分别为：

Pj(k'|k'-1)＝Fj(k')Poj(k'-1|k'-1)[Fj(k')]^T+Qj(k')

其中，Qj(k')表示k'采样时刻第j个机动转弯子模型的过程噪声协方差矩阵，Fj(k')表示k'采样时刻第j个机动转弯子模型的状态转移矩阵。

然后分别计算k'采样时刻第j个机动转弯子模型的新息值Vj(k')、k'采样时刻第j个机动转弯子模型的新息协方差矩阵Sj(k')、k'采样时刻第j个机动转弯子模型的增益矩阵Kj(k')，其表达式分别为：

Sj(k')＝Hj(k')Pj(k'|k'-1)[Hj(k')]^T+Rj(k')

其中，Zj(k')表示k'采样时刻第j个机动转弯子模型的量测值，

Zj(k')＝[Zx(k'),Zy(k'),Zz(k')]^T，Zx(k')表示k'采样时刻在雷达直角坐标系下雷达对目标x方向的量测值，Zy(k')表示k'采样时刻在雷达直角坐标系下雷达对目标y方向的量测值，Zz(k')表示k'采样时刻在雷达直角坐标系下雷达对目标z方向的量测值，Hj(k')表示k'采样时刻第j个机动转弯子模型的量测矩阵，Rj(k')表示k'采样时刻第j个机动转弯子模型在雷达直角坐标系下雷达对目标的量测噪声协方差矩阵，可以表示如下：

其中，r11(k′)表示k'采样时刻第j个机动转弯子模型在雷达直角坐标系下雷达对目标的量测噪声协方差矩阵Rj(k')中第1行、第1列元素，r12(k′)表示k'采样时刻第j个机动转弯子模型在雷达直角坐标系下雷达对目标的量测噪声协方差矩阵Rj(k')中第1行、第2列元素，r13(k′)表示k'采样时刻第j个机动转弯子模型在雷达直角坐标系下雷达对目标的量测噪声协方差矩阵Rj(k')中第1行、第3列元素，r22(k′)表示k'采样时刻第j个机动转弯子模型在雷达直角坐标系下雷达对目标的量测噪声协方差矩阵Rj(k')中第2行、第2列元素，r23(k′)表示k'采样时刻第j个机动转弯子模型在雷达直角坐标系下雷达对目标的量测噪声协方差矩阵Rj(k')中第2行、第3列元素，r33(k′)表示k'采样时刻第j个机动转弯子模型在雷达直角坐标系下雷达对目标的量测噪声协方差矩阵Rj(k')中第3行、第3列元素，ρ(k')表示k'采样时刻在雷达极坐标系下雷达获取的目标径向距离量测值，θ(k')表示k'采样时刻在雷达极坐标系下雷达获取的目标方位角量测值，ε(k')表示k'采样时刻在雷达极坐标系下雷达获取的目标俯仰角量测值，A(k')表示k'采样时刻雷达极坐标系到雷达直角坐标系的噪声转换矩阵，表示点目标P与雷达之间的径向距离ρ的量测噪声方差值，表示点目标P相对于雷达的方位角θ的量测噪声方差值，表示点目标P相对于雷达的俯仰角ε的量测噪声方差值，点目标P为雷达扫描范围内目标所在轨迹中的任意一点。

最后计算得到k'采样时刻第j个机动转弯子模型的状态向量滤波输出值和k'采样时刻第j个机动转弯子模型的误差协方差滤波输出值Pj(k'|k')，其表达式为分别为：

Pj(k'|k')＝Pj(k'|k'-1)-Kj(k')Sj(k')[Kj(k')]^T

步骤4，利用三个模型滤波输出的新息以及对应的新息协方差服从高斯分布的特性构造似然函数；然后利用各个模型k'采样时刻的似然函数，k'-1采样时刻的模型概率以及模型转移概率更新k'采样时刻的模型概率；最后选择模型概率中最大的概率，利用模型概率最大的模型的距离函数对目标进行机动判别。

步骤4的子步骤为：

(4a)根据k'采样时刻第j个机动转弯子模型的新息值Vj(k')、k'采样时刻第j个机动转弯子模型的新息协方差矩阵Sj(k')，计算k'采样时刻第j个机动转弯子模型的距离函数Dj(k')，然后计算k'采样时刻第j个机动转弯子模型的似然函数Λj(k')，其表达式为：

其中，j＝1,2,3；然后利用k'采样时刻第j个机动转弯子模型的似然函数Λj(k')和k'-1采样时刻第i个机动转弯子模型概率μi(k'-1)，计算k'采样时刻第j个机动转弯子模型概率μj(k')，其表达式为：

进而得到k'采样时刻交互模型集概率pk'，pk'＝{μ1(k'),μ2(k'),μ3(k')}，μ1(k')表示k'采样时刻第1个机动转弯子模型概率，μ2(k')表示k'采样时刻第2个机动转弯子模型概率，μ3(k')表示k'采样时刻第3个机动转弯子模型概率。

(4b)找出k'采样时刻交互模型集概率pk'中的最大概率max(pk')，

max(pk')＝max{μ1(k'),μ2(k'),μ3(k')}。

(4c)选择最大概率对应的机动转弯子模型，记为k'采样时刻第j'个机动转弯子模型，利用k'采样时刻第j'个机动转弯子模型的距离函数Dj'(k')对目标进行机动判别；鉴于Dj'(k')服从量测维数为m的χ²分布，假设目标发生强机动的概率为0.01，则查询χ²分布表可得门限为6.637，设置机动判别门限为M＝7。

当Dj'(k')>M时，判定目标在k'采样时刻机动；否则，判定目标在k'采样时刻非机动。

步骤5，当判定目标在k'采样时刻机动时，计算距离函数和机动门限之差的平方根作为最小网格的调整倍数；当判定目标在k'采样时刻为非机动时，利用模型概率搜索在非机动时段内的模型集中最符合实际模型的角速度。

步骤5的子步骤为：

(5a)设定G为网格间距，G0为设置的最小网格间距，一般取0.1°～1°之间，可以根据要跟踪的目标的机动性大小取值，机动性比较大取大些，反之取小些；网格间距G会根据目标在k'采样时刻的机动性做自适应放缩调整，目标在k'采样时刻的机动大时，网格间距G会变大；反之变小。

(5b)计算k'采样时刻交互模型集转弯角速度Mk'，Mk'＝{ω1(k'),ω2(k'),ω3(k')}，其子步骤为：

(5b.1)设定若max(pk')＝μ2(k')，μ2(k')表示k'采样时刻第2个机动转弯子模型概率；如果D2(k')>M，则

ω2(k')＝ω2(k'-1),ω1(k')＝ω2(k')-G,ω3(k')＝ω2(k')+G；反之，对k'采样时刻交互模型集进行寻优概率逼近，得到：

其中，D2(k')表示k'采样时刻第2个机动转弯子模型的距离函数，G表示网格间距，ω2(k')表示k'采样时刻第2个机动转弯子模型的转弯角速度，ω2(k'-1)表示k'-1采样时刻第2个机动转弯子模型的转弯角速度，ω1(k')表示k'采样时刻第1个机动转弯子模型的转弯角速度，ω1(k'-1)表示k'-1采样时刻第1个机动转弯子模型的转弯角速度，ω3(k')表示k'采样时刻第3个机动转弯子模型的转弯角速度，ω3(k'-1)表示k'-1采样时刻第3个机动转弯子模型的转弯角速度。

(5b.2)设定max(pk')＝μ1(k')，μ1(k')表示k'采样时刻第1个机动转弯子模型概率；如果D1(k')>M，则

ω2(k')＝ω1(k'-1),ω1(k')＝ω2(k')-G,ω3(k')＝ω2(k')+G；反之，对k'采样时刻交互模型集进行寻优概率逼近，得到：

其中，D1(k')表示k'采样时刻第1个机动转弯子模型的距离函数，G表示网格间距，ω1(k')表示k'采样时刻第1个机动转弯子模型的转弯角速度，ω1(k'-1)表示k'-1采样时刻第1个机动转弯子模型的转弯角速度，ω2(k')表示k'采样时刻第2个机动转弯子模型的转弯角速度，ω2(k'-1)表示k'-1采样时刻第2个机动转弯子模型的转弯角速度，ω3(k')表示k'采样时刻第3个机动转弯子模型的转弯角速度，ω3(k'-1)表示k'-1采样时刻第3个机动转弯子模型的转弯角速度，μ2(k')表示k'采样时刻第2个机动转弯子模型概率，μ3(k')表示k'采样时刻第3个机动转弯子模型概率。

(5b.3)设定max(pk′)＝μ3(k′)，μ3(k′)表示k′采样时刻第3个机动转弯子模型概率；如果D3(k′)＞M，则

ω2(k′)＝ω3(k′-1)，ω1(k′)＝ω2(k′)-G，ω3(k′)＝ω2(k′)+G；反之，对k′采样时刻交互模型集进行寻优概率逼近，得到：

其中，D3(k′)表示k′采样时刻第3个机动转弯子模型的距离函数，G表示网格间距，ω2(k′)表示k′采样时刻第2个机动转弯子模型的转弯角速度，ω3(k′-1)表示k′-1采样时刻第3个机动转弯子模型的转弯角速度，ω3(k′)表示k′采样时刻第3个机动转弯子模型的转弯角速度，ω1(k′)表示k′采样时刻第1个机动转弯子模型的转弯角速度，ω1(k′-1)表示k′-1采样时刻第1个机动转弯子模型的转弯角速度，ω2(k′-1)表示k′-1采样时刻第2个机动转弯子模型的转弯角速度，μ2(k′)表示k′采样时刻第2个机动转弯子模型概率，μ2(k′)表示k′采样时刻第2个机动转弯子模型概率。

步骤6，令k′加1，并将k′采样时刻交互模型集概率pk′和k′采样时刻交互模型集转弯角速度Mk′作为新的交互模型集概率集和交互模型集转弯角速度，重复执行步骤3至步骤5，直到得到3采样时刻第j个机动转弯子模型的状态向量滤波输出值至N采样时刻第j个机动转弯子模型的状态向量滤波输出值和3采样时刻第j个机动转弯子模型的误差协方差滤波输出值Pj(3|3)至N采样时刻第j个机动转弯子模型的误差协方差滤波输出值Pj(N|N)，并记为一种调整网格间距的改进AGIMM跟踪结果。

通过以下仿真对本发明效果作进一步验证说明。

(一)仿真环境1：目标设置为转弯机动模型，目标的初始状态为仿真时长为350s，每隔70s，目标改变一次角速度，角速度具体改变设置为w＝[0.5°，-10°，0.7°，6°，-0.7°]，采样时间间隔T＝1s。设置量测噪声1：距离标准差为50m，方位角和俯仰角标准差均为0.1°。

初始模型集设置为W1＝{-10°，0.1°，10°}，即ωmax＝10°，最小网格间距设置为G0＝0.5°。

(二)仿真环境2：在仿真环境1的基础上改变量测噪声，设置量测噪声2：距离标准差为100m，方位角和俯仰角标准差为0.2°。

(三)仿真环境3：目标设置为临近空间高超声速飞行器模型，目标初始状态为目标在z轴一直做匀加速运动。

t＝0～20s：目标在x轴方向做匀速直线运动，y方向加速度为1g；t＝21～380s：目标沿x轴做匀速转弯运动，匀速转弯的角速度为0.098rad/s。t＝81～400s：目标做匀加速直线运动，y方向加速度为-1g；量测噪声设置为噪声1，初始模型集设置为W1，最小网格间距设置为G0＝0.5°。

(四)仿真环境4：在仿真环境3的基础上改变初始模型集的设置，设置初始模型集为W2＝{-20°,0.1°,20°}，即ωmax＝20°。

(五)仿真结果分析：

5.1，仿真环境1结果分析:结合图3和图4可以看出，在噪声1的情况下，即在噪声较小的情况下，改进AGIMM算法虽然比AGIMM算法要好一些，但是优势并不是很明显。但是对比图5a和图5b结合仿真环境设置的转弯角度，可以看出，两种算法在目标机动时刻71s和211s时，对角速度的估计也跟着发生变化，但是改进AGIMM算法相对于AGIMM算法对目标角速度的估计相对而言更为准确。

5.2，仿真环境2结果分析：观察图6和图7可以看出，在增大量测噪声，即把噪声设置为噪声2时，AGIMM算法对目标的跟踪性能大大降低，而改进的AGIMM算法依旧能维持性能的稳定，说明改进算法的抗干扰性能更好些。,

5.3，仿真环境3结果分析：观察图8和图9可以看出，无论是距离RMSE对比图，还是速度RMSE对比图，改进算法对临近空间高超声速飞行器的跟踪效果都要更好。

5.4，仿真环境4结果分析：分别设置初始模型集W1和W2，观察图10和图11，可以看出，改进AGIMM算法受初始模型集的影响很小，而AGIMM算法对初始模型集设置要求更高，当设置的初始模型集和目标运动可能发生的转弯角速度偏离太大则会严重影响跟踪滤波效果；观察图12a和图12b，也可以看出，改进AGIMM算法就算在初始模型集设置范围很大的情况下仍然可以迅速收敛找到接近目标实际运动的角速度。

综上所述，仿真实验验证了本发明的正确性，有效性和可靠性。

显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围；这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。