CN105785359B - 一种多约束机动目标跟踪方法 - Google Patents

Abstract

本发明涉及一种多约束机动目标跟踪方法，所述方法包括以下步骤：A、获得机动目标的位置量测信息；B、将所述位置量测信息转换为笛卡尔坐标量测；C、获得所述机动目标的运动轨迹的约束条件；D、依据所述机动目标的运动特点，为每个约束条件单独分配运动模型，从而获得多个运动模型；E、针对各所述运动模型分别进行滤波。依据本发明的方法能够提高滤波性能。

Description

一种多约束机动目标跟踪方法

技术领域

本发明涉及机动目标跟踪，尤其涉及多约束机动目标跟踪方法。

背景技术

现代战场中，反辐射导弹极大地威胁雷达的生存。目前应对反辐射导弹的方法主要包括采用低拦截概率技术，控制雷达间歇发射，采用主动诱饵干扰反辐射导弹和采用预警系统等，其中最为经济有效的方式是采用主动诱饵干扰的方法。

图1示出了受主动诱饵干扰情况下的反辐射导弹运动轨迹示意图。如图1所示，反辐射导弹在受到主动诱饵干扰之后运动方向存在多种可能，即继续向原打击目标A运动，或是向干扰源B运动，也有可能向两个辐射源中间的某处运动。因而导弹偏离了针对目标A的轨迹，目标A就得到了保护，因而这种方法非常的有效。但是，对于这种方法来说，保证主动诱饵准确对准反辐射导弹发射信号是十分必要的，而这需要跟踪算法提供较高的滤波和预测精度。考虑到反辐射导弹在其运动末段具有高速和高机动的特点，雷达观测的数据率可能相对较低。这又要求跟踪方法在低数据率情况下仍然是稳定有效的。

对于机动目标跟踪而言，通常包括雷达信号处理(检测目标，获得量测)和雷达数据处理。雷达数据处理又进一步包括：A)量测数据预处理；B)数据互联，即建立某时刻雷达量测数据和其他时刻量测数据的关系，以确定这些量测数据是否来自同一个目标的处理过程；C)跟踪，即对量测进行滤波，得到更准确的目标位置信息；以及D)形成航迹的步骤。其中如何进行跟踪将直接影响机动目标跟踪系统的性能。

对于反辐射导弹来说，由于其导引头角跟踪的导引策略，其在笛卡尔坐标系下X-Y平面上的运动轨迹是指向打击目标的一条直线，机动主要发生在沿运动轨迹方向，在垂直运动轨迹的方向上难以发生大的机动。这符合等式约束估计算法的应用条件，可以从其轨迹中提取先验的约束信息来提高滤波和预测性能。

然而，目前的反辐射导弹跟踪方法并没有有效利用约束先验信息，如J.Zou,K.Gaoand E.Zhang,“Inducing anti-radiation missile safely under coherent decoybased on VD-Kalman Filter,”IEEE International Conference on InformationEngineering and Computer Science,pp.1--4,2009(基于变维卡尔曼滤波的反辐射导弹相参诱偏)，单纯设置机动模型和非机动模型，以变维卡尔曼滤波方法处理反辐射导弹跟踪问题，先验的约束条件没有得到合理利用从而造成信息浪费；估计结果不满足约束条件造成性能损失。此外，由于主动诱饵的存在，反辐射导弹可能存在多个可能的打击目标，因此可能存在多个约束条件同时作用，不能简单以单约束情况来看待这个问题。而前人对于多约束目标跟踪方法的研究，如J.Dunik,M.Simandl and O.Straka,“Multiple-modelfiltering with multiple constraints,”IEEE American Control Conference,pp.6858--6863,2010(多约束条件下的多模型滤波)，只考虑静态约束场景，没有把动态约束纳入考虑；对约束不确定性考虑不充分，也难以适用于反辐射导弹跟踪问题。

综上所述，目前的反辐射导弹跟踪方法无法提供足够的滤波和预测精度保证主动诱饵准确对准反辐射导弹。

发明内容

本发明鉴于现有技术的上述问题作出，用以克服或缓解现有技术中存在的一项或更多的缺点，至少提供一种有益的选择。

为实现上述目的，本发明提供了一种机动目标跟踪方法，所述方法包括以下步骤：A、获得机动目标的位置量测信息；B、将所述位置量测信息转换为笛卡尔坐标量测；C、获得所述机动目标的运动轨迹的约束条件；D、依据所述机动目标的运动特点，为每个约束条件单独分配运动模型，从而获得多个运动模型；E、针对各所述运动模型分别进行滤波。

根据本发明的一种实施方式，所述机动目标为反辐射导弹，所述机动目标的位置量测信息为导弹相对观测雷达坐标系原点的距离、方位角和俯仰角，利用去偏量测转换方法将所述位置量测信息转换为笛卡尔坐标量测。

根据本发明的一种实施方式，在步骤C中，用量测信息中提供的机动目标的位置来代替起点，依据下式构造出近似的线性约束条件：

其中，C_k是k时刻包含线性等式约束条件的约束矩阵，x_k是状态向量，其中包含x_k和y_k为目标在X，Y方向上的位置；和为目标在X，Y方向上的速度，δ_k是约束直线与Y轴夹角，和是k时刻X，Y方向的转换量测值，(x_n,y_n)是先验的目的地坐标，约束直线是其上的点都满足约束条件的直线。

根据本发明的一种实施方式，针对各所述运动模型，分别采用不同的方法对X-Y平面和Z方向进行滤波，所述方法还包括将X-Y平面和Z方向的滤波预测结果合并的步骤。

根据本发明的一种实施方式，所述步骤E包括计算各运动模型对应的模型概率的步骤，以及按模型概率对针对各所述运动的滤波结果进行过加权融合的步骤。

根据本发明的一种实施方式，对于时刻k各运动模型对应的概率依据以下公式计算运动模型j的模型似然值：

并且如下地对运动模型j的模型概率进行更新：

根据本发明的一种实施方式，所述方法还包括基于k时刻针对各运动模型的滤波结果外推下一时刻预测结果，并根据k时刻模型概率对针对各运动模型的预测结果加权融合，得到k+1时刻融合预测结果的步骤。

根据本发明的一些实施方式，能够适时的改变模型概率，使得更加接近物理实际的运动模型对应概率更高，从而可以提高滤波性能。

附图说明

结合附图，可以更好地理解本发明，但是附图仅仅是示例性的，不是对本发明的限制。

图1示出了受主动诱饵干扰情况下的反辐射导弹运动轨迹示意图；

图2示出了依据本发明的一种实施方式的流程图；

图3示出了根据本发明的一种实施方式的方法在实验中的X-Y平面的仿真轨迹；

图4示出了根据本发明的一种实施方式的方法在实验中的Z平面的仿真轨迹；

图5示出了仿真数据方位角滤波结果；

图6示出了仿真数据俯仰角滤波结果；

图7示出了仿真数据方位角预测结果的均方根误差；

图8示出了仿真数据俯仰角预测结果的均方根误差。

具体实施方式

下面结合附图描述本发明的具体实施方式。附图和实施方式中的描述仅仅是示例性的，不是对本发明的保护范围的限制。

图2示出了依据本发明的一种实施方式的流程图。如图2所示，首先在步骤S210，从观测雷达处获得雷达位置量测信息。在一种实施方式中，雷达位置量测信息为导弹相对观测雷达坐标系原点的距离、方位角和俯仰角。至于雷达如何获得这些信息、以及本发明实施方式的方法如何从雷达处获得这些信息，本领域技术人员可以采取各种方式来实现，无论采用哪种方法都在本发明的保护范围内。

接着，在步骤S220，将来自观测雷达的雷达位置量测信息(如上文所示，即导弹相对观测雷达坐标系原点的距离、方位角和俯仰角)转换为笛卡尔坐标量测。可以采用多种方法进行这种转换，无论采用哪种方法都在本发明的范围内。在一种实施方式中，优选地，采用去偏量测转换方法。

去偏量测转换方法如下：

其中和是从雷达获取的距离，方位角和俯仰角量测，是基于目标量测位置得到的平均真实偏差，是量测向量，和是转换后得到的笛卡尔坐标量测。

和是对应三个方向的平均真实误差，和分别是对应方位角和俯仰角的测量噪声方差。

对应的协方差矩阵为

其中

和分别对应三个方向的转换量测噪声方差，和表示各个方向之间转换量测噪声协方差。

其中和两组参数分别为：

sinh是双曲正弦函数，cosh是双曲余弦函数，

然后，在步骤S230，获得导弹的运动轨迹的约束条件(即约束条件建模)。

对于X-Y平面，利用雷达位置量测信息(该即上文的距离、方位角、俯仰角)和先验的目的地信息(在本实施例中，是指导弹攻击目标的坐标)对反辐射导弹X-Y平面运动轨迹中隐含的线性等式约束条件进行近似。

对于反辐射导弹来说，由于其导引头角跟踪的导引策略，其在笛卡尔坐标系下X-Y平面上的运动轨迹是指向打击目标的一条直线。并且对于跟踪方来说，反辐射导弹打击的目标的位置(在本实施方式中，该目标为受保护雷达(导弹攻击目标)，其与观测雷达不一定是同一个，另外，该目标也可为主动干扰源)也是先验已知的信息。于是隐含线性约束条件的形状和终点是可以确定的，但起点无法确定，于是用量测点(量测点即量测信息中提供的导弹位置)来代替起点，构造出近似的线性约束条件如下式：

C_k是k时刻包含线性等式约束条件的约束矩阵，x_k是状态向量，其中包含x_k和y_k为目标在X，Y方向上的位置；和为目标在X，Y方向上的速度。

其中

δ_k是约束直线与Y轴夹角，和是k时刻x，y方向的转换量测值，(x_n,y_n)是先验的目的地坐标，可能不止一个。约束直线是其上的点都满足约束条件的直线。可以看到构建的线性等式约束条件事实上是对目标速度的约束。

因而，根据本发明的一种实施方式，通过表达式利用雷达位置量测和目的地先验信息构造了近似的线性等式约束条件。另外，根据场景不同，可能存在多个约束条件。

因Z方向上目标运动特点与X-Y平面不同，无明显线性等式约束条件，因此可以不对Z方向进行约束条件建模。

接着，在步骤S240，根据目标运动特点设置运动模型。运动模型用来描述目标动态特性，例如匀速模型和匀加速模型，不同模型对应不同的状态方程。在本步骤S240中，为每个约束条件单独分配运动模型，从而获得多个运动模型。也就是说，依据本发明的实施方式，同一运动模型在不同约束条件下应用即看作不同的模型。举例来说，在有一个主动干扰源的情况下，匀速模型在本发明的一些实施方式中可以分为约束条件1下的匀速模型和约束条件2下的匀速模型，这两者是不同的模型，两个约束条件下的所有运动模型共同组成模型集。在后续操作中，将针对不同的模型进行滤波。

运动模型是根据目标运动特点来设置的，在本发明的一些实施方式中，反辐射导弹在单个方向上(X,Y,Z)主要做匀速或匀加速运动，因此在交互式多模型滤波中设置匀速和匀加速模型。

之后，在步骤S250，进行模型滤波器初始化。

为进行滤波，需要建立状态方程和系统观测方程。状态方程和系统观测方程应该在滤波器初始化前进行，即建立方程后再初始化，然后进行滤波。确定约束条件需要在滤波开始前，约束条件数量会影响交互式多模型滤波中模型集模型数量，因此推荐在建立方程前确定约束条件。

状态方程是目标运动规律的假设，根据选定的运动模型写出目标状态的递推表达式x_k＝Φ_kx_k-1+Γ_kv_k-1，其中Φ_k是状态转移矩阵，Γ_k是过程噪声分布矩阵，与运动模型有关；x_k是状态向量，v_k-1是过程噪声向量。

量测方程是雷达测量过程的假设，对于线性系统而言量测方程可表示为z_k＝H_kx_k+w_k，其中z_k为量测向量，H_k是量测矩阵，x_k是状态向量，w_k是量测噪声向量。

尽管为了描述的方便，将模型滤波器的初始化设为单独的一个步骤，但是根据上下文，其也可以认为是后面滤波步骤的一部分。

根据本发明的一种实施方式，采用两点差分法对各模型滤波器进行初始化。可以采用其他的方法对各模型滤波器进行初始化。各模型滤波器对应不同运动模型设立，对量测值进行滤波。

对于X-Y平面

初始状态协方差

在CA模型滤波初始化(滤波器初始化)时需要进行状态扩维，即加入加速度初值和及其协方差和的先验信息或是经验值。

对于Z方向

初始状态协方差

在CA模型滤波初始化时需要进行状态扩维，即加入加速度初值及其协方差的先验信息或是经验值。

之后，在步骤S260进行滤波。

针对每一个运动模型，都分为针对X-Y平面的滤波和针对Z方向的滤波。

下面首先描述X-Y方向的滤波。

针对状态方程

x_k＝Φ_kx_k-1+Γ_kv_k-1

对于X-Y平面

或

分别对应NCV模型和NCA模型，状态变量分别为和(采用NCA模型时需要先将状态扩维，引入加速度分量，滤波后再降维)，T为雷达扫描周期。v_k-1为零均值高斯白噪声，其协方差为

其中q_x和q_y是X和Y方向的过程噪声标准差。

系统观测方程为

其中H_k为量测矩阵。

或

w_k为去偏转换测量噪声，其统计特性如下式所示

执行交互式多模型滤波，这里引入变结构思想，分两种情况处理。认为干扰开启时刻t_n是先验已知的，当k<t_n时，即干扰开启前，认为反辐射导弹只存在一个打击目标，即真实目标。因此滤波器设置为只有一个约束条件(这个约束条件是通过前面的约束条件建模得到的)参与滤波过程(约束条件参与滤波过程是在下文求约束投影的步骤)。同时设置模型过渡概率中保持模型不变概率较高。当k≥t_n时，即干扰开启后，认为真实目标和干扰源均可能被反辐射导弹作为打击目标，此时滤波器设置为两个约束条件同时作用。同时提高模型过渡概率中模型改变的概率(这个概率是下面公式中的ρ_ij，表示从模型i转移到模型j的转移概率，是先验给定的，这里在k≥t_n时改变这个固定参数)，使得新加入约束对应模型的概率提高可能性增大。这两种变结构设置可以使算法适应性更强，更加接近物理实际。

换言之，在本实施方式中，变结构主要体现在两个方面，一个是根据先验的干扰开启时刻t_n信息改变模型过渡概率ρ_ij的值，即设置模型保持不变概率较高或是提高模型改变概率；另一个是根据先验的干扰开启时刻t_n信息改变参与交互式多模型滤波的模型滤波器数量，即只有对应约束条件1的模型滤波器发挥作用或是对应约束条件1和2的模型滤波器都发挥作用。

子滤波器模型即之前提到的不同运动模型，每个滤波器对应一个运动模型，整个算法中包括多个子滤波器，因此运动模型也称为子滤波器模型。子滤波器模型Mj和Mi是各自独立存在的模型，但存在着相互转换的关系，i|j即表示目标实际运动模型由i模型转换到j模型(可能是部分转换，程度由概率变化表征)。因为存在模型转换，不能单纯以上一时刻状态估计结果作为滤波器输入，需要计算混合初值来进行代替，相互作用求滤波初值是每一时刻滤波循环的起始。

在X_Y平面进行滤波时，已经知道的变量包括上一时刻各模型滤波器状态估计结果状态估计误差协方差上一时刻模型概率(上一时刻滤波结果)，本时刻转换量测(雷达得到)，过程噪声协方差矩阵Q_k-1，量测噪声协方差矩阵约束矩阵C_k，状态转移矩阵Φ_k-1，量测矩阵H_k(先验已知)。然后进行了相互作用求混合初值，各模型滤波器单独滤波(包括计算状态一步预测及其协方差，计算新息及其协方差，计算滤波增益，更新状态及其协方差)，模型概率更新，状态估计融合等步骤得到了滤波结果。

子滤波器模型M_j在k-1时刻的混合状态估值由模型M_i的输出相应的模型概率以及过渡概率ρ_ij来计算，即

在k-1时刻，子滤波器模型M_j的状态误差协方差阵为

其中，为k-1时刻第i个子滤波器的状态估计，为相应的状态协方差矩阵，N为模型数目，此处包括为各个约束条件单独分配的运动模型。

将状态向量及其方差与转换量测值一起作为k时刻第j个模型的输入值。由于状态变量与量测间存在非线性关系，可以选用无迹卡尔曼滤波器(UKF)进行滤波。

然后进行状态一步预测(状态的一步预测，即利用k-1时刻滤波结果相互作用得到的混合初值对k时刻的状态进行预测，包括目标的位置，速度等。

其中，Φ_k-1为状态转移矩阵，为相互作用得到的混合初值。应注意，混合初值是基于上一时刻各模型滤波器状态估计结果计算出的，替代上一时刻各模型滤波器状态估计结果作为滤波器输入使用，属于状态向量的一种。

计算状态一步预测协方差

其中Q_k-1过程噪声协方差矩阵

确定状态一步预测约束投影值

其中C_k为约束矩阵

确定量测预测

其中H_k为量测矩阵；

接着进行新息计算。新息指量测值与量测预测值的差。

其中为转换量测值

计算量测预测协方差

其中为量测噪声协方差矩阵

确定滤波增益

进行状态更新，获得k时刻j模型滤波结果

进行协方差更新，获得k时刻j模型滤波误差协方差

在得到各个模型k时刻的输出和后，需要更新该时刻各模型对应的概率首先计算对应模型j的可能性为

模型j的模型更新为

其中，ρ_ij为模型过渡概率，为k-1时刻i模型概率。

于是k时刻交互式多模型滤波的融合输出为

于是便得到了对量测值的滤波结果及其协方差，接着对目标状态(包括位置和速度)进行外推预测，首先采用与滤波过程相同的方法得到混合初值及其协方差然后求出k+1时刻的状态估计

其中，是k+1时刻j模型状态的预测值，是k时刻各模型滤波器滤波结果相互作用得到的混合初值，Φ_k是状态转移矩阵。

按k时刻模型概率得到k+1时刻预测结果为

其中，是k+1时刻各模型预测值融合结果，是k时刻j模型概率。

对于Z方向

或

分别对应NCV模型和NCA模型，状态变量分别为和(采用NCA模型时需要先将状态扩维，引入加速度分量，滤波后再降维)，T为雷达扫描周期，其中，Φ_k为状态转移矩阵，Γ_k为过程噪声分布矩阵。v_k-1为零均值高斯白噪声，其协方差为

其中q_z是Z方向的过程噪声标准差。

系统观测方程为

其中H_k为量测矩阵。

H_k＝[1 0]或H_k＝[1 0 0]

w_k为去偏转换测量噪声，其统计特性如下式所示

执行交互式多模型滤波，与X-Y平面的滤波方法基本相同，只是去掉了变结构和约束投影的部分。

最后将X-Y平面和Z方向的滤波预测结果合并得到整体的滤波预测结果。

最后，在步骤S270，将滤波结果转换到球坐标系下

通过上式将滤波结果转换到球坐标系下，得到目标距离，方位角和俯仰角滤波结果然后转第一步等待下一时刻k+1时刻雷达观测数据。

本发明的一些实施方式所述的机动目标跟踪方法为反辐射导弹跟踪提供了一条新途径，合理利用其运动轨迹中隐含的线性等式约束信息，避免了信息浪费。该方法的另一个优点是结合了交互式多模型方法，为每个约束条件单独分配运动模型，可以有效应对受到干扰的情况下反辐射导弹存在多个可能目标的情况。还提高了应对目标可能存在的变速机动的能力，多种运动模型同时作用有效匹配目标实际运动模型，减少模型失配导致的性能恶化。

为了验证本发明的实施方式的效果，根据反辐射导弹的运动特点和噪声统计特性，人为构造出一组仿真数据，并利用这组仿真数据进行蒙特卡洛实验来证实算法的性能。图3示出了根据本发明的一种实施方式的方法在实验中的X-Y平面的仿真轨迹。图4示出了根据本发明的一种实施方式的方法在实验中的Z平面的仿真轨迹。在图3和图4所示场景中，采样间隔T＝5s，共进行100次蒙特卡洛实验。从图3和图4可以看出，本发明的实施方式的方法能够有效应对多约束场景。

图5示出了仿真数据方位角滤波结果,图6示出了仿真数据俯仰角滤波结果,展示了算法的滤波精度。从图5和图6中可以明显看出多约束算法有效减小了由目标运动方向变化带来的误差峰值，均方根误差曲线在整个滤波过程中波动都比较小。对于方位角来说，多约束算法将均方根误差控制在0.8deg左右，相比非约束算法1.6deg的峰值，优化效果十分明显。对于俯仰角来说也是一样。这是由于方法本身引入了多约束和变结构的思想，在目标改变运动方向(干扰开启)之后，能够适时的改变模型概率，使得更加接近物理实际的运动模型对应概率更高，从而避免滤波性能的恶化。

图7示出了仿真数据方位角预测结果的均方根误差。图8示出了仿真数据俯仰角预测结果的均方根误差，展示了算法的预测精度。从图7和图8中可以看出，多约束算法方位角预测均方根误差在整个预测过程中均明显低于非约束算法，除最后一个采样点以外整体控制在2deg以内。俯仰角均方根误差除最后一个点外也基本控制在2deg内。最后一个采样点误差明显增大是由于目标距离原点太近，根据量测计算出的目的地约束条件也没有前面准确，因此造成了误差的增大。

以上的实施方式都是示例性的，不是对本发明的保护范围的限制，在本发明的构思的范围内的任何等同变换、补充和改进都在本发明的保护范围内。

Claims (5)

1.一种多约束机动目标跟踪方法，所述方法包括以下步骤：

A、获得机动目标的位置量测信息；

B、将所述位置量测信息转换为笛卡尔坐标量测；

C、获得所述机动目标的运动轨迹的约束条件；

D、依据所述机动目标的运动特点，为每个约束条件单独分配运动模型，从而获得多个运动模型；

E、针对各所述运动模型分别进行滤波；

所述机动目标为反辐射导弹，所述机动目标的位置量测信息为导弹相对观测雷达坐标系原点的距离、方位角和俯仰角，在所述步骤B中，利用去偏量测转换方法将所述位置量测信息转换为笛卡尔坐标量测；

在步骤C中，用量测信息中提供的机动目标的位置来代替起点，依据下式构造出近似的线性约束条件：

$C_k{x}_k=\&lsqb;0,1,0,-{\mathrm{tan\&delta;}}_k\&rsqb;\left[\begin{array}{c}{x}_k\\ {\stackrel{\&CenterDot;}{x}}_k\\ y_k\\ {\stackrel{\&CenterDot;}{y}}_k\end{array}\right]=0$

其中，C_k是k时刻包含线性等式约束条件的约束矩阵，x_k是状态向量，其中包含x_k和y_k为目标在X，Y方向上的位置；和为目标在X，Y方向上的速度，

δ_k是约束直线与Y轴夹角，和是k时刻X，Y方向的转换量测值，(x_n,y_n)是先验的目的地坐标，约束直线是其上的点都满足约束条件的直线。

2.根据权利要求1所述的方法，其特征在于，在所述步骤E中，针对各所述运动模型，分别采用不同的方法对X-Y平面和Z方向进行滤波，所述方法还包括将X-Y平面和Z方向的滤波预测结果合并的步骤。

3.根据权利要求2所述的方法，其特征在于，所述步骤E包括计算各运动模型对应的模型概率的步骤，以及按模型概率针对各所述运动的滤波结果进行加权融合的步骤。

4.根据权利要求3所述的方法，其特征在于，在所述步骤E中，针对X-Y平面，进行滤波包括如下的步骤：

A1、将k-1时刻滤波结果相互作用初值及其方差与转换量测值一起作为k时刻第j个模型的输入值，利用公式进行状态一步预测，其中，Φ_k-1为状态转移矩阵，

B1、计算状态一步预测协方差

$P_{k-1|k}^j={\mathrm{\&Phi;}}_{k-1}{P}_{k-1|k-1}^{j0}{\mathrm{\&Phi;}}_{k-1}^T+Q_{k-1}$

其中Q_k-1过程噪声协方差矩阵；

C1、确定状态一步预测约束投影值

${\hat{x}}_{k|k-1}^{p,j}={\hat{x}}_{k|k-1}^j-{\left(P_{k-1|k}^j\right)}^{-1}{C_k}^T{\&lsqb;C_k{\left(P_{k-1|k}^j\right)}^{-1}{C}_k\&rsqb;}^{-1}{C}_k{\hat{x}}_{k|k-1}^j$

其中C_k为约束矩阵

D1、确定量测预测

${\hat{z}}_{k|k-1}^j=H_k{\hat{x}}_{k|k-1}^{p,j}$

其中H_k为量测矩阵

E1、计算量测值与量测预测值的差

$v_k^j=z_k^c-{\hat{z}}_{k|k-1}^j$

其中为转换量测值

F1、计算量测预测协方差

$S_k^j=H_k{P}_{k|k-1}^j{H}_k^T+R_k^a$

其中为量测噪声协方差矩阵

G1、确定滤波增益K_k

$K_k=P_{k|k-1}^j{H}_k{\left(S_k^j\right)}^{-1}$

H1、进行状态更新，获得k时刻j模型滤波结果

${\hat{x}}_{k|k}^j={\hat{x}}_{k|k-1}^{p,j}+K_k^j{v}_k^j$

I1、进行协方差更新，获得k时刻j模型滤波误差协方差

$P_{k|k}^j=P_{k|k-1}^j-K_k^j{P}_{k,zz}^j{\left(K_k^j\right)}^T$

J1、在得到各个模型k时刻的输出和后，更新该时刻k各模型对应的概率

依据以下公式计算运动模型j的模型似然值：

${\mathrm{\&Lambda;}}_k^j=\frac{1}{\sqrt{\left|2{\mathrm{\&pi;S}}_k^j\right|}}\exp \&lsqb;-\frac{1}{2}{\left(v_k^j\right)}^{\&prime;}{\left(S_k^j\right)}^{-1}\left(v_k^j\right)\&rsqb;$

并且如下地对运动模型j的模型概率进行更新：

$u_k^j=\frac{{\mathrm{\&Lambda;}}_k^j\underset{i=1}{\overset{N}{\&Sigma;}}{\mathrm{\&rho;}}_{ij}{u}_{k-1}^i}{\underset{j=1}{\overset{N}{\&Sigma;}}\left({\mathrm{\&Lambda;}}_k^j\underset{i=1}{\overset{N}{\&Sigma;}}{\mathrm{\&rho;}}_{ij}{u}_{k-1}^i\right)}.$

5.根据权利要求4所述的方法，其特征在于，所述方法还包括基于k时刻针对各运动模型的滤波结果外推下一时刻预测结果，并根据k时刻模型概率对针对各运动模型的预测结果加权融合，得到k+1时刻融合预测结果的步骤。