一种机动策略自适应的目标跟踪信息滤波算法

摘要

本发明提供了一种机动策略自适应的目标跟踪信息滤波算法，首先建立多机动策略和多运动模式的目标跟踪模型，然后进行多机动策略多模型下的目标跟踪信息滤波算法，得到目标跟踪轨迹。本发明引入了机动决策的概念，建立了多机动策略、多运动模型的目标跟踪模型，并通过不匹配机动策略的误差压缩率，利用后验信息实时校正机动策略转移概率矩阵，显著提高目标跟踪过程中机动策略的匹配度，进而提高运动模型的匹配度。同时，通过结合自适应变结构模型和卡尔曼信息滤波，有效融合多传感器量测信息，显著提高目标跟踪精度和稳定度。

说明书

技术领域

本发明属于目标跟踪领域，涉及一种目标跟踪信息滤波算法。

背景技术

随着经济的发展，人们对商用客机的使用需求显著提升，客机数量的提升对如何高效的进行空中交通管理提出了新的要求。同时，随着科技的进步，客机飞行速度的提升和其他民用飞行器的逐渐投入使用，对如何高效进行空中管理提出了挑战。高效空中管理的前提是获取各飞行器精确的运动状态信息，这要求滤波跟踪算法能有效地对各种不同类型目标进行高精度跟踪。但是目前一般的跟踪算法是基于卡尔曼滤波算法，主要存在以下几个问题：1.适合于弱机动能力单一运动方式的目标，对现在空域中具备不同机动能力的飞行器(客机、直升机、无人机等)显然不完全适用。2.随着传感器技术的发展，海量的传感器探测数据要求滤波算法能有效地融合各传感器的量测数据，但是一般的卡尔曼滤波算法不具备多量测信息融合的能力。

发明内容

为了克服现有技术在跟踪不同机动能力的飞行器时自适应能力较差，缺乏多量测信息融合能力的问题，提出一种机动策略自适应的目标跟踪信息滤波算法。

本发明解决其技术问题所采用的技术方案包括以下步骤：

步骤1：建立多机动策略和多运动模式的目标跟踪模型

设目标共有m种可能的运动模型，m取1～3，则k时刻第i种运动模型的目标运动模型与观测模型为：

$>\left(\begin{array}{c}{x}_k=F_{k-1}^i{x}_{k-1}+G_{k-1}^i{\xi }_{k-1}^i\\ Z_k=H_k^i{x}_k+{\eta }_k^i\end{array}\right),i=\mathrm{1,2}...m$>

式中，x_k和Z_k分别为状态向量和观测向量；为模型的噪声向量，为观测噪声向量，且与为相互独立不相关的零均值高斯白噪声，方差分别为和和分别为对应的状态转移矩阵、过程噪声输入阵和观测矩阵；

用表示在机动策略d的作用下由转移到的模型转移概率，该机动策略对应的模型转移概率矩阵为：

式中，d＝1,2,…n表示目标有n种可能的机动策略，

k-1时刻所有可能的模型转移概率矩阵的集合为

用表示机动策略的转移概率，即由转变为的转移概率，P_dl为模型转移概率矩阵之间的转移概率，机动策略之间的转移概率矩阵为：

步骤2：多机动策略多模型下的目标跟踪信息滤波算法

设共有N个坐标雷达并行量测，即k时刻对目标的位置量测信息共有N组分别为：k-1时刻目标的运动模型和机动策略分别为和状态滤波值和协方差分别为与且已知和初值分别为和用表示目标初始时刻运动模型为Mⁱ的概率，表示目标在初始时刻采用机动策略的概率；机动策略转移概率矩阵初值设为T₀＝{P_ij|i,j＝1,2,…n}；$>{\mu }_{k-1}^{\mathrm{id}}=P\{M_{k-1}^i,I_{k-1}^d\}=P\left\{M_{k-1}^i\right\}·P\left\{I_{k-1}^d\right\},{\mu }_0^{\mathrm{id}}=P\left\{M_0^i\right\}·P\left\{I_0^d\right\},$>包含以下步骤：

2.1机动策略与模型联合条件概率的混合估计

运动模型j与机动策略l的联合概率预测值式中，为转变为的转移概率；表示k-1时刻采用机动策略d时对应的模型转移概率矩阵中位于第i行、第j列的项，即由转移到的概率；为k-1时刻目标的运动模型i和机动策略d的联合条件概率；

运动模型j与机动策略l的联合条件概率的混合估计

2.2滤波器混合初始条件计算

机动策略l下运动模型j的初始状态混合估计式中，为k-1时刻机动策略d下运动模型i的滤波估计值；

机动策略l下运动模型j初始状态混合估计的协方差$>{\tilde{P}}_{k-1}^{\mathrm{jl}}=\underset{i}{\Sigma }\underset{d}{\Sigma }\{P_{k-1}^{\mathrm{id}}+[{\hat{x}}_{k-1}^{\mathrm{id}}-{\tilde{x}}_{k-1}^{\mathrm{jl}}]·{[{\hat{x}}_{k-1}^{\mathrm{id}}-{\tilde{x}}_{k-1}^{\mathrm{jl}}]}^T\}{\hat{x}}_{k-1}^{\mathrm{id}}{\mu }^{\mathrm{id}|\mathrm{jl}},$>式中，为k-1时刻机动策略d下运动模型i的滤波估计协方差；

2.3利用卡尔曼信息滤波估计各机动策略及运动模型下的状态与协方差

1).计算k时刻观测序列Z_k对信息状态y_k和Fisher信息Y_k的贡献i_k和I_k

$>\left(\begin{array}{c}{i}_k=\underset{i=1}{\overset{N}{\Sigma }}{\left(H_k^i\right)}^T·{\left(R_k^i\right)}^{-1}·Z_k^i\\ I_k=\underset{i=1}{\overset{N}{\Sigma }}{\left(H_k^i\right)}^T·{\left(R_k^i\right)}^{-1}·H_k^i\end{array}\right)$>

式中，和分别为第i个观测的量测矩阵和量测误差方差，为第i个观测序列；

2).时间更新

$>x_{k|k-1}^{\mathrm{jl}}=F_k^{\mathrm{jl}}·{\tilde{x}}_{k-1}^{\mathrm{jl}}$>

$>P_{k|k-1}^{\mathrm{jl}}=F_k^{\mathrm{jl}}{\tilde{P}}_{k-1}^{\mathrm{jl}}{\left(F_k^{\mathrm{jl}}\right)}^T+Q_k^{\mathrm{jl}}$>

$>Z_{k|k-1}^{\mathrm{jl}}=H_k^{\mathrm{jl}}·x_{k|k-1}^{\mathrm{jl}}$>

式中，分别为机动策略l下运动模型j的状态转移矩阵、观测矩阵和过程噪声方差；分别为机动策略l下运动模型j初始状态混合估计及估计协方差，分别为k时刻机动策略l下运动模型j的状态预测值和量测预测值，为状态预测协方差；

3).卡尔曼增益及模型似然概率计算

$>v_k^{\mathrm{jl}}=Z_k-Z_{k|k-1}^{\mathrm{jl}}$>

$>S_k^{\mathrm{jl}}=H_k^{\mathrm{jl}}{P}_{k|k-1}^{\mathrm{jl}}{\left(H_k^{\mathrm{jl}}\right)}^T+R_k$>

$>{\mathrm{Kg}}_k^{\mathrm{jl}}=P_{k|k-1}^{\mathrm{jl}}{\left(H_k^{\mathrm{jl}}\right)}^T{\left(S_k^{\mathrm{jl}}\right)}^{-1}$>

$>{\Lambda }_k^{\mathrm{jl}}=\frac{\exp [-0.5{\left(v_k^{\mathrm{jl}}\right)}^T{\left(S_k^{\mathrm{jl}}\right)}^{-1}{v}_k^{\mathrm{jl}}]}{\sqrt{\det \left(2\pi S_k^{\mathrm{jl}}\right)}}$>

式中，分别为k时刻机动策略l下运动模型j的新息、新息协方差、卡尔曼滤波增益、模型似然概率；

4).机动策略l与模型j的联合条件概率更新

$>{\mu }_k^{\mathrm{jl}}=P\{M_k^j,I_k^l|Z_k\}=\frac{{\Lambda }_k^{\mathrm{jl}}{\hat{\mu }}_k^{\mathrm{jl}}}{\underset{j}{\Sigma }\underset{l}{\Sigma }{\Lambda }_k^{\mathrm{jl}}{\hat{\mu }}^{\mathrm{jl}}};$>

5).信息状态、Fisher信息更新

$>\left(\begin{array}{c}{Y}_{k|k-1}^{\mathrm{jl}}={\left(P_{k|k-1}^{\mathrm{jl}}\right)}^{-1}\end{array},y_{k|k-1}^{\mathrm{jl}}=Y_{k|k-1}^{\mathrm{jl}}{x}_{k|k-1}^{\mathrm{jl}}\right)$>

$>\left(\begin{array}{cc}{Y}_k^{\mathrm{jl}}=Y_{k|k-1}^{\mathrm{jl}}+\mathrm{Ik}& y_k^{\mathrm{jl}}=y_{k|k-1}^{\mathrm{jl}}+\mathrm{ik};\end{array}\right)$>

6).滤波估计结果

$>\left(\begin{array}{cc}{\hat{x}}_k^{\mathrm{jl}}={\left(Y_k^{\mathrm{jl}}\right)}^{-1}{y}_k^{\mathrm{jl}}& P_k^{\mathrm{jl}}={\left(Y_k^{\mathrm{jl}}\right)}^{-1};\end{array}\right)$>

2.4输出综合

$>\left(\begin{array}{c}{\hat{x}}_k=\underset{j}{\Sigma }\underset{l}{\Sigma }{\hat{x}}_k^{\mathrm{jl}}{\mu }_k^{\mathrm{jl}}\\ P_k=\underset{j}{\Sigma }\underset{l}{\Sigma }\{P_k^{\mathrm{jl}}+[{\hat{x}}_k^{\mathrm{jl}}-{\hat{x}}_k\left]{[{\hat{x}}_k^{\mathrm{jl}}-{\hat{x}}_k]}^T\right\}{\mu }_k^{\mathrm{jl}}\end{array}\right);$>

2.5机动策略转移概率矩阵自适应调整

将原机动策略转移概率矩阵T中各元素P_ij做如下修正：

$>\left(\begin{array}{cc}{P}_{\mathrm{ij}}^{\prime }=P_{\mathrm{ij}}·\frac{1+\frac{P_{\mathrm{ij}}}{P_{\mathrm{ii}}}·\frac{{\mu }_{\mathrm{ms}}^j\left(k\right)}{{\mu }_{\mathrm{ms}}^i\left(k\right)}}{1+\frac{P_{\mathrm{ij}}}{P_{\mathrm{ii}}}·\frac{{\mu }_{\mathrm{ms}}^i\left(k\right)}{{\mu }_{\mathrm{ms}}^j\left(k\right)}}& i\ne j\\ P_{\mathrm{ii}}^{\prime }=1-\underset{j,i\ne j}{\Sigma }{P}_{\mathrm{ij}}^{\prime }& \end{array}\right);$>

即可得到自适应调整后的机动策略转移概率矩阵T'＝{P'_ij|i,j＝1…n}；

不断重复2.1～2.5，直到满足终止条件；

步骤3：步骤2.4所得的终止时刻滤波结果输出即可得到终端滤波值，每一时刻的滤波估计结果为对应每一时刻目标运动的跟踪滤波值，以连续形式输出，即可得到目标跟踪轨迹。

本发明的有益效果是：引入了机动决策的概念，建立了多机动策略、多运动模型的目标跟踪模型，并通过不匹配机动策略的误差压缩率，利用后验信息实时校正机动策略转移概率矩阵，显著提高目标跟踪过程中机动策略的匹配度，进而提高运动模型的匹配度。同时，通过结合自适应变结构模型和卡尔曼信息滤波，有效融合多传感器量测信息，显著提高目标跟踪精度和稳定度。

附图说明

图1是本发明实现方法流程图；

图2是目标真实运动轨迹；

图3是本发明方法与IMM、P-MmMs的跟踪轨迹及目标真实轨迹对比图；

图4是本发明方法与IMM、P-MmMs在目标位置跟踪上的RMSE图；

图5是本发明方法与IMM、P-MmMs在目标速度跟踪上的RMSE图；

图中，IMM为标准交互多模型算法；P-MmMs为机动策略自适应的多模型多机动策略算法；IF-P-MmMs为本发明方法，即机动策略自适应的目标跟踪信息滤波算法。

具体实施方式

下面结合附图和实施例对本发明进一步说明，本发明包括但不仅限于下述实施例。

一种机动策略自适应的目标跟踪信息滤波算法，其特征在于，引入机动决策和机动策略概率转移矩阵的概念，建立具有多种机动决策和多种运动模型的目标跟踪模型；同时，以各个机动策略的误差压缩率反映机动策略相对目标真实机动的匹配度，并由此利用后验信息在线调整机动策略概率转移矩阵；引入卡尔曼信息滤波算法实时融合多传感器量测信息。该算法包括以下步骤：

步骤1：建立多机动策略和多运动模式的目标跟踪模型

设目标共有m种可能的运动模型(m一般取1～3)，则k时刻运动模型i(用表示)的目标运动模型与观测模型为：

$>\left(\begin{array}{c}{x}_k=F_{k-1}^i{x}_{k-1}+G_{k-1}^i{\xi }_{k-1}^i\\ Z_k=H_k^i{x}_k+{\eta }_k^i\end{array}\right)(i=\mathrm{1,2}...m)---\left(1\right)$>

式中，x_k和Z_k分别为状态向量和观测向量；为模型的噪声向量，为观测噪声向量，且与为相互独立不相关的零均值高斯白噪声，方差分别为和和分别为对应的状态转移矩阵、过程噪声输入阵和观测矩阵。

用表示在机动策略d的作用下，由转移到的模型转移概率，因此，该机动策略对应的模型转移概率矩阵为：

式中，d＝1,2,…n表示目标有n种可能的机动策略，n一般取2。于是，k-1时刻所有可能的模型转移概率矩阵的集合为：

用表示机动策略的转移概率，即由转变为的转移概率(P_dl为模型转移概率矩阵之间的转移概率)。因此，机动策略之间的转移概率矩阵为：

式(1)～(3)即组成了具有多种机动策略及多种运动模式的目标跟踪模型。

步骤2：多机动策略多模型下的目标跟踪信息滤波算法

设共有N个坐标雷达并行量测(N一般取2)，即k时刻对目标的位置量测信息共有N组分别为：k-1时刻目标的运动模型和机动策略分别为和状态滤波值和协方差分别为与且已知和初值分别为和用表示目标初始时刻运动模型为Mⁱ的概率，表示目标在初始时刻采用机动策略的概率。机动策略转移概率矩阵初值设为T₀＝{P_ij|i,j＝1,2,…n}。由于目标采用何种运动模式和采用何种机动策略是相互独立的两个事件，因此认为目标在各时刻采用的机动策略只对其下一时刻将要采用的运动模式产生影响，且$>{\mu }_{k-1}^{\mathrm{id}}=P\{M_{k-1}^i,I_{k-1}^d\}=P\left\{M_{k-1}^i\right\}·P\left\{I_{k-1}^d\right\},{\mu }_0^{\mathrm{id}}=P\left\{M_0^i\right\}·P\left\{I_0^d\right\}.$>整体算法包含以下步骤：

2.1机动策略与模型联合条件概率的混合估计

运动模型j与机动策略l的联合概率预测值为：

$>{\hat{\mu }}_k^{\mathrm{jl}}=\underset{i}{\Sigma }\underset{d}{\Sigma }P\left\{I_k^l\right|I_{k-1}^d\}·I_{k-1}^d\left(\mathrm{ij}\right)·{\mu }_{k-1}^{\mathrm{id}}---\left(4\right)$>

式中，为转变为的转移概率；表示k-1时刻采用机动策略d时对应的模型转移概率矩阵中位于第i行、第j列的项，即由转移到的概率；为k-1时刻目标的运动模型i和机动策略d的联合条件概率。

运动模型j与机动策略l的联合条件概率的混合估计μ^id|jl为：

$>{\mu }^{\mathrm{id}|\mathrm{jl}}=\frac{P\left\{I_k^l\right|I_{k-1}^d\}{I}_{k-1}^d\left(\mathrm{ij}\right)·{\mu }_{k-1}^{\mathrm{id}}}{{\hat{\mu }}_k^{\mathrm{jl}}}---\left(5\right)$>

式中各变量定义与式(4)相同。

2.2滤波器混合初始条件计算

机动策略l下运动模型j的初始状态混合估计为：

$>{\tilde{x}}_{k-1}^{\mathrm{jl}}=E\left({\hat{x}}_{k-1}\right|M_k^j,I_k^l,Z_{k-1})=\underset{i}{\Sigma }\underset{d}{\Sigma }{\hat{x}}_{k-1}^{\mathrm{id}}{\mu }^{\mathrm{id}|\mathrm{jl}}---\left(6\right)$>

式中，为k-1时刻机动策略d下运动模型i的滤波估计值，μ^id|jl为式(5)计算出的运动模型j与机动策略l的联合条件概率的混合估计。

机动策略l下运动模型j初始状态混合估计的协方差为：

$>{\tilde{P}}_{k-1}^{\mathrm{jl}}=\underset{i}{\Sigma }\underset{d}{\Sigma }\{P_{k-1}^{\mathrm{id}}+[{\hat{x}}_{k-1}^{\mathrm{id}}-{\tilde{x}}_{k-1}^{\mathrm{jl}}]·{[{\hat{x}}_{k-1}^{\mathrm{id}}-{\tilde{x}}_{k-1}^{\mathrm{jl}}]}^T\}{\hat{x}}_{k-1}^{\mathrm{id}}{\mu }^{\mathrm{id}|\mathrm{jl}}---\left(7\right)$>

式中，为k-1时刻机动策略d下运动模型i的滤波估计协方差，其他变量定义与式(6)中定义相同。

2.3利用卡尔曼信息滤波估计各机动策略及运动模型下的状态与协方差

1).计算k时刻观测序列Z_k对信息状态y_k和Fisher信息Y_k的贡献i_k和I_k

$>\left(\begin{array}{c}{i}_k=\underset{i=1}{\overset{N}{\Sigma }}{\left(H_k^i\right)}^T·{\left(R_k^i\right)}^{-1}·Z_k^i\\ I_k=\underset{i=1}{\overset{N}{\Sigma }}{\left(H_k^i\right)}^T·{\left(R_k^i\right)}^{-1}·H_k^i\end{array}\right)---\left(8\right)$>

式中，和分别为第i个观测的量测矩阵和量测误差方差，为第i个观测序列。

2).时间更新

$>\left(\begin{array}{c}{x}_{k|k-1}^{\mathrm{jl}}=F_k^{\mathrm{jl}}·{\tilde{x}}_{k-1}^{\mathrm{jl}}\\ P_{k|k-1}^{\mathrm{jl}}=F_k^{\mathrm{jl}}{\tilde{P}}_{k-1}^{\mathrm{jl}}{\left(F_k^{\mathrm{jl}}\right)}^T+Q_k^{\mathrm{jl}}\\ Z_{k|k-1}^{\mathrm{jl}}=H_k^{\mathrm{jl}}·x_{k|k-1}^{\mathrm{jl}}\end{array}\right)---\left(9\right)$>

式中，分别为机动策略l下运动模型j的状态转移矩阵、观测矩阵和过程噪声方差；分别为机动策略l下运动模型j初始状态混合估计及估计协方差。分别为k时刻机动策略l下运动模型j的状态预测值和量测预测值，为状态预测协方差。

3).卡尔曼增益及模型似然概率计算

$>\left(\begin{array}{c}{v}_k^{\mathrm{jl}}=Z_k-Z_{k|k-1}^{\mathrm{jl}}\\ S_k^{\mathrm{jl}}=H_k^{\mathrm{jl}}{P}_{k|k-1}^{\mathrm{jl}}{\left(H_k^{\mathrm{jl}}\right)}^T+R_k\\ {\mathrm{Kg}}_k^{\mathrm{jl}}=P_{k|k-1}^{\mathrm{jl}}{\left(H_k^{\mathrm{jl}}\right)}^T{\left(S_k^{\mathrm{jl}}\right)}^{-1}\\ {\Lambda }_k^{\mathrm{jl}}=\frac{\exp [-0.5{\left(v_k^{\mathrm{jl}}\right)}^T{\left(S_k^{\mathrm{jl}}\right)}^{-1}{v}_k^{\mathrm{jl}}]}{\sqrt{\det \left(2\pi S_k^{\mathrm{jl}}\right)}}\end{array}\right)---\left(10\right)$>

式中，分别为k时刻机动策略l下运动模型j的新息、新息协方差、卡尔曼滤波增益、模型似然概率。

4).机动策略l与模型j的联合条件概率更新

$>{\mu }_k^{\mathrm{jl}}=P\{M_k^j,I_k^l|Z_k\}=\frac{{\Lambda }_k^{\mathrm{jl}}{\hat{\mu }}_k^{\mathrm{jl}}}{\underset{j}{\Sigma }\underset{l}{\Sigma }{\Lambda }_k^{\mathrm{jl}}{\hat{\mu }}^{\mathrm{jl}}}---\left(11\right)$>

5).信息状态、Fisher信息更新

$>\left(\begin{array}{cc}{Y}_{k|k-1}^{\mathrm{jl}}={\left(P_{k|k-1}^{\mathrm{jl}}\right)}^{-1}& y_{k|k-1}^{\mathrm{jl}}=Y_{k|k-1}^{\mathrm{jl}}{x}_{k|k-1}^{\mathrm{jl}}\\ Y_k^{\mathrm{jl}}=Y_{k|k-1}^{\mathrm{jl}}+\mathrm{Ik}& y_k^{\mathrm{jl}}=y_{k|k-1}^{\mathrm{jl}}+\mathrm{ik}\end{array}\right)---\left(12\right)$>

6).滤波估计结果

$>\left(\begin{array}{cc}{\hat{x}}_k^{\mathrm{jl}}={\left(Y_k^{\mathrm{jl}}\right)}^{-1}{y}_k^{\mathrm{jl}}& P_k^{\mathrm{jl}}={\left(Y_k^{\mathrm{jl}}\right)}^{-1}---\left(13\right)\end{array}\right)$>

2.4输出综合

$>\left(\begin{array}{c}{\hat{x}}_k=\underset{j}{\Sigma }\underset{l}{\Sigma }{\hat{x}}_k^{\mathrm{jl}}{\mu }_k^{\mathrm{jl}}\\ P_k=\underset{j}{\Sigma }\underset{l}{\Sigma }\{P_k^{\mathrm{jl}}+[{\hat{x}}_k^{\mathrm{jl}}-{\hat{x}}_k\left]{[{\hat{x}}_k^{\mathrm{jl}}-{\hat{x}}_k]}^T\right\}{\mu }_k^{\mathrm{jl}}\end{array}\right)---\left(14\right)$>

2.5机动策略转移概率矩阵自适应调整

将原机动策略转移概率矩阵T中各元素P_ij做如下修正：

$>\left(\begin{array}{cc}{P}_{\mathrm{ij}}^{\prime }=P_{\mathrm{ij}}·\frac{1+\frac{P_{\mathrm{ij}}}{P_{\mathrm{ii}}}·\frac{{\mu }_{\mathrm{ms}}^j\left(k\right)}{{\mu }_{\mathrm{ms}}^i\left(k\right)}}{1+\frac{P_{\mathrm{ij}}}{P_{\mathrm{ii}}}·\frac{{\mu }_{\mathrm{ms}}^i\left(k\right)}{{\mu }_{\mathrm{ms}}^j\left(k\right)}}& i\ne j\\ P_{\mathrm{ii}}^{\prime }=1-\underset{j,i\ne j}{\Sigma }{P}_{\mathrm{ij}}^{\prime }& \end{array}\right)---\left(15\right)$>

即可得到自适应调整后的机动策略转移概率矩阵T'＝{P'_ij|i,j＝1…n}。

不断重复2.1～2.5，即可完成迭代滤波，直到满足终止条件，即可停止滤波。

步骤3：输出结果

将式(14)中所得的终止时刻滤波结果输出即可得到终端滤波值，每一时刻的滤波估计结果为对应每一时刻目标运动的跟踪滤波值，以连续形式输出，即可得到目标跟踪轨迹。

假设一架无人机在二维平面(即xy平面)内机动飞行，机动时做匀速转弯运动，初始状态为：[x₀,V_x0,y₀,V_y0]^T＝[20000,-150,20000,-100]^T，仿真时长为600s。各时间段的机动方式如表1中所示，目标真实运动轨迹如图3所示。

表1目标真实机动方式

设有2个两坐标雷达并行观测，雷达扫描周期为T＝1s,k时刻可提供的观测信息为观测噪声为高斯白噪声，分别为R₁＝diag{[2020]},R₂＝diag{[3030]}。各雷达观测矩阵为：$>H^1=H^2=H=\left(\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& 0\end{array}\right)$>

步骤1:建立多机动策略和多运动模式的初始目标跟踪模型

假设由先验知识得知，无人机所考虑的威胁等级有：低威胁等级A和高威胁等级B，对应的机动策略分别为MS¹,MS²。各机动策略下均包括3个可能的目标运动模型，分别为1个CV模型，2个CT模型(角速率分别为w₁＝0,w₂＝2/57.3,w₃＝-2/57.3rad/s)。机动策略的转移概率矩阵初值为T₀，机动策略与模型的联合概率初值为μ₀。k时刻威胁等级A和B对应的模型转移概率矩阵分别为当无人机处于低威胁等级A时，其机动频率较低，故对角线元素较大；而处于高威胁等级B时，其机动频率较高，的对角线元素与非对角线元素相当。

$>T_0=\left(\begin{array}{cc}0.8& 0.2\\ 0.2& 0.8\end{array}\right),{\mu }_0=\left(\begin{array}{cc}0.17& 0.17\\ 0.165& 0.165\\ 0.165& 0.165\end{array}\right)---\left(16\right)$>

$>I^1=\left(\begin{array}{ccc}0.9& 0.05& 0.05\\ 0.05& 0.9& 0.05\\ 0.05& 0.05& 0.9\end{array}\right),I^2=\left(\begin{array}{ccc}0.34& 0.33& 0.33\\ 0.33& 0.34& 0.33\\ 0.33& 0.33& 0.34\end{array}\right)---\left(17\right)$>

机动策略I^d下运动模型的数学模型为：

$>\left(\begin{array}{c}{x}_k={F^{\mathrm{id}}{x}_{k-1}+\mathrm{G\xi }}^{\mathrm{id}}\\ Z_k={\mathrm{Hx}}_k+{\eta }_k^{\mathrm{id}}\end{array}\right)---\left(18\right)$>

其中，i＝1,2,3；d＝1,2；和分别为系统噪声和观测噪声；F^1d为CV模型对应的状态转移矩阵，F^id(i＝2,3)为CT模型对应的状态转移矩阵，G为过程噪声输入阵，可分别表示为：

$>F^{1d}=\left(\begin{array}{cccc}1& T& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}\right),F^{\mathrm{id}}=\left(\begin{array}{cccc}1& \frac{\sin w_{i}T}{w_i}& 0& \frac{\cos w_{i}T-1}{w_i}\\ 0& \cos w_{i}T& 0& -\sin w_{i}T\\ 0& \frac{\cos w_{i}T-1}{w_i}& 1& \frac{\sin w_{i}T}{w_i}\\ 0& \sin w_{i}T& 0& \cos w_{i}T\end{array}\right)(i=\mathrm{2,3})---\left(19\right)$>

$>G=\left(\begin{array}{cc}{T}^2/2& 0\\ T& 0\\ 0& T^2/2\\ 0& T\end{array}\right)---\left(20\right)$>

步骤2:多机动策略多模型下的目标跟踪信息滤波算法

2.1:机动策略与运动模型联合条件概率的混合估计

运动模型j与机动策略l的联合概率预测值为：

$>{\hat{\mu }}_k^{\mathrm{jl}}=\underset{i=1}{\overset{3}{\Sigma }}\underset{d=1}{\overset{2}{\Sigma }}T\left(\mathrm{dl}\right)·I_{k-1}^d\left(\mathrm{ij}\right)·{\mu }_{k-1}^{\mathrm{id}}---\left(21\right)$>

式中，T(dl)为转变为的转移概率；表示k-1时刻采用机动策略d时对应的模型转移概率矩阵中位于第i行、第j列的项，即由转移到的概率；为k-1时刻目标的运动模型i和机动策略d的联合条件概率。

从而运动模型j与机动策略l的联合条件概率的混合估计μ^id|jl为：

$>{\mu }^{\mathrm{id}|\mathrm{jl}}=\frac{T\left(\mathrm{dl}\right)I^d\left(\mathrm{ij}\right)·{\mu }_{k-1}^{\mathrm{id}}}{{\hat{\mu }}_k^{\mathrm{jl}}}---\left(22\right)$>

2.2:滤波器混合初始条件计算

各机动策略与运动模型下的初始状态混合估计和协方差分别为：

$>\left(\begin{array}{c}{\tilde{x}}_{k-1}^{\mathrm{jl}}=\underset{i=1}{\overset{3}{\Sigma }}\underset{d=1}{\overset{2}{\Sigma }}{\hat{x}}_{k-1}^{\mathrm{id}}{\mu }^{\mathrm{id}|\mathrm{jl}}\\ {\tilde{P}}_{k-1}^{\mathrm{jl}}=\underset{i=1}{\overset{3}{\Sigma }}\underset{d=1}{\overset{2}{\Sigma }}\{P_{k-1}^{\mathrm{id}}+[{\hat{x}}_{k-1}^{\mathrm{id}}-{\tilde{x}}_{k-1}^{\mathrm{jl}}]·{[{\hat{x}}_{k-1}^{\mathrm{id}}-{\tilde{x}}_{k-1}^{\mathrm{jl}}]}^T\}{\hat{x}}_{k-1}^{\mathrm{id}}{\mu }^{\mathrm{id}|\mathrm{jl}}\end{array},-,-,-,\left(23\right)\right)$>

式中，和分别为机动策略d下运动模型i的滤波估计值和滤波估计协方差，μ^id|jl为式(22)计算出的运动模型j与机动策略l的联合条件概率的混合估计。

2.3:利用卡尔曼信息滤波估计各机动策略及运动模型下的状态与协方差

1).计算k时刻观测序列Z_k对信息状态y_k和Fisher信息Y_k的贡献i_k和I_k

$>\left(\begin{array}{c}{i}_k=\underset{i=1}{\overset{2}{\Sigma }}{\left(H\right)}^T·{\left(R^i\right)}^{-1}·Z_k^i\\ I_k=\underset{i=1}{\overset{2}{\Sigma }}{\left(H\right)}^T·{\left(R^i\right)}^{-1}·H\end{array}\right)---\left(24\right)$>

式中，H为各量测序列的观测矩阵(本例中观测信息均来自两坐标雷达，故H相同)。和Rⁱ分别为第i个量测序列k时刻的量测信息和量测误差方差。

2).时间更新

$>\left(\begin{array}{c}{x}_{k|k-1}^{\mathrm{jl}}=F^{\mathrm{jl}}·{\tilde{x}}_{k-1}^{\mathrm{jl}}\\ P_{k|k-1}^{\mathrm{jl}}=F^{\mathrm{jl}}·{\tilde{P}}_{k-1}^{\mathrm{jl}}{\left(F^{\mathrm{jl}}\right)}^T+Q^{\mathrm{jl}}\\ Z_{k|k-1}^{\mathrm{jl}}=H·x_{k|k-1}^{\mathrm{jl}}\end{array}\right)---\left(25\right)$>

式中，F^jl、H、Q^jl分别为机动策略l下运动模型j的状态转移矩阵、观测矩阵和过程噪声方差；分别为机动策略l下运动模型j初始状态混合估计及估计协方差。分别为k时刻机动策略l下运动模型j的状态预测值和量测预测值，为状态预测协方差。

3).卡尔曼增益及模型似然概率计算

$>\left(\begin{array}{c}{v}_k^{\mathrm{jl}}=Z_k-Z_{k|k-1}^{\mathrm{jl}}\\ S_k^{\mathrm{jl}}={\mathrm{HP}}_{k|k-1}^{\mathrm{jl}}{H}^T+R_k\\ {\mathrm{Kg}}_k^{\mathrm{jl}}=P_{k|k-1}^{\mathrm{jl}}{H}^T{\left(S_k^{\mathrm{jl}}\right)}^{-1}\\ {\Lambda }_k^{\mathrm{jl}}=\frac{\exp [-0.5{\left(v_k^{\mathrm{jl}}\right)}^T{\left(S_k^{\mathrm{jl}}\right)}^{-1}{v}_k^{\mathrm{jl}}]}{\sqrt{\det \left(2\pi S_k^{\mathrm{jl}}\right)}}\end{array}\right)---\left(26\right)$>

式中，分别为k时刻机动策略l下运动模型j的新息、新息协方差、卡尔曼滤波增益、模型似然概率。

4).机动策略与模型的联合条件概率更新

$>{\mu }_k^{\mathrm{jl}}=\frac{{\Lambda }_k^{\mathrm{jl}}{\hat{\mu }}_k^{\mathrm{jl}}}{\underset{j=1}{\overset{3}{\Sigma }}\underset{l=1}{\overset{2}{\Sigma }}{\Lambda }_k^{\mathrm{jl}}{\hat{\mu }}^{\mathrm{jl}}}---\left(27\right)$>

式中，为运动模型j与机动策略l的联合概率预测值，似然概率由式(26)计算。

5).信息状态、Fisher信息更新

$>\left(\begin{array}{cc}{Y}_{k|k-1}^{\mathrm{jl}}={\left(P_{k|k-1}^{\mathrm{jl}}\right)}^{-1}& y_{k|k-1}^{\mathrm{jl}}=Y_{k|k-1}^{\mathrm{jl}}{x}_{k|k-1}^{\mathrm{jl}}\\ Y_k^{\mathrm{jl}}=Y_{k|k-1}^{\mathrm{jl}}+\mathrm{Ik}& y_k^{\mathrm{jl}}=y_{k|k-1}^{\mathrm{jl}}+\mathrm{ik}\end{array}\right)---\left(28\right)$>

6).滤波估计结果

$>\left(\begin{array}{c}{\hat{x}}_k^{\mathrm{jl}}={\left(Y_k^{\mathrm{jl}}\right)}^{-1}{y}_k^{\mathrm{jl}}\\ P_k^{\mathrm{jl}}={\left(Y_k^{\mathrm{jl}}\right)}^{-1}\end{array}\right)---\left(29\right)$>

2.4输出综合

$>\left(\begin{array}{c}{\hat{x}}_k=\underset{j=1}{\overset{3}{\Sigma }}\underset{l=1}{\overset{2}{\Sigma }}{\hat{x}}_k^{\mathrm{jl}}{\mu }_k^{\mathrm{jl}}\\ P_k=\underset{j=1}{\overset{3}{\Sigma }}\underset{l=1}{\overset{2}{\Sigma }}\{P_k^{\mathrm{jl}}+[{\hat{x}}_k^{\mathrm{jl}}-{\hat{x}}_k\left]{[{\hat{x}}_k^{\mathrm{jl}}-{\hat{x}}_k]}^T\right\}{\mu }_k^{\mathrm{jl}}\end{array}\right)---\left(30\right)$>

2.5机动策略转移概率矩阵自适应调整

不同机动策略(Maneuvering Strategy,MS)的决策概率计算为：

$>{\mu }_{\mathrm{ms}}^l\left(k\right)=\underset{j=1}{\overset{3}{\Sigma }}{\mu }_k^{\mathrm{jl}}---\left(31\right)$>

从而可得自适应调整后的机动策略转移概率矩阵T'：

$>\left(\begin{array}{c}{P}_{\mathrm{ij}}^{\prime }=P_{\mathrm{ij}}·\frac{1+\frac{P_{\mathrm{ij}}}{P_{\mathrm{ii}}}·\frac{{\mu }_{\mathrm{ms}}^j\left(k\right)}{{\mu }_{\mathrm{ms}}^i\left(k\right)}}{1+\frac{P_{\mathrm{ij}}}{P_{\mathrm{ii}}}·\frac{{\mu }_{\mathrm{ms}}^i\left(k\right)}{{\mu }_{\mathrm{ms}}^j\left(k\right)}}(i\ne j)\\ P_{\mathrm{ii}}=1-P_{\mathrm{ij}}^{\prime }\end{array}\Rightarrow T^{\prime }=\left(\begin{array}{cc}{P}_{11}^{\prime }& P_{12}^{\prime }\\ P_{21}^{\prime }& P_{22}^{\prime }\end{array}\right)\right)---\left(32\right)$>

由步骤2～步骤3不断循环迭代，直到满足终止条件即可。

步骤3：输出结果

将式(14)中所得的终止时刻滤波结果输出即可得到终端滤波值，每一时刻的滤波估计结果为对应每一时刻目标运动的跟踪滤波值，以连续形式输出，即可得到目标跟踪轨迹。

图3、图4、图5分别为本文发明方法IF-P-MmMs与IMM及P-MmMs方法在相同条件下对目标的跟踪轨迹对比图、目标位置跟踪RMSE、目标速度跟踪RMSE。图3中由于横纵坐标单位较大(为10⁴)，因此三种方法在轨迹跟踪上的区别并不明显，但从图4和图5的RMSE变化曲线即可清楚地看到，采用本发明方法可以在目标采取机动时仍然保持稳定的跟踪，并且跟踪精度始终处于较高水平。因此，本发明方法比标准IMM方法和多机动策略多模型方法以及机动策略自适应的多机动策略多模型方法均在目标跟踪稳定度和精度上有显著的提升。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。