CN104252178A - 一种基于强机动的目标跟踪方法 - Google Patents

Abstract

一种基于强机动的目标跟踪方法，包括以下步骤，参数初始化、模型输入交互、判断协方差矩阵、并行滤波、模型概率更新、模型输出交换、固定延迟平滑滤波、状态更新是否完成判断；在IMM算法的基础上，使用了重新计算权重的IMM算法，即RIMM，该方法不仅利用了模型概率，还充分利用了滤波协方差矩阵，使得跟踪精确度更高。另外，在滤波预测阶段使用SRCKF方法，它利用球形积分准则和径向积分准则。相比非线性滤波中使用较广泛的UKF算法，它优化了UKF中的sigma点采样策略和权重分配。同时，SRCKF中引入QR分解，避开了矩阵开方操作，提高了滤波的稳定性。在上述的基础上，本发明又引入了固定延迟平滑滤波，从而进一步提高了目标跟踪的实时性和准确性。

Description

一种基于强机动的目标跟踪方法

技术领域

本发明属于控制科学工程技术领域，涉及机动目标跟踪的模型匹配和非线性滤波技术，用于实现强机动目标的跟踪，以及使跟踪的稳定性和应对状态突变的跟踪鲁棒性得到了提高，尤其涉及一种基于强机动的目标跟踪方法。

背景技术

机动目标跟踪是状态估计、信息融合和目标跟踪领域的研究热点和难点之一。目标跟踪问题实际上就是目标状态的跟踪滤波问题，即根据传感器己获得的目标量测数据对目标状态进行精确的估计。目标跟踪的应用很广，在国防领域，可用于弹道导弹防御、空天预警、火力控制、拦截制导和低空突防等。在民用领域，则用于交通管制、海上监视、机器人的道路规划和障碍躲避、民用驾驶车的跟踪行驶和电子医学等。为此，几十年来许多科学家和工程师一直致力于该项课题的研究。

目标跟踪问题作为科学技术发展的一个方面，可以追溯到第二次世界大战前夕，即1937年世界上出现第一部跟踪雷达站SCR－28的时候。之后，各种雷达、红外、声纳和激光等目标跟踪***相继得到发展并且日趋完善。

由于运动目标的机动会使跟踪***的性能恶化，使得其研究相对困难。随着现代航空航天技术的飞速发展，各种飞行器的机动性能大幅提高，机动形式不再局限于加速度阶跃、蛇行机动等形式，也可能会出现加速度变化率的阶跃机动等更复杂的机动。目标的高机动使得跟踪滤波采用的目标动力学模型和机动目标实际动力学模型可能不匹配，导致跟踪滤波器发散，跟踪性能严重下降。因此，提高对高机动目标的跟踪性能便成为越来越重要的问题。

目前，国际上关于机动目标跟踪的研究虽然已取得了一系列成果，但由于机动出现的复杂性、随机性和多样性，不管在理论还是实践上，高速高机动目标跟踪都有较高的技术难度，仍然是一个具有挑战性的问题。

到目前为止，已经提出了很多分析目标跟踪的方法，但是大部分方法还存在一定的缺陷。建立目标的运动模型和自适应滤波是机动目标跟踪的两个关键部分。早期研究目标跟踪的算法，主要包括简单的目标模型和线性卡尔曼滤波方法。

目标模型研究的主要任务是建立一个符合实际的目标模型，以便为机动目标的滤波跟踪提供较为精确的符合实际情况的机动目标模型。经过国内外几十年的研究，已经得到了一系列经典的目标跟踪模型，归纳起来主要有：匀速(Constant Velocity)模型、匀加速(Constant Acceleration)模型、Singer模型、转弯模型(Coordinate Turn)、“当前”统计模型(Current Statistical Model)和jerk模型等。CV、CA模型是将目标的运动先验地定义为简单的匀速或匀加速运动，机动被看作是一种随机的输入，其大小体现在过程噪声的协方差矩阵中。目标无机动的情况下，有很好的跟踪效果。但实际上，目标保持匀速或匀加速运动的情况极少出现，当目标的运动特性复杂时，这两个模型就会带来很大的模型误差。CT模型的运动特点是目标的角速度和速度大小保持不变，而速度方向在时刻变化，因此是一种特殊的机动运动。该模型因需要目标机动的先验知识，所以在实际应用中很少单独使用，更多地是应用在多模型算法中。1969年，R.Singer提出了机动目标的零均值、一阶时间相关机动加速度模型(即Singer模型)，他将目标的机动加速度表示为随机状态噪声驱动的结果，而不是统计独立的白噪声，并由此建立起机动目标运动的统计模型。由于Singer模型对于机动加速度均值为零和机动加速度的概率密度函数服从均匀分布的假设一般是不符合实际的，因此，它只适用于匀速和匀加速范围内的目标运动。1983年，周宏仁教授提出了机动目标“当前”统计模型，认为当目标以某一加速度机动时，下一时刻的加速度取值是有限的且只能在“当前”加速度的邻域内，该模型本质上是一种非零均值的时间相关模型，其加速度“当前”概率密度用修正的瑞利分布描述，均值为“当前”加速度的预测值，随机机动加速度在时间轴上仍符合一阶时间相关过程。与Singer模型相比，“当前”统计模型考虑到当前时刻的具体机动，更为真实地反映了目标机动范围和强度变化，是一种实用的模型，比较适合于目标的实际机动。1997年Kishore提出了一种jerk模型，在目标机动模型的状态分量中加入了目标位置的三阶导数，即加速度的变化率或jerk，Kishore将jerk表示为一零均值的白噪声过程，以此可得到对加速度更加精确的估计，从而达到对机动目标的跟踪。由于其增加了一维状态分量，故计算量也随之增大。另外，在上述模型的基础上，Blom和Shalom提出的交互式多模型方法(IMM)通过建立一组模型集来并行跟踪目标，模型之间的切换假设服从马尔可夫过程，目标运动状态的最终估计是所有参与估计的模型滤波器输出的加权和。目前，IMM广泛应用于机动目标跟踪领域。

滤波算法是目标跟踪中另一个重要的组成部分，最经典的是卡尔曼滤波算法，卡尔曼滤波理论最先是由R.E.Kalman在1960年提出的，这是一种以无偏最小方差为最优准则，并采用递推算法的线性滤波理论，由递推方程随时间给出新的状态估计，计算量和存储量小，比较容易满足实时计算的要求。由于其是在线性高斯情况下利用最小均方误差准则获得目标状态的估计，但在实际中，许多情况下观测数据与目标动态参数间的关系是非线性的。由于机动目标跟踪问题在实际情况下的非线性，使得非线性***的状态估计无论在理论上还是在工程中都十分重要。从20世纪70年代起，非线性***的状态估计理论得到了很大发展，目前使用最广泛的非线性滤波算法有：扩展卡尔曼滤波(EKF)、不敏卡尔曼滤波(UKF)、粒子滤波(PF)、容积卡尔曼滤波(CKF)等。

目前，较常用的一种非线性滤波算法是EKF，但在处理强非线性***时，EKF会导致很大线性化误差，造成滤波精度降低，甚至发散。UKF算法通过采用一组确定的加权采样点来逼近随机变量的分布函数，从而避免了对非线性方程作近似线性化带来的误差和复杂雅克比矩阵的计算。当这组采样点通过非线性函数传播时，捕获非线性函数的统计特性，精度可达到二阶，但是UKF需要合理地选择参数才能达到较好的滤波效果。CKF是一种新型的非线性滤波算法，其利用数值积分原则对目标状态的后验概率进行近似，即采用一组等权值的容积点来计算后验概率密度。相比于EKF、UKF等算法，CKF算法具有更好的估计性能，并且其实现更为简单。然而CKF在递推过程中，存在计算量大及数值不稳定等缺点，为此借鉴平方根滤波的思想，提出了一种基于平方根滤波的容积卡尔曼滤波方法，即SRCKF。

发明内容

本发明的目的在于克服上述已有技术的不足，提出一种基于重新计算权重的固定延迟平方根容积滤波方法，该方法能够较好地实现对机动目标的跟踪，不仅能够很好地抑制滤波发散，而且能够使计算的复杂度降低，以及算法的稳定性和状态突变的跟踪鲁棒性得到提高，对进一步机动目标跟踪的实时性分析具有一定的意义。

本发明的技术方案是利用本发明提出的以重新计算权重为基础，以固定延迟平方根容积卡尔曼算法为非线性滤波算法，以CV、CA、CT构成的交互式多模型为目标模型，对机动目标进行跟踪。首先对模型的参数进行初始化，然后运用重新计算权重的交互式多模型算法(RIMM)，RIMM算法与IMM算法基本一样，最主要的不同是在输入交互和输出交互阶段，IMM算法使用模型概率作为权重对几个平行的滤波器的输入和输出进行处理，而RIMM算法同时使用模型概率和滤波协方差矩阵作为权重。在此基础上，运用平方根容积卡尔曼方法(SRCKF)为滤波算法，最后在RIMM融合输出状态估计之后，采用平滑技术，即固定延迟方法，进一步提高机动目标的跟踪性能。

为实现上述目的，本发明采用以下技术方案，一种基于强机动的目标跟踪方法，包括以下步骤：

(1)参数初始化：针对目标的运动情况选取相应的模型，然后对选取的对应模型的状态转移矩阵，量测矩阵，过程噪声协方差矩阵，量测噪声协方差矩阵，状态转移矩阵以及预测模型概率矩阵进行初始化；

(2)模型输入交互：选取经过初始化处理的模型j，并输入k时刻的初始状态，通过交互k-1时刻各模型滤波器的状态得到混合协方差矩阵值；计算公式如下：

${\hat{x}}_j^0(k-1|k-1)=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\hat{x}}_i(k-1|k-1){\mathrm{\ζ}}_{i|j}(k-1|k-1)$

ζ(k-1|k-1)＝μ_ij(k-1|k-1)M_0j(k-1)M_ij ^-1(k-1)F_j

式中，μ_i|j(k-1)＝P{m_i(k-1)|m_j(k),z^k-1}

$\begin{array}{c}{P}_j^0(k-1|k-1)=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}\mathrm{\ζ}(k-1|k-1)\{P_i(k-1|k-1)+[{\hat{x}}_i(k-1|k-1)-{\hat{x}}_j^0(k-1|k-1)]\×\\ {[{\hat{x}}_i(k-1|k-1)-{\hat{x}}_j^0(k-1|k-1)]}^T\};\end{array}$

μ_ij(k-1|k-1)是模型概率矩阵，F_j是状态转移矩阵，P是模型转移概率矩阵，ζ(k-1|k-1)是模型概率矩阵与滤波协方差矩阵的加权权重,z^k-1为观测矩阵，M_0j(k-1)为模型转移概率与协方差矩阵的混合权重，M_ij ^-1(k-1)为重新计算权重矩阵的逆矩阵，c_j为归一化参数，T代表转置矩阵；

(3)判断协方差矩阵：根据步骤(2)中求出的混合协方差矩阵值，即当该矩阵为实对称正定矩阵，即该矩阵满足正定性，继续下一步，当协方差矩阵不满足正定性，则返回到参数初始化过程中，从新迭代；

(4)并行滤波：根据步骤(2)中求出的k-1时刻模型初始条件，利用SRCKF滤波方法，计算出k时刻各个模型的状态估计和协方差阵P_j(k|k)；

(5)模型概率更新：利用步骤(2)中求出的k-1时刻的预测模型概率μ_ij(k-1)，并通过以下算式得到模型更新概率μ_j(k)；

运用下列算式计算模型更新概率μ_j(k)

${\mathrm{\μ}}_j\left(k\right)=\frac{{\mathrm{\Λ}}_j\left(k\right)}{c}\underset{l=1}{\overset{N}{\mathrm{\Σ}}}{p}_{\mathrm{lj}}{\mathrm{\μ}}_l(k-1)$

其中c为归一化常数， $c=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}\underset{l=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\Λ}}_j\left(k\right)p_{\mathrm{li}}{\mathrm{\μ}}_l(k-1)$

式中， ${\mathrm{\Λ}}_j\left(k\right)=\frac{1}{\sqrt{2\mathrm{\π}\left|{\mathrm{\Ω}}_j\left(k\right)\right|}}\exp \{-\frac{1}{2}{{\mathrm{\γ}}_j}^T\left(k\right){\mathrm{\Ω}}_j^{-1}\left(k\right){\mathrm{\γ}}_j\left(k\right)\};$ 为k时刻每个模型的似然函数，p_lj为模型转移概率矩阵；

(6)模型输出交互：利用组合各滤波器得到k时刻滤波器组的目标状态的最终估计及其协方差矩阵，具体如下：

$\hat{x}\left(k\right|k)=\underset{j=1}{\overset{N}{\mathrm{\Σ}}}{\hat{x}}_j\left(k\right|k){\mathrm{\ζ}}_j\left(k\right)$

$P\left(k\right|k)=\underset{j=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ζ}}_j\left(k\right)\{P_j\left(k\right|k)+[{\hat{x}}_j\left(k\right|k)-\hat{x}{\left(k\right|k)\left]\right[{\hat{x}}_j\left(k\right|k)-\hat{x}\left(k\right|k)]}^T\};$

(7)固定延迟平滑滤波：在步骤(6)的基础上，运用固定延迟平滑方法得出延迟d个采样时刻目标状态的延迟平滑估值和相应的误差协方差矩阵，具体如以下算式：

${\hat{x}}_{k-d|k}=E\left\{x_{k-d}\right|Z_k\}$

$P_{k-d|k}=E\left\{\right[x_{k-d}-{\hat{x}}_{k-d|k}\left]{[x_{k-d}-{\stackrel{T}{x}}_{k-d|k}]}^T\right|Z_k\}$

为此，对目标状态进行扩维，令：

${\hat{x}}_k=\begin{array}{cccc}{\left({\hat{x}}_k^{\left(0\right)}\right)}^T& {\left({\hat{x}}_k^{\left(1\right)}\right)}^T& \·\·\·& {\left({\hat{x}}_k^{\left(d\right)}\right)}^T\end{array}{]}^T$

式中： ${\hat{x}}_k^{\left(0\right)}=x_k,{\hat{x}}_k^{\left(1\right)}=x_{k-1},\·\·\·,{\hat{x}}_k^{\left(d\right)}=x_{k-d}$

在引入增强状态变量后，***的状态方程和观测方程改写为：

${\stackrel{\‾}{x}}_k=\stackrel{\‾}{f}\left({\stackrel{\‾}{x}}_{k-1}\right)+{\stackrel{\‾}{G}}_k{w}_{k-1}={\left[\begin{array}{cccc}{f}^{\left(0\right)}\left({\stackrel{\‾}{x}}_{k-1}\right)& f^{\left(1\right)}\left({\stackrel{\‾}{x}}_{k-1}\right)& \·\·\·& f^{\left(d\right)}\left({\stackrel{\‾}{x}}_{k-1}\right)\end{array}\right]}^T+{\stackrel{\‾}{G}}_k{w}_{k-1}$

$z_k=\stackrel{\‾}{h}\left({\stackrel{\‾}{x}}_k\right)+v_k$

式中： $f^{\left(0\right)}\left({\stackrel{\‾}{x}}_{k-1}\right)=f\left({\stackrel{\‾}{x}}_{k-1}^0\right),f^{\left(1\right)}\left({\stackrel{\‾}{x}}_{k-1}\right)={\stackrel{\‾}{x}}_{k-1}^0,f^{\left(d\right)}\left({\stackrel{\‾}{x}}_{k-1}\right)={\stackrel{\‾}{x}}_{k-1}^{(d-1)};{\stackrel{\‾}{G}}_k={\left[\begin{array}{cc}{G}_k^T& 0_{\mathrm{Nd}\×\left(\mathrm{Nd}\right)}\end{array}\right]}^t;$

***的过程噪声协方差矩阵修正为为扩展维度的控制矩阵，w_k-1为加速度区间，为扩展维度后的观测矩阵，v_k为观测噪声矩阵，z_k为观测值，E代表在|后面的基本条件下得到的结果值；

(8)状态更新是否完成判断：根据设定轨迹的运行时间来判定状态是否更新结束，若运行时间未结束，则返回步骤(4)继续通过并行滤波来获取状态的预测值，再由量测方程得到的量测值，对状态预测值进行修正，得到状态估计值，同时更新了协方差矩阵，一直到运行时间结束；若运行结束，则输出最终的滤波预测轨迹；

其中步骤(2)所述的是基于重新计算权重的混合状态估计值和协方差值：引入的计算公式如下：

${M_{0j}}^{-1}(k-1)=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\μ}}_{\mathrm{ij}}(k-1|k-1)M_{\mathrm{ij}}^{-1}(k-1)$

$M_{\mathrm{ij}}(k-1)=p_{\mathrm{ij}}/c_j{F}_j{P}_i(k-1|k-1)F_j^{-1}+Q_j$

式中，为归一化参数。M_0j(k-1)为模型转移概率与协方差矩阵的混合权重，M_ij ^-1(k-1)为重新计算权重矩阵的逆矩阵,F_j为状态转移矩阵，Q_j为过程噪声矩阵，μ_ij为模型概率，P_i为模型转移概率矩阵；

其中步骤(5)所述的模型更新概率公式中的测量新息γ_j(k)和协方差阵Ω_j(k)为：

$\left\{\begin{array}{c}{\mathrm{\γ}}_j\left(k\right)=z\left(k\right)-{\hat{z}}_j\left(k\right|k-1)\\ {\mathrm{\Ω}}_j\left(k\right)=S_{\mathrm{zz}}\left(k\right|k-1)S_{\mathrm{zz}}^T\left(k\right|k-1)\end{array}\right.$

其中方差阵平方根S_zz(k|k-1)由步骤(4)所述的平方根容积滤波方法所确定，z(k)为观测值，为观测预测值。

所述步骤(4)中的利用SRCKF滤波方法，具体步骤如下：

(4a)时间更新，先求容积点(i＝1,2,3…m)

式中，m＝2n，参数由下式给出：

传播后容积点为 $x_i^{*}\left(k\right|k-1)=f\left(x_i(k-1|k-1)\right)$

状态预测估计值为 $\hat{x}\left(k\right|k-1)=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{x}_i^{*}\left(k\right|k-1)$

预测误差方差阵的平方根为S(k|k-1)＝qr([X^*(k|k-1) S_Q(k)])

(4b)量测更新，先求容积点

计算传播后的容积点z_i(k|k-1)＝h(x_i(k|k-1))

(4c)测量预测 $\hat{z}\left(k\right|k-1)=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{z}_i\left(k\right|k-1)$

计算方差阵的平方根S_zz(k|k-1)＝qr([z(k|k-1) S_R(k)])

求互协方差阵P_xz(k|k-1)＝x(k|k-1)z^T(k|k-1)

(4d)增益阵为 $K\left(k\right)=P_{\mathrm{xz}}\left(k\right|k-1)\·{\left[S_{\mathrm{zz}}^T\left(k\right|k-1)\right]}^{-1}\×S_{\mathrm{zz}}{\left(k\right|k-1)}^{-1}$

(4e)更新状态 $\hat{x}\left(k\right|k)=\hat{x}\left(k\right|k-1)+K\left(k\right)[z\left(k\right)-\hat{z}\left(k\right|k-1)]$

更新方差阵的平方根S(k|k)＝qr([x(k|k-1)-K(k)z(k|k-1) K(k)S_R(k)])。

所述步骤(7)中的扩维状态估值及对应的误差协方差矩阵是利用SRCKF算法对进行滤波，在滤波实现过程中，采用扩维之后的状态初始值和协方差初始值替代原来的初始值；另外，用扩维后的状态转移函数，观测矩阵函数，噪声矩阵替换原来的对应函数；

具体算式如下：

${\widehat{\stackrel{\‾}{x}}}_{k|k}=E\left\{{\hat{x}}_k\right|Z_k\}$

${\stackrel{\‾}{P}}_{k|k}=E\left\{({\stackrel{\‾}{x}}_k-{\widehat{\stackrel{\‾}{x}}}_{k|k}){({\stackrel{\‾}{x}}_k-{\widehat{\stackrel{\‾}{x}}}_{k|k})}^T\right|Z_k\}$

可得固定延迟平滑估值及相应的误差协方差矩阵，即：

${\hat{x}}_{k-l|k}={\widehat{\stackrel{\‾}{x}}}_{k|k}^{\left(l\right)}=E\left\{{\stackrel{\‾}{x}}_k^l\right|Z_k\}$

$P_{k-l|k}={\stackrel{\‾}{P}}_{k|k}^{(l,l)}=E\left\{({\stackrel{\‾}{x}}_k^{\left(l\right)}-{\widehat{\stackrel{\‾}{x}}}_{k|k}^{\left(l\right)}){({\stackrel{\‾}{x}}_k^{\left(l\right)}-{\widehat{\stackrel{\‾}{x}}}_{k|k}^{\left(l\right)})}^T\right|Z_k\},l=\mathrm{0,1},\·\·\·d,$ E代表在|后面的基本条件下得到的结果值。

本发明采用上述技术方案，具有以下优点：在IMM算法的基础上，使用了重新计算权重的IMM算法，即RIMM，该方法不仅利用了模型概率，还充分利用了滤波协方差矩阵，使得跟踪精确度更高。另外，在滤波预测阶段使用SRCKF方法，它利用球形积分准则和径向积分准则。相比非线性滤波中使用较广泛的UKF算法，它优化了UKF中的sigma点采样策略和权重分配。同时，SRCKF中引入QR分解，避开了矩阵开方操作，提高了滤波的稳定性。在上述的基础上，本发明又引入了固定延迟平滑滤波，从而进一步提高了目标跟踪的实时性和准确性。

以下将结合附图对本发明做进一步详细说明。

附图说明

图1是本发明的流程框图；

图2是本发明实施例中目标跟踪真实轨迹示意图；

图3是本发明实施例中目标跟踪滤波轨迹结果图；

图4是本发明实施例中滤波轨迹的位置均方根误差结果图；

图5是本发明实施例中滤波轨迹的速度均方根误差结果图。

具体实施方式

下面结合附图和实施例进一步对一种基于强机动的目标跟踪方法进行详细的说明。

与IMM算法类似，RIMM算法也分为四步。即输入交互，滤波，模型概率更新，输出交互。设m_i(k)代表在k时刻，模型m_i和***模型匹配，μ_i(k)是第i个模型(i＝1,2,3,…,N)在时间间隔(k-1,k)内起作用的概率。因此有

${\mathrm{\μ}}_i\left(k\right)=P\left\{m_i\left(k\right)\right|z^k\}$

式中，z^k表示k时刻以前的所有观测的集合，假设各个模型之间的转移符合Markov规律，则模型转移概率可以表示为：

P_ij＝P{m_j(k)|m_i(k-1)}

记k时刻用第j个模型滤波得到的状态估计和相应的协方差矩阵分别为P_j(k|k)。RIMM-SRCKF-FLS方法主要由以下9个步骤组成。

如图1所示，本发明的具体步骤如下：

(1)参数初始化：选取模型，并对选取的模型的状态转移矩阵，量测矩阵，过程噪声协方差矩阵，量测噪声协方差矩阵，状态转移矩阵以及预测模型概率矩阵进行初始化；选取的目标如强机动目标可以选取转弯模型，当前统计模型等，弱机动目标可以选取匀速模型和匀加速模型，本实施例主要针对一个蛇形机动轨迹，选用匀速运动模型(CV)，匀加速运动模型(CA)，转弯模型(CT)构成交互式多模型结构。初始化各个运动模型参数；

(2)模型输入交互：选取经过初始化处理的模型j，并输入k时刻的初始状态，通过交互k-1时刻各模型滤波器的状态得到混合协方差矩阵值；计算公式如下：

${\hat{x}}_j^0(k-1|k-1)=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\hat{x}}_i(k-1|k-1){\mathrm{\ζ}}_{i|j}(k-1|k-1)$

ζ(k-1|k-1)＝μ_ij(k-1|k-1)M_0j(k-1)M_ij ^-1(k-1)F_j

式中，μ_ij(k-1)＝P{m_i(k-1)|m_j(k),z^k-1}

${M_{0j}}^{-1}(k-1)=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\μ}}_{\mathrm{ij}}(k-1|k-1)M_{\mathrm{ij}}^{-1}(k-1)$

$M_{\mathrm{ij}}(k-1)=p_{\mathrm{ij}}/c_j{F}_j{P}_i(k-1|k-1)F_j^{-1}+Q_j$

式中， $c_j=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{p}_{\mathrm{ij}}{\mathrm{\μ}}_i(k-1)$

$\begin{array}{c}{P}_j^0(k-1|k-1)=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}\mathrm{\ζ}(k-1|k-1)\{P_i(k-1|k-1)+[{\hat{x}}_i(k-1|k-1)-{\hat{x}}_j^0(k-1|k-1)]\×\\ {[{\hat{x}}_i(k-1|k-1)-{\hat{x}}_j^0(k-1|k-1)]}^T\};\end{array}$

μ_ij(k-1|k-1)是模型概率矩阵，F_j是状态转移矩阵，P是模型转移概率矩阵，ζ(k-1|k-1)是模型概率矩阵与滤波协方差矩阵的加权权重,z^k-1为观测矩阵，M_0j(k-1)为模型转移概率与协方差矩阵的混合权重，M_ij ^-1(k-1)为重新计算权重矩阵的逆矩阵，c_j为归一化参数，T代表转置矩阵；

在输入交互阶段，采用重新计算权重的方法，即在模型交互输入阶段，采用重新获得的权重因子得到目标的混合初始状态估计值和混合协方差初始值，同时获得模型预测概率和混合权重。由于接下来要对状态预测值和协方差矩阵进行固定延迟平滑滤波，因而同时对模型状态变量进行扩维处理，获得扩维之后的目标运动模型状态变量；

(3)判断协方差矩阵：根据步骤(2)中求出的混合协方差矩阵值，即若该矩阵为实对称正定矩阵，即该矩阵满足正定性，实对称矩阵的判定条件为其所有特征值必须大于零。否则协方差矩阵不满足正定性，则在参数初始化过程中，重新调整协方差矩阵的初始值，可以通过同比例增大矩阵值来调节。在这里，判断协方差矩阵是否正定的主要目的是为了防止在步骤(4)中滤波发散，从而导致目标跟踪丢失。

(4)并行滤波：根据步骤(2)中求出的k-1时刻模型初始条件，利用SRCKF滤波方法，计算出k时刻各个模型的状态估计和协方差阵P_j(k|k)；具体是按照以下步骤进行的：

(a)时间更新，先求容积点(i＝1,2,3…m)

式中，m＝2n，参数由下式给出：

传播后容积点为 $x_i^{*}\left(k\right|k-1)=f\left(x_i(k-1|k-1)\right)$

状态预测估计值为 $\hat{x}\left(k\right|k-1)=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{x}_i^{*}\left(k\right|k-1)$

预测误差方差阵的平方根为S(k|k-1)＝qr([X^*(k|k-1) S_Q(k)])

(b)量测更新，先求容积点

计算传播后的容积点z_i(k|k-1)＝h(x_i(k|k-1))

(c)测量预测 $\hat{z}\left(k\right|k-1)=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{z}_i\left(k\right|k-1)$

计算方差阵的平方根S_zz(k|k-1)＝qr([z(k|k-1) S_R(k)])

求互协方差阵P_xz(k|k-1)＝x(k|k-1)z^T(k|k-1)

(d)增益阵为 $K\left(k\right)=P_{\mathrm{xz}}\left(k\right|k-1)\·{\left[S_{\mathrm{zz}}^T\left(k\right|k-1)\right]}^{-1}\×S_{\mathrm{zz}}{\left(k\right|k-1)}^{-1}$

(e)更新状态 $\hat{x}\left(k\right|k)=\hat{x}\left(k\right|k-1)+K\left(k\right)[z\left(k\right)-\hat{z}\left(k\right|k-1)]$

更新方差阵的平方根S(k|k)＝qr([x(k|k-1)-K(k)z(k|k-1) K(k)S_R(k)])；

主要是在上述步骤的基础上，在滤波预测阶段使用平方根容积卡尔曼滤波方法。本实施例中的状态维数为6维，则容积点数选为12，权值为容积点数的倒数。通过滤波预测之后，得到预测误差方差阵的平方根，从而确定渐消因子，然后由渐消因子决定是否回溯到上一步，即由渐消因子自适应调整预测误差协方差。接着由得到的滤波增益值，最后获得状态估值和协方差更新值，以及预测误差方差阵的平方根更新值。

(5)模型概率更新：利用步骤(2)中求出的k-1时刻的预测模型概率μ_ij(k-1)，并通过以下算式得到模型更新概率μ_j(k)；

首先计算出测量新息γ_j(k)和协方差阵Ω_j(k)

$\left\{\begin{array}{c}{\mathrm{\γ}}_j\left(k\right)=z\left(k\right)-{\hat{z}}_j\left(k\right|k-1)\\ {\mathrm{\Ω}}_j\left(k\right)=S_{\mathrm{zz}}\left(k\right|k-1)S_{\mathrm{zz}}^T\left(k\right|k-1)\end{array}\right.$

然后运用下列算式计算模型更新概率μ_j(k)

${\mathrm{\μ}}_j\left(k\right)=\frac{{\mathrm{\Λ}}_j\left(k\right)}{c}\underset{l=1}{\overset{N}{\mathrm{\Σ}}}{p}_{\mathrm{lj}}{\mathrm{\μ}}_l(k-1)$

其中c为归一化常数， $c=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}\underset{l=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\Λ}}_j\left(k\right)p_{\mathrm{li}}{\mathrm{\μ}}_l(k-1)$

式中， ${\mathrm{\Λ}}_j\left(k\right)=\frac{1}{\sqrt{2\mathrm{\π}\left|{\mathrm{\Ω}}_j\left(k\right)\right|}}\exp \{-\frac{1}{2}{{\mathrm{\γ}}_j}^T\left(k\right){\mathrm{\Ω}}_j^{-1}\left(k\right){\mathrm{\γ}}_j\left(k\right)\};$ 为k时刻每个模型的似然函数，p_li为模型概率转移矩阵。

(6)模型输出交换：利用组合各滤波器得到k时刻滤波器组的目标状态的最终估计及其协方差矩阵，具体如下：

$\hat{x}\left(k\right|k)=\underset{j=1}{\overset{N}{\mathrm{\Σ}}}{\hat{x}}_j\left(k\right|k){\mathrm{\ζ}}_j\left(k\right)$

$P\left(k\right|k)=\underset{j=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ζ}}_j\left(k\right)\{P_j\left(k\right|k)+[{\hat{x}}_j\left(k\right|k)-\hat{x}{\left(k\right|k)\left]\right[{\hat{x}}_j\left(k\right|k)-\hat{x}\left(k\right|k)]}^T\};$

在模型交互输出阶段前，由采用的重新计算权重的权重因子，得到各滤波器的融合状态估计值和协方差估计值；

(7)固定延迟平滑滤波：在步骤(6)的基础上，运用固定延迟平滑方法得出延迟d个采样时刻目标状态的延迟平滑估值和相应的误差协方差矩阵，具体如以下算式：

${\hat{x}}_{k-d|k}=E\left\{x_{k-d}\right|Z_k\}$

$P_{k-d|k}=E\left\{\right[x_{k-d}-{\hat{x}}_{k-d|k}\left]{[x_{k-d}-{\stackrel{T}{x}}_{k-d|k}]}^T\right|Z_k\}$

为此，对目标状态进行扩维，令：

${\hat{x}}_k=\begin{array}{cccc}{\left({\hat{x}}_k^{\left(0\right)}\right)}^T& {\left({\hat{x}}_k^{\left(1\right)}\right)}^T& \·\·\·& {\left({\hat{x}}_k^{\left(d\right)}\right)}^T\end{array}{]}^T$

式中： ${\hat{x}}_k^{\left(0\right)}=x_k,{\hat{x}}_k^{\left(1\right)}=x_{k-1},\·\·\·,{\hat{x}}_k^{\left(d\right)}=x_{k-d}$

在引入增强状态变量后，***的状态方程和观测方程改写为：

${\stackrel{\‾}{x}}_k=\stackrel{\‾}{f}\left({\stackrel{\‾}{x}}_{k-1}\right)+{\stackrel{\‾}{G}}_k{w}_{k-1}={\left[\begin{array}{cccc}{f}^{\left(0\right)}\left({\stackrel{\‾}{x}}_{k-1}\right)& f^{\left(1\right)}\left({\stackrel{\‾}{x}}_{k-1}\right)& \·\·\·& f^{\left(d\right)}\left({\stackrel{\‾}{x}}_{k-1}\right)\end{array}\right]}^T+{\stackrel{\‾}{G}}_k{w}_{k-1}$

$z_k=\stackrel{\‾}{h}\left({\stackrel{\‾}{x}}_k\right)+v_k$

式中： $f^{\left(0\right)}\left({\stackrel{\‾}{x}}_{k-1}\right)=f\left({\stackrel{\‾}{x}}_{k-1}^0\right),f^{\left(1\right)}\left({\stackrel{\‾}{x}}_{k-1}\right)={\stackrel{\‾}{x}}_{k-1}^0,f^{\left(d\right)}\left({\stackrel{\‾}{x}}_{k-1}\right)={\stackrel{\‾}{x}}_{k-1}^{(d-1)};{\stackrel{\‾}{G}}_k={\left[\begin{array}{cc}{G}_k^T& 0_{\mathrm{Nd}\×\left(\mathrm{Nd}\right)}\end{array}\right]}^t;$ ***的过程噪声协方差矩阵修正为为扩展维度的控制矩阵，w_k-1为加速度区间，为扩展维度后的观测矩阵，v_k为观测噪声矩阵，z_k为观测值，E代表在|后面的基本条件下得到的结果值；

应用步骤(4)的SRCKF算法对进行滤波，在滤波实现过程中，采用扩维之后的状态初始值和协方差初始值替代原来的初始值；另外，用扩维后的状态转移函数，观测矩阵函数，噪声矩阵替换原来的对应函数；

应用SRCKF滤波方法可得到目标的扩维状态估值及对应的误差协方差矩阵即

${\widehat{\stackrel{\‾}{x}}}_{k|k}=E\left\{{\hat{x}}_k\right|Z_k\}$

${\stackrel{\‾}{P}}_{k|k}=E\left\{({\stackrel{\‾}{x}}_k-{\widehat{\stackrel{\‾}{x}}}_{k|k}){({\stackrel{\‾}{x}}_k-{\widehat{\stackrel{\‾}{x}}}_{k|k})}^T\right|Z_k\}$

可得固定延迟平滑估值及相应的误差协方差矩阵，即：

${\hat{x}}_{k-l|k}={\widehat{\stackrel{\‾}{x}}}_{k|k}^{\left(l\right)}=E\left\{{\stackrel{\‾}{x}}_k^l\right|Z_k\}$

$P_{k-l|k}={\stackrel{\‾}{P}}_{k|k}^{(l,l)}=E\left\{({\stackrel{\‾}{x}}_k^{\left(l\right)}-{\widehat{\stackrel{\‾}{x}}}_{k|k}^{\left(l\right)}){({\stackrel{\‾}{x}}_k^{\left(l\right)}-{\widehat{\stackrel{\‾}{x}}}_{k|k}^{\left(l\right)})}^T\right|Z_k\},l=\mathrm{0,1},\·\·\·d;$

E代表在|后面的基本条件下得到的结果值；

(8)状态更新是否完成判断：根据设定轨迹的运行时间来判定状态是否更新结束，若运行时间未结束，则返回步骤(4)继续通过并行滤波来获取状态的预测值，再由量测方程得到的量测值，对状态预测值进行修正，得到状态估计值，同时更新了协方差矩阵，一直到运行时间结束；若运行结束，则输出最终的滤波预测轨迹。

本发明的效果可以通过以下实验进一步说明：

1.仿真条件：

在CPU为core22.4GHZ、内存2G、WINDOWS XP***上使用Matlab进行了仿真。

2.仿真内容：

选取一个蛇形机动目标轨迹，目标轨迹的运动时间为50秒，Monte Carlo仿真次数为100，采样间隔为T＝1秒。仿真中，我们目标的起始状态设为：目标运动的具体过程为：x₀＝[x,v_x,a_x,y,v_y,a_y]^T，即对应的具体值x₀＝＝[29.8km,-250m/s,0,35km,-215m/s,0]^T，模型初始化概率μ₀＝[0.60.20.2]状态转移矩阵为 $\mathrm{\π}=\left[\begin{array}{ccc}0.8& 01& 0.1\\ 0.1& 0.8& 0.1\\ 0.1& 0.1& 0.8\end{array}\right],$ 量测噪声方差阵为R＝150I，过程噪声方差阵为Q＝25I。如图2所示，目标轨迹的真实状态及速度和加速度变化情况。表1所示为目标机动过程，表2所示为各方法的相对运行时间。

实验中，我们选取的状态维数为6维，则容积点数为m＝2·6＝12，权值为ω＝112，选取d＝3个采样时刻。实验结果如图3所示，目标预测轨迹与真实轨迹吻合，即实现了机动目标的跟踪。

由实验结果图4和图5可知，采用本发明中提出的目标跟踪方法，克服了传统方法的不足，不仅可以实时准确地跟踪目标轨迹，还可以使状态突变的跟踪鲁棒性得到提高，同时增强了滤波的收敛速度和数值稳定性。由表2可知，使用基于EKF滤波的目标跟踪方法与加入固定延迟平滑的方法相比，后者复杂度更高。而基于固定延迟的目标跟踪方法与本发明提出的方法相比，由于本发明采用SRCKF滤波方法，使得复杂度明显降低。通过三个方法整体比较可知，固定延迟平滑方法增加了复杂度，但SRCKF使得复杂度降低。因此，采用本发明提出的方法，虽在复杂度上增加了一些，但整体跟踪精度明显提高。

表1 目标机动状态

时间(s)	运动状态
		0-9	目标为匀速运动
10-22	目标为左旋转运动
		23-25	目标为匀速运动
26-38	目标为右旋转运动
		29-50	目标为匀速运动

表2 各方法相对运行时间

RIMM	RIMM-FL	RIMM-SRCKF-FL
			1.1070	2.5438	1.3627

上述实施方式仅是本发明的一个实例，不构成对本发明的任何限制，例如用本发明方法还可以对计算机生成的具有不同机动情况的轨迹进行跟踪，以及对实际中的目标轨迹进行跟踪。

以上例举仅仅是对本发明的举例说明，并不构成对本发明的保护范围的限制，凡是与本发明相同或相似的设计均属于本发明的保护范围之内。本实施例没有详细叙述的部件和结构属本行业的公知部件和常用结构或常用手段，这里不一一叙述。

Claims (5)

1.一种基于强机动的目标跟踪方法，其特征在于，包括以下步骤： 

(1)参数初始化：针对目标的运动情况选取相应的模型，然后对选取的对应模型的状态转移矩阵，量测矩阵，过程噪声协方差矩阵，量测噪声协方差矩阵，状态转移矩阵以及预测模型概率矩阵进行初始化； 

(2)模型输入交互：选取经过初始化处理的模型j，并输入k时刻的初始状态，通过交互k-1时刻各模型滤波器的状态得到混合状态初始值 和混合协方差矩阵值计算公式如下： 

ζ(k-1|k-1)＝μ_ij(k-1|k-1)M_0j(k-1)M_ij ^-1(k-1)F_j

式中，μ_ij(k-1)＝P{m_i(k-1)|m_j(k),z^k-1} 

μ_ij(k-1|k-1)是模型概率矩阵，F_j是状态转移矩阵，P是模型转移概率矩阵，ζ(k-1|k-1)是模型概率矩阵与滤波协方差矩阵的加权权重,z^k-1为观测矩阵，M_0j(k-1)为模型转移概率与协方差矩阵的混合权重，M_ij ^-1(k-1)为重新计算权重矩阵的逆矩阵，c_j为归一化参数，T代表转置矩阵； 

(3)判断协方差矩阵：根据步骤(2)中求出的混合协方差矩阵值，即 当该矩阵为实对称正定矩阵，即该矩阵满足正定性，继续下一步，当协方差矩阵不满足正定性，则返回到参数初始化过程中，从新迭代； 

(4)并行滤波：根据步骤(2)中求出的k-1时刻模型初始条件，利用SRCKF滤波方法，计算出k时刻各个模型的状态估计和协方差阵P_j(k|k)； 

(5)模型概率更新：利用步骤(2)中求出的k-1时刻的预测模型概率 

μ_ij(k-1)，并通过以下算式得到模型更新概率μ_j(k)； 

运用下列算式计算模型更新概率μ_j(k) 

其中c为归一化常数，

式中， 为k时刻每个模型的似然函数，p_lj为模型转移概率矩阵。 

(6)模型输出交换：利用组合各滤波器得到k时刻滤波器组的目标状态的最终估计及其协方差矩阵，具体如下： 

(7)固定延迟平滑滤波：在步骤(6)的基础上，运用固定延迟平滑方法得出延迟d个采样时刻目标状态的延迟平滑估值和相应的误差协方差矩阵，具体如以下算式： 

为此，对目标状态进行扩维，令： 

式中：

在引入增强状态变量后，***的状态方程和观测方程改写为： 

式中： ***的过程噪声协方差矩阵修正为为扩展维度的控制矩阵，w_k-1为加速度区间，为扩展维度后的观测矩阵，v_k 为观测噪声矩阵，z_k为观测值，d代表延迟的采样时刻，E代表在|后面的基本条件下得到的结果值； 

(8)状态更新是否完成判断：根据设定轨迹的运行时间来判定状态是否更新结束，若运行时间未结束，则返回步骤(4)继续通过并行滤波来获取状态的预测值，再由量测方程得到的量测值，对状态预测值进行修正，得到状态估计值，同时更新了协方差矩阵，一直到运行时间结束；若运行结束，则输出最终的滤波预测轨迹。 

2.根据权利要求1所述的一种基于强机动的目标跟踪方法，其特征在于，其中步骤(2)所述的是基于重新计算权重的混合状态估计值和协方差值：引入的计算公式如下： 

式中，为归一化参数。M_0j(k-1)为模型转移概率与协方差矩阵的混合权重，M_ij ^-1(k-1)为重新计算权重矩阵的逆矩阵,F_j为状态转移矩阵，Q_j为过程噪声矩阵，μ_ij为模型概率，P_i为模型转移概率矩阵。 

3.根据权利要求1所述的基于强机动的目标跟踪方法，其特征在于，所述的步骤(5)中的模型更新概率公式中的测量新息γ_j(k)和协方差阵Ω_j(k)为： 

其中方差阵平方根S_zz(k|k-1)由步骤(4)所述的平方根容积滤波方法所确定,z(k)为观测值，为观测预测值。 

4.根据权利要求1所述的基于强机动的目标跟踪方法，其特征在于，所述步骤(4)中的利用SRCKF滤波方法，具体步骤如下： 

(4a)时间更新，先求容积点(i＝1,2,3…m) 

式中，m＝2n，参数由下式给出： 

传播后容积点为

状态预测估计值为

预测误差方差阵的平方根为S(k|k-1)＝qr([X^*(k|k-1) S_Q(k)]) 

(4b)量测更新，先求容积点

计算传播后的容积点z_i(k|k-1)＝h(x_i(k|k-1)) 

(4c)测量预测

计算方差阵的平方根S_zz(k|k-1)＝qr([z(k|k-1) S_R(k)]) 

求互协方差阵P_xz(k|k-1)＝x(k|k-1)z^T(k|k-1) 

(4d)增益阵为

(4e)更新状态

更新方差阵的平方根S(k|k)＝qr([x(k|k-1)-K(k)z(k|k-1) K(k)S_R(k)])。 

5.根据权利要求1所述的基于强机动的目标跟踪方法，其特征在于，所述步骤(7)中的扩维状态估值及对应的误差协方差矩阵是利用SRCKF算法对进行滤波，在滤波实现过程中，采用扩维之后的状态初始值和协方差初始值替代原来的初始值；另外，用扩维后的状态转移函数，观测矩阵函数，噪声矩阵替换原来的对应函数； 

具体算式如下： 

可得固定延迟平滑估值及相应的误差协方差矩阵，即： 

E代表在|后面的基本条件下得到的结果值。