CN103955600A - 一种目标跟踪方法及截断积分卡尔曼滤波方法、装置 - Google Patents

Abstract

本发明公开了一种目标跟踪方法、系统及截断积分卡尔曼滤波方法、装置，该截断积分卡尔曼滤波方法包括根据高斯-厄米特积分获取目标状态的原始先验概率密度函数；根据原始先验概率密度函数获取目标状态的第一后验概率密度函数；根据当前目标观测时刻的目标观测向量修正原始先验概率密度函数，以获取修正先验概率密度函数；根据修正先验概率密度函数获取目标状态的第二后验概率密度函数；根据第一后验概率密度函数以及第二后验概率密度函数获取目标状态的联合后验概率密度函数。本发明能够有效减少目标状态先验分布方差，自适应地根据观测信息的精度进行状态更新，有效提高滤波精度且实用性较高。

Description

一种目标跟踪方法及截断积分卡尔曼滤波方法、装置

技术领域

本发明涉及非线性滤波领域，特别是涉及一种目标跟踪方法、系统及截断积分卡尔曼滤波方法、装置。

背景技术

被动传感器（如红外、声纳等）本身不发射电磁波，其通过接收以目标为载体的发动机、通信、雷达等所辐射的红外线、电磁波，或目标所反射的外来电磁波等来探测目标的位置等信息。通常采用多个被动传感器即组成一被动传感器阵列对同一目标进行观测以实现对目标的跟踪。

针对被动传感器阵列中的目标跟踪问题，现有技术主要采用如下几种方法：第一种为通过自适应调整目标的状态模型，以实现对目标的准确跟踪，该类方法例如有交互多模型（IMM）方法。第二种为基于粒子滤波的交互多模型跟踪算法，例如多模Rao-Blackwellized粒子滤波方法，该方法将机动目标跟踪问题划分为模型选择和目标跟踪两个子问题。

本申请发明人在长期研发中发现，现有技术中第一种方法当目标突然机动时，其不能及时对机动目标进行检测，导致目标机动时跟踪性能降低，从而可能出现目标丢失的情况；基于粒子滤波的第二种方法会随着目标状态维数的增加，粒子维数和计算量也随之增加，一般很难实际应用。另外，采用上述两种方法对目标进行跟踪时，当目标机动时，由于运动模型的不准确以及观测误差的存在，使得目标的预测误差迅速增大，从而造成目标状态先验分布方差增大，目标跟踪性能变差。

发明内容

本发明主要解决的技术问题是提供一种目标跟踪方法、系统及截断积分卡尔曼滤波方法、装置，能够有效减少目标状态先验分布方差，自适应地根据观测信息的精度进行状态更新，有效提高滤波精度且实用性较高。

为解决上述技术问题，本发明的第一方面是：提供一种目标跟踪方法，包括：对目标进行观测以获得当前目标观测时刻的目标观测向量；根据高斯-厄米特积分获取当前目标观测时刻目标状态的原始先验概率密度函数；根据原始先验概率密度函数获取当前目标观测时刻目标状态的第一后验概率密度函数；根据当前目标观测时刻的目标观测向量修正原始先验概率密度函数，以获取修正先验概率密度函数；根据修正先验概率密度函数获取当前目标观测时刻目标状态的第二后验概率密度函数；根据第一后验概率密度函数以及第二后验概率密度函数获取当前目标观测时刻目标状态的联合后验概率密度函数；利用目标状态的联合后验概率密度函数对目标状态进行估计，以获得当前目标观测时刻的目标状态估计值；输出当前目标观测时刻的目标状态估计值，以实现对目标的跟踪。

为解决上述技术问题，本发明的第二方面是：提供一种目标跟踪系统，包括：被动传感器阵列以及截断积分卡尔曼滤波装置，被动传感器阵列连接至截断积分卡尔曼滤波装置，被动传感器阵列由多个被动传感器组成，被动传感器阵列用于对目标进行观测以获得当前目标观测时刻的目标观测向量；截断积分卡尔曼滤波装置包括：原始先验概率密度函数获取模块，用于根据高斯-厄米特积分获取当前目标观测时刻目标状态的原始先验概率密度函数；第一后验概率密度函数获取模块，用于根据原始先验概率密度函数获取当前目标观测时刻目标状态的第一后验概率密度函数；修正先验概率密度函数获取模块，用于根据当前目标观测时刻的目标观测向量修正原始先验概率密度函数，以获取修正先验概率密度函数；第二后验概率密度函数获取模块，用于根据修正先验概率密度函数获取当前目标观测时刻目标状态的第二后验概率密度函数；联合后验概率密度函数获取模块，用于根据第一后验概率密度函数以及第二后验概率密度函数获取当前目标观测时刻目标状态的联合后验概率密度函数；目标状态估计模块，用于利用目标状态的联合后验概率密度函数对目标状态进行估计，以获得当前目标观测时刻的目标状态估计值；目标状态估计值输出模块，用于输出当前目标观测时刻的目标状态估计值。

为解决上述技术问题，本发明的第三方面是：提供一种截断积分卡尔曼滤波方法，包括：根据高斯-厄米特积分获取当前目标观测时刻目标状态的原始先验概率密度函数；根据原始先验概率密度函数获取当前目标观测时刻目标状态的第一后验概率密度函数；根据当前目标观测时刻的目标观测向量修正原始先验概率密度函数，以获取修正先验概率密度函数；根据修正先验概率密度函数获取当前目标观测时刻目标状态的第二后验概率密度函数；根据第一后验概率密度函数以及第二后验概率密度函数获取当前目标观测时刻目标状态的联合后验概率密度函数，完成截断积分卡尔曼滤波过程。

其中，根据高斯-厄米特积分获取当前目标观测时刻目标状态的原始先验概率密度函数的步骤具体包括：根据高斯-厄米特积分获取m个积分点具体如下式所示：

$X_{i,k-1|k-1}={\left(\sqrt{P_{k-1|k-1}}\right)}^T{\mathrm{\ξ}}_i+{\hat{x}}_{k-1|k-1}---\left(1\right)$

其中，为均值P_k-1|k-1为协方差，ξ_i表示高斯-厄米特积分点，i=1,2,...,m；

将积分点代入非线性的过程函数，以获取积分点对应的m个预测值具体如下式所示：

$X_{0,k|k-1}^i=f\left(X_{i,k-1/k-1}\right)---\left(2\right)$

其中，f(·)为非线性的过程函数；

根据积分点对应的m个预测值获取k时刻目标状态的原始先验概率密度函数对应的均值和协方差，具体如下所示：

${\hat{x}}_{p,0,k|k-1}=\underset{i=1}{\overset{N_s}{\mathrm{\Σ}}}{w}_i{X}_{0,k|k-1}^i---\left(3\right)$

$P_{p,0,k|k-1}=Q_k+\underset{i=1}{\overset{N_s}{\mathrm{\Σ}}}{w}_i(X_{0,k|k-1}^i-{\hat{x}}_{p,0,k|k-1}){(X_{0,k|k-1}^i-{\hat{x}}_{p,0,k|k-1})}^T---\left(4\right)$

其中，为积分点对应的权值，Q_k为过程噪声向量的协方差，k时刻表示当前目标观测时刻，为k时刻目标状态的原始先验概率密度函数的均值，P_p,0,k|k-1为k时刻目标状态的原始先验概率密度函数的协方差。

其中，根据原始先验概率密度函数获取当前目标观测时刻目标状态的第一后验概率密度函数的步骤具体包括：根据均值和协方差P_p,0,k|k-1获取m个积分点具体如下式所示：

$X_{i,0,k|k-1}={\left(\sqrt{P_{p,0,k|k-1}}\right)}^T{\mathrm{\ξ}}_i+{\hat{x}}_{p,0,k|k-1}---\left(5\right)$

将积分点代入非线性的观测函数，以获取积分点对应的m个预测值具体如下式所示：

$z_{0,k|k-1}^i=h\left(X_{i,0,k|k-1}\right)---\left(6\right)$

其中，h(·)为非线性的观测函数；

获取积分点对应的m个预测值的均值具体如下式所示：

${\hat{z}}_{0,k|k-1}=\underset{i=1}{\overset{N_s}{\mathrm{\Σ}}}{w}_i{z}_{0,k|k-1}^i---\left(7\right)$

根据预测值和均值获取对应的新息协方差以及交叉协方差，具体如下所示：

$P_{\mathrm{zz},0,k|k-1}=R_k+\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{w}_i(z_{0,k|k-1}^i-{\hat{z}}_{0,k|k-1}){(z_{0,k|k-1}^i-{\hat{z}}_{0,k|k-1})}^T---\left(8\right)$

$P_{\mathrm{xz},0,k|k-1}=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{w}_i({\mathrm{\χ}}_{0,k|k-1}^i-{\hat{x}}_{p,0,k|k-1}){({\mathrm{\χ}}_{0,k|k-1}^i-{\hat{x}}_{p,0,k|k-1})}^T---\left(9\right)$

其中，P_zz,0,k|k-1为新息协方差，P_xz,0,k|k-1为交叉协方差，R_k为观测噪声协方差；

根据新息协方差P_zz,0,k|k-1、交叉协方差P_xz,0,k|k-1以及k时刻的目标观测向量获取k时刻目标状态的第一后验概率密度函数对应的均值和协方差，具体如下所示：

${\hat{x}}_{u,0,k|k}={\hat{x}}_{p,0,k|k-1}+P_{\mathrm{xz},0,k|k-1}{P}_{\mathrm{zz},0,k|k-1}^{-1}(z_k-{\hat{z}}_{0,k|k-1})---\left(10\right)$

$P_{u,0,k|k}=P_{p,0,k|k-1}-P_{\mathrm{xz},0,k|k-1}{P}_{\mathrm{zz},0,k|k-1}^{-1}{P}_{\mathrm{xz},0,k|k-1}^T---\left(11\right)$

其中，为k时刻目标状态的第一后验概率密度函数对应的均值，P_u,0,k|k为k时刻目标状态的第一后验概率密度函数对应的协方差，z_k为k时刻的目标观测向量。

其中，根据当前目标观测时刻的目标观测向量修正原始先验概率密度函数，以获取修正先验概率密度函数的步骤具体包括：根据k时刻的目标观测向量利用最小二乘交叉定位方法获取k时刻的目标状态估计值T；

采用目标状态估计值T代替最大似然估计值，具体如下式所示：

${\tilde{a}}_k\left(z_k\right)=T---\left(12\right)$

其中，为最大似然估计值；

根据最大似然估计值修正原始先验概率密度函数，获取修正先验概率密度函数的均值和协方差，具体如下所示：

${\hat{x}}_{p,1,k|k-1}=\left[\begin{array}{c}{\mathrm{\μ}}_{a_k,1}\\ {\mathrm{\μ}}_{b_k,1}\end{array}\right]---\left(13\right)$

$P_{p,1,k|k-1}=\left[\begin{array}{cc}{\mathrm{\Σ}}_{a_k,1}& {\mathrm{\Σ}}_{a_k{b}_k,1}\\ {\left({\mathrm{\Σ}}_{a_k{b}_k,1}\right)}^T& {\mathrm{\Σ}}_{b_k,1}\end{array}\right]---\left(14\right)$

其中，

其中，为修正先验概率密度函数的均值，P_p,1,k|k-1为修正先验概率密度函数的协方差，a为目标的状态分量，cov[v]=R，v为观测噪声。

其中，根据修正先验概率密度函数获取当前目标观测时刻目标状态的第二后验概率密度函数的步骤具体包括：根据均值和协方差P_p,1,k|k-1获取m个积分点具体如下式所示：

$X_{i,1,k|k-1}={\left(\sqrt{P_{p,1,k|k-1}}\right)}^T{\mathrm{\ξ}}_i+{\hat{x}}_{p,1,k|k-1}---\left(15\right)$

将积分点代入非线性的观测函数h(·)，以获取积分点对应的m个预测值具体如下式所示：

$z_{1,k|k-1}^i=h\left(X_{i,1,k|k-1}\right)---\left(16\right)$

获取积分点对应的m个预测值的均值具体如下式所示：

${\hat{z}}_{1,k|k-1}=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{w}_i{z}_{1,k|k-1}^i---\left(17\right)$

根据预测值和均值获取对应的观测误差协方差以及交叉协方差，具体如下所示：

$P_{\mathrm{zz},1,k|k-1}=R_k+\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{w}_i(z_{1,k|k-1}^i-{\hat{z}}_{1,k|k-1}){(z_{1,k|k-1}^i-{\hat{z}}_{1,k|k-1})}^T---\left(18\right)$

$P_{\mathrm{xz},1,k|k-1}=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{w}_i({\mathrm{\χ}}_{1,k|k-1}^i-{\hat{x}}_{p,1,k|k-1}){(z_{1,k|k-1}^i-{\hat{z}}_{1,k|k-1})}^T---\left(19\right)$

其中，P_zz,1,k|k-1为观测误差协方差，P_xz,1,k|k-1为交叉协方差；

根据观测误差协方差P_zz,1,k|k-1、交叉协方差P_xz,1,k|k-1以及k时刻的目标观测向量获取k时刻目标状态的第二后验概率密度函数对应的均值和协方差，具体如下所示：

${\hat{x}}_{u,1,k|k}={\hat{x}}_{p,1,k|k-1}+P_{\mathrm{xz},1,k|k-1}{P}_{\mathrm{zz},1,k|k-1}^{-1}(z_k-{\hat{z}}_{1,k|k-1})---\left(20\right)$

$P_{u,1,k|k}=P_{p,1,k|k}-P_{\mathrm{xz},1,k|k-1}{P}_{\mathrm{zz},1,k|k-1}^{-1}{P}_{\mathrm{xz},1,k|k-1}^T---\left(21\right)$

其中，为k时刻目标状态的第二后验概率密度函数对应的均值，P_u,1,k|k为k时刻目标状态的第二后验概率密度函数对应的协方差。

其中，根据第一后验概率密度函数以及第二后验概率密度函数获取当前目标观测时刻目标状态的联合后验概率密度函数的步骤具体包括：根据第一后验概率密度函数以及第二后验概率密度函数各自对应的均值获取目标状态估计权值，具体如下所示：

${\mathrm{\μ}}_0\left({\hat{x}}_{u,0,k|k}\right)=\frac{1}{\sqrt{\left|P_{\mathrm{zz},0,k|k-1}\right|}}\·\exp (-\frac{{(z_k-h\left({\hat{x}}_{u,0,k|k}\right))}^2}{2})---\left(22\right)$

${\mathrm{\μ}}_1\left({\hat{x}}_{u,1,k|k}\right)=\frac{1}{\sqrt{\left|P_{\mathrm{zz},1,k|k-1}\right|}}\·\exp (-\frac{{(z_k-h\left({\hat{x}}_{u,1,k|k}\right))}^2}{2})---\left(23\right)$

${\mathrm{\α}}_k=\frac{{\mathrm{\μ}}_1\left({\hat{x}}_{u,1,k|k}\right)}{{\mathrm{\μ}}_0\left({\hat{x}}_{u,0,k|k}\right)+{\mathrm{\μ}}_1\left({\hat{x}}_{u,1,k|k}\right)}---\left(24\right)$

其中，α_k为目标状态估计权值；

根据第一后验概率密度函数、第二后验概率密度函数各自对应的均值、协方差以及目标状态估计权值α_k获取k时刻目标状态的联合后验概率密度函数，具体如下所示：

$p\left(x_k\right|z_{1:k})=N({\hat{x}}_{k|k},P_{k|k})---\left(25\right)$

${\hat{x}}_{k|k}={\mathrm{\α}}_k\·{\hat{x}}_{u,1,k|k}+(1-{\mathrm{\α}}_k)\·{\hat{x}}_{u,0,k|k}---\left(26\right)$

其中，p(x_k|z_1:k)为k时刻目标状态的联合后验概率密度函数，为k时刻目标状态的联合后验概率密度函数的均值，P_k|k为k时刻目标状态的联合后验概率密度函数的协方差。

为解决上述技术问题，本发明的第四方面是：提供一种截断积分卡尔曼滤波装置，包括：原始先验概率密度函数获取模块，用于根据高斯-厄米特积分获取当前目标观测时刻目标状态的原始先验概率密度函数；第一后验概率密度函数获取模块，用于根据原始先验概率密度函数获取当前目标观测时刻目标状态的第一后验概率密度函数；修正先验概率密度函数获取模块，用于根据当前目标观测时刻的目标观测向量修正原始先验概率密度函数，以获取修正先验概率密度函数；第二后验概率密度函数获取模块，用于根据修正先验概率密度函数获取当前目标观测时刻目标状态的第二后验概率密度函数；联合后验概率密度函数获取模块，用于根据第一后验概率密度函数以及第二后验概率密度函数获取当前目标观测时刻目标状态的联合后验概率密度函数。

本发明的有益效果是：区别于现有技术的情况，本发明根据高斯-厄米特积分获取当前目标观测时刻目标状态的原始先验概率密度函数，然后根据高斯-厄米特积分获取当前目标观测时刻目标状态的原始先验概率密度函数；此外还根据当前目标观测时刻的目标观测向量修正原始先验概率密度函数，以获取修正先验概率密度函数，进一步根据修正先验概率密度函数获取当前目标观测时刻目标状态的第二后验概率密度函数，最后根据第一后验概率密度函数以及第二后验概率密度函数获取当前目标观测时刻目标状态的联合后验概率密度函数，能够有效减少目标状态先验分布方差，自适应地根据观测信息的精度进行状态更新，有效提高滤波精度且实用性较高。

附图说明

图1是本发明截断积分卡尔曼滤波方法一实施方式的流程图；

图2是本发明截断积分卡尔曼滤波方法一实施方式中根据高斯-厄米特积分获取当前目标观测时刻目标状态的原始先验概率密度函数的流程图；

图3是本发明截断积分卡尔曼滤波方法一实施方式中根据原始先验概率密度函数获取当前目标观测时刻目标状态的第一后验概率密度函数的流程图；

图4是本发明截断积分卡尔曼滤波方法一实施方式中根据当前目标观测时刻的目标观测向量修正原始先验概率密度函数，以获取修正先验概率密度函数的流程图；

图5是本发明截断积分卡尔曼滤波方法一实施方式中根据修正先验概率密度函数获取当前目标观测时刻目标状态的第二后验概率密度函数的流程图；

图6是本发明截断积分卡尔曼滤波方法一实施方式中根据第一后验概率密度函数以及第二后验概率密度函数获取当前目标观测时刻目标状态的联合后验概率密度函数的流程图；

图7是本发明截断积分卡尔曼滤波方法一实施方式中目标的实际轨迹图；

图8是本发明截断积分卡尔曼滤波方法一实施方式中当过程噪声方差σ_w=0.001时QKF、MMRBPF和TQKF的均方根误差对比图；

图9是本发明截断积分卡尔曼滤波方法一实施方式中当过程噪声方差σ_w=0.01时UKF、QKF、MMRBPF和TQKF的均方根误差对比图；

图10是本发明截断积分卡尔曼滤波方法一实施方式中当过程噪声方差σ_w=0.04时UKF、QKF、MMRBPF、IMMEKF和TQKF的均方根误差对比图；

图11是本发明截断积分卡尔曼滤波装置一实施方式的原理框图；

图12是本发明目标跟踪系统一实施方式的结构示意图。

具体实施方式

下面将结合本发明实施方式中的附图，对本发明实施方式中的技术方案进行清楚、完整地描述，显然，所描述的实施方式仅仅是本发明一部分实施方式，而不是全部的实施方式。基于本发明中的实施方式，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施方式，均属于本发明保护的范围。

本发明的截断积分卡尔曼滤波（Truncated Quadrature KalmanFiltering，TQKF）为从一系列的不完全及包含噪声的目标观测向量中，对目标在某一时刻的状态进行估计。本发明的截断积分卡尔曼滤波方法为针对被动传感器阵列中的目标跟踪问题，本发明对应的系统模型以及本发明的基本理论具体如下所述。

本发明对应的系统模型：

考虑如下所示的非线性一阶马尔可夫离散系统：

x_k=f(x_k-1)+w_k

z_k=h(x_k)+v_k

其中，表示系统在k时刻的状态向量，表示系统在k时刻的观测向量，z_k也即为k时刻的目标观测向量，f(·)表示非线性的过程函数，h(·)表示非线性的观测函数，w_k～Ν(0,Q_k)为过程噪声向量，v_k～Ν(0,R_k)为观测噪声向量。

由于被动传感器只能获得目标的方位角和俯仰角信息，为对各被动传感器的目标观测进行有效处理，建立如下的多被动传感器观测模型。设每个传感器都可获得方位角和俯仰角两个目标观测，则对于n(n≥2)个被动传感器组成的被动传感器阵列，建立如下的非线性观测模型：

Z_k=H(x_k)+V_k

式中，

$\begin{array}{c}{Z}_k={\left[\begin{array}{cccc}{z_k^1}^T& {z_k^2}^T& ...& {z_k^n}^T\end{array}\right]}^T\\ =\left[\begin{array}{cccc}{({\mathrm{\θ}}_1,{\mathrm{\β}}_1)}^T& {({\mathrm{\θ}}_2,{\mathrm{\β}}_2)}^T& ...& {({\mathrm{\θ}}_n,{\mathrm{\β}}_n)}^T\end{array}\right]T\end{array}$

$\begin{array}{c}H\left(X_k\right)={\left[\begin{array}{cccc}{h}_1{\left(x_k\right)}^T& h_2{\left(x_k\right)}^T& ...& h_n{\left(x_k\right)}^T\end{array}\right]}^T\\ V_k={\left[\begin{array}{cccc}{v_k^1}^T& {v_k^2}^T& ...& {v_k^n}^T\end{array}\right]}^T\end{array}$

其中，h_i(x_k)为被动传感器的目标观测与目标位置的非线性方程，V_k为观测噪声向量。假设观测噪声任意两两之间互不相关，则V_k的协方差矩阵R_k为：

$\begin{array}{c}{R}_k=E\left(V_k{V_k}^T\right)\\ =\mathrm{diag}\left[\begin{array}{ccccc}{{\mathrm{\σ}}_{\mathrm{\θ}}^2}_1& {{\mathrm{\σ}}_{\mathrm{\β}}^2}_1,& ...,& {{\mathrm{\σ}}_{\mathrm{\θ}}^2}_n& {{\mathrm{\σ}}_{\mathrm{\β}}^2}_n\end{array}\right]\end{array}$

本发明的基本理论：

假设k时刻目标的状态向量为 $x_k={[{a_k}^T,{b_k}^T]}^T,a_k\∈R^{n_a},b_k\∈R^{n_b},$ 并且n_x=n_a+n_b，a_k、b_k为目标的状态分量，其中a_k具体可为目标的位置信息，b_k具体可为目标的速度信息，对应的观测方程可为：

z_k=h(a_k)+v_k

进一步作如下两个假设：（1）观测函数为连续的、单射的函数。（2）噪声的概率密度函数为有界的、连通的，即

p_η(v)=0

$\mathrm{\η}\∉I_n\⋐R^{n_z}$

其中，I_η表示一个n_x维的连通区域。基于上述假设（2），在目标状态已知的条件下观测似然函数为：

$p\left(z_k\right|x_k)=p\left(z_k\right|a_k)=p_{\mathrm{\η}}(z_k-h\left(a_k\right)){\mathrm{\χ}}_{I_{\mathrm{\η}}}(z_k-h\left(a_k\right))---\left(a\right)$

其中，表示子集I_η的指示函数。

根据上述假设（1），式（a）可以进一步表示为：

$p\left(z_k\right|x_k)=p_v(z_k-h\left(a_k\right)){\mathrm{\χ}}_{I_x\left(z\right)}\left(x_k\right)$

其中，

$\begin{array}{c}{I}_x\left(z_k\right)=\left\{x_k\right|x_k={[{\left(h^{-1}(z_k-v_k)\right)}^T,{b_k}^T]}^T,v_k\∈I_v,b_k\∈R^{n_b}\}\\ =I_a\left(z_k\right)\×R^{n_b}\end{array}---\left(b\right)$

根据贝叶斯准则，x_k的后验概率密度函数对应为：

$p\left(x_k\right|z_k)\∝p\left(z_k\right|x_k)\·p_0\left(x_k\right)=p_v(z_k-h\left(a_k\right)){\mathrm{\χ}}_{I_x\left(z\right)}\left(x_k\right)p_0\left(x_k\right)$

进一步地，上式可以表示为：

p(x_k|z_k)∝p(z_k|x_k)·p₁(x_k;z_k)

$p_1(x_k;z_k)=\frac{1}{{\mathrm{\ϵ}}_1}{p}_0\left(x_k\right){\mathrm{\χ}}_{I_x\left(z\right)}\left(x_k\right)---\left(c\right)$

其中，ε₁为标准化常数，从式（c）可以看出，基于当前观测时刻的目标观测向量z_k，式（c）构建了一种修正的先验概率密度函数p₁(·)，可以有效减少原始先验概率密度函数p₀(·)的方差。

请参阅图1，本发明截断积分卡尔曼滤波方法一实施方式包括以下步骤：

步骤S11：根据高斯-厄米特积分获取当前目标观测时刻目标状态的原始先验概率密度函数；

高斯-厄米特（Gauss-Hermite）积分：

对于一个概率密度函数为的随机变量，根据高斯-厄米特积分理论，下式所示的函数积分可以近似表示为：

其中ξ_l为高斯-厄米特积分点，m′为高斯-厄米特积分点的数目，w_l为高斯-厄米特积分点ξ_l相应的权值，l=1,2,...,m′。

进一步利用正交多项式与上三角矩阵之间的关系来计算高斯-厄米特积分点ξ_l及其权值w_l，假设J为一个对角元素为0的对角矩阵且

$J_{i,i+1}=\sqrt{i/2},1\≤i\≤(m^{\′}-1)$

因此将高斯-厄米特积分点ξ_l设置为其中ε_l为对角矩阵的第个特征值；权值其中(v_l)₁为对角矩阵J第l个标准化特征值的第1个元素。

由于矢量中各个元素分量互不相关，因此可以将上面所述的一维积分点的情况推广到多维积分点的情况：假设一个具有高斯密度的随机矢量x，表示n_x×n_x的单位矩阵，多维积分点公式如下式所示：

$\begin{array}{c}E\left(f\left(x\right)\right)=\underset{R^{n_x}}{\∫}f\left(x\right)N(x;0,I_{n_x})\mathrm{dx}\\ \≈\underset{l_{n_x}=1}{\overset{m^{\′}}{\mathrm{\Σ}}}{w}_{l_{n_x}}...\underset{l=1}{\overset{m^{\′}}{\mathrm{\Σ}}}{w}_{l}f({\mathrm{\ξ}}_{l_1},{\mathrm{\ξ}}_{l_2},...,{\mathrm{\ξ}}_{l_{n_x}})\\ =\underset{l=1}{\overset{m^{\′n_x}}{\mathrm{\Σ}}}{w}_{l}f\left({\mathrm{\ξ}}_l\right)\end{array}$

其中高斯-厄米特积分点权值

本发明截断积分卡尔曼滤波方法一实施方式的步骤S11为根据上述高斯-厄米特积分获取当前目标观测时刻目标状态的原始先验概率密度函数，请参阅图2，该步骤具体包括以下子步骤：

子步骤S111：根据高斯-厄米特积分获取m个积分点；

根据上述高斯-厄米特积分获取m个积分点具体如下式（1）所示：

$X_{i,k-1|k-1}={\left(\sqrt{P_{k-1|k-1}}\right)}^T{\mathrm{\ξ}}_i+{\hat{x}}_{k-1|k-1}---\left(1\right)$

其中，为均值，P_k-1|k-1为协方差，P_k-1|k-1具体为协方差矩阵，本步骤中首先假设系统的初始状态的均值为协方差为P_k-1|k-1，均值和协方差P_k-1|k-1具体可根据当前目标观测时刻之前^（具体为上一目标观测时刻）的目标观测向量而获得，ξ_i表示高斯-厄米特积分点，i=1,2,...,m。

子步骤S112：将积分点代入非线性的过程函数，以获取积分点对应的m个预测值；

将子步骤S111中获取的积分点代入非线性的过程函数，以获取积分点对应的m个预测值具体如下式（2）所示：

$X_{0,k|k-1}^i=f\left(X_{i,k-1/k-1}\right)---\left(2\right)$

其中，f(·)为非线性的过程函数。

子步骤S113：根据积分点对应的m个预测值获取k时刻目标状态的原始先验概率密度函数对应的均值和协方差。

根据上述积分点对应的m个预测值获取k时刻目标状态的原始先验概率密度函数对应的均值和协方差，具体如下式（3）、（4）所示：

${\hat{x}}_{p,0,k|k-1}=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{w}_i{X}_{0,k|k-1}^i---\left(3\right)$

$P_{p,0,k|k-1}=Q_k+\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{w}_i(X_{0,k|k-1}^i-{\hat{x}}_{p,0,k|k-1}){(X_{0,k|k-1}^i-{\hat{x}}_{p,0,k|k-1})}^T---\left(4\right)$

其中，为积分点对应的权值，Q_k为过程噪声向量的协方差，k时刻表示当前目标观测时刻，为k时刻目标状态的原始先验概率密度函数的均值，P_p,0,k|k-1为k时刻目标状态的原始先验概率密度函数的协方差。k-1时刻表示上一目标观测时刻，上述预测值为积分点在k时刻对应的预测值。

即通过步骤S11将当前目标观测时刻（k时刻）目标状态的原始先验概率密度函数p₀(·)近似成一个均值为协方差为P_p,0,k|k-1的高斯概率密度函数

步骤S12：根据原始先验概率密度函数获取当前目标观测时刻目标状态的第一后验概率密度函数；

本步骤进一步基于上述原始先验概率密度函数进行观测更新以对应获取当前目标观测时刻（k时刻）目标状态的第一后验概率密度函数，请参阅图3，具体包括以下子步骤：

子步骤S121：根据均值和协方差P_p,0,k|k-1获取m个积分点；

根据上述原始先验概率密度函数的均值和协方差P_p,0,k|k-1获取m个积分点具体如下式（5）所示：

$X_{i,0,k|k-1}={\left(\sqrt{P_{p,0,k|k-1}}\right)}^T{\mathrm{\ξ}}_i+{\hat{x}}_{p,0,k|k-1}---\left(5\right)$

子步骤S122：将积分点代入非线性的观测函数，以获取积分点对应的m个预测值；

将积分点代入非线性的观测函数，以获取积分点对应的m个预测值具体如下式（6）所示：

$z_{0,k|k-1}^i=h\left(X_{i,0,k|k-1}\right)---\left(6\right)$

其中，h(·)为非线性的观测函数。预测值即为积分点在k时刻对应的观测预测值。

子步骤S123：获取积分点对应的m个预测值的均值；

获取积分点对应的m个预测值的均值具体如下式（7）所示：

${\hat{z}}_{0,k|k-1}=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{w}_i{z}_{0,k|k-1}^i---\left(7\right)$

子步骤S124：根据预测值和均值获取对应的新息协方差以及交叉协方差；

根据积分点对应的m个预测值和均值获取对应的新息协方差以及交叉协方差，具体如下式（8）、（9）所示：

$P_{\mathrm{zz},0,k|k-1}=R_k+\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{w}_i(z_{0,k|k-1}^i-{\hat{z}}_{0,k|k-1}){(z_{0,k|k-1}^i-{\hat{z}}_{0,k|k-1})}^T---\left(8\right)$

$P_{\mathrm{xz},0,k|k-1}=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{w}_i({\mathrm{\χ}}_{0,k|k-1}^i-{\hat{x}}_{p,0,k|k-1}){({\mathrm{\χ}}_{0,k|k-1}^i-{\hat{x}}_{p,0,k|k-1})}^T---\left(9\right)$

其中，P_zz,0,k|k-1为新息协方差，P_xz,0,k|k-1为交叉协方差，R_k为观测噪声协方差。

子步骤S125：根据新息协方差、交叉协方差以及k时刻的目标观测向量获取k时刻目标状态的第一后验概率密度函数对应的均值和协方差。

根据新息协方差P_zz,0,k|k-1、交叉协方差P_xz,0,k|k-1以及k时刻的目标观测向量获取k时刻目标状态的第一后验概率密度函数对应的均值和协方差，具体如下式（10）、（11）所示：

${\hat{x}}_{u,0,k|k}={\hat{x}}_{p,0,k|k-1}+P_{\mathrm{xz},0,k|k-1}{P}_{\mathrm{zz},0,k|k-1}^{-1}(z_k-{\hat{z}}_{0,k|k-1})---\left(10\right)$

$P_{u,0,k|k}=P_{p,0,k|k-1}-P_{\mathrm{xz},0,k|k-1}{P}_{\mathrm{zz},0,k|k-1}^{-1}{P}_{\mathrm{xz},0,k|k-1}^T---\left(11\right)$

其中，为k时刻即当前目标观测时刻目标状态的第一后验概率密度函数对应的均值，P_u,0,k|k为k时刻即当前目标观测时刻目标状态的第一后验概率密度函数对应的协方差，z_k为k时刻的目标观测向量。目标观测向量为被动传感器阵列中的多个被动传感器对目标进行观测而获得的目标状态的相关观测数据。

即通过步骤S12将当前目标观测时刻（k时刻）目标状态的第一后验概率密度函数近似成一个均值为协方差为P_u,0,k|k的高斯概率密度函数

步骤S13：根据当前目标观测时刻的目标观测向量修正原始先验概率密度函数，以获取修正先验概率密度函数；

本步骤根据当前目标观测时刻的目标观测向量z_k修正步骤S11所获取的原始先验概率密度函数，以获取修正先验概率密度函数，请参阅图4，具体包括以下子步骤：

子步骤S131：根据k时刻的目标观测向量利用最小二乘交叉定位方法获取k时刻的目标状态估计值；

根据k时刻的目标观测向量利用最小二乘交叉定位方法获取k时刻的目标状态估计值T，最小二乘交叉定位方法具体如下所述。

最小二乘交叉定位方法：

假设k时刻同时接收到N个被动传感器的N条观测信息其中，θ_i表示方位角，β_i表示俯仰角，被动传感器i的位置为(x_i,y_i,z_i)，i=1,2,...N。由相应的几何知识可知：每个被动传感器测得的目标的角度信息（θ_i、β_i）可以确定一条空间的定位线，在观测误差为零的情况下，N条定位线交于一点，该点即为目标的位置。但是，由于实际观测过程中观测误差一般不为零，因此上述N条定位线并不会交于一点。对此，可以将离N条定位线距离和最短的点当作目标估计位置，利用最小二乘交叉定位方法估计目标的位置的过程为：

设L_i表示由被动传感器i得到的定位线，T(x_T,y_T,z_T)为目标的位置，A_i(x_i0,y_i0,z_i0)表示目标到定位线L_i的垂足，则定位线L_i的公式为：

$\frac{x-x_i}{l_i}=\frac{y-y_i}{m_i}=\frac{z-z_i}{n_i}$

其中，(l_i,m_i,n_i)表示定位线L_i的方向余弦，且

l_i=sinβ_icosα_i，m_i=sinβ_isinα_i，n_i=cosβ_i

根据几何关系及相应的数学变换，目标相对于N条定位线L_i的距离平方和可以表示为：

$d=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\left|A_{i}T\right|}^2=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}(u_i^2+v_i^2+w_i^2)$

式中，

$u_i=(x_T-x_i)l_i^2+(y_T-y_i)m_i{l}_i+(z_T-z_i)l_i{n}_i+x_i-x_T$

$v_i=(x_T-x_i)m_i{l}_i+(y_T-y_i)m_i^2+(z_T-z_i)m_i{n}_i+y_i-y_T$

$w_i=(x_T-x_i)l_i{n}_i+(y_T-y_i)m_i{n}_i+(z_T-z_i)n_i^2+z_i-z_T$

令 $\frac{\∂d}{{\∂x}_T}=0,\frac{\∂d}{\∂y_T}=0,\frac{\∂d}{\∂z_T}=0,$ 可得：

$\left[\begin{array}{ccc}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}(m_i^2+n_1^2)& -\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{m}_i{l}_i& -\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{l}_i{n}_i\\ -\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{m}_i{l}_i& \underset{i=1}{\overset{N}{\mathrm{\Σ}}}(m_i^2+l_i^2)& -\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{m}_i{n}_i\\ -\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{l}_i{n}_i& -\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{m}_i{n}_i& \underset{i=1}{\overset{N}{\mathrm{\Σ}}}(l_i^2+l_i^2)\end{array}\right]\left[\begin{array}{c}{x}_T\\ y_T\\ z_T\end{array}\right]=\left[\begin{array}{c}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}[(m_i^2+n_i^2)x_i-m_i{l}_i{y}_i-l_i{n}_i{z}_i]\\ \underset{i=1}{\overset{N}{\mathrm{\Σ}}}[(l_i^2+n_i^2)y_i-m_i{l}_i{x}_i-m_i{n}_i{z}_i]\\ \underset{i=1}{\overset{N}{\mathrm{\Σ}}}[(m_i^2+l_i^2)x_i-n_i{l}_i{x}_i-m_i{n}_i{y}_i]\end{array}\right]$

上式即为目标位置的最小二乘估计值，上式所示方程的解如下：

$\begin{array}{c}{\hat{x}}_T=(\mathrm{EMN}+\mathrm{FRS}+\mathrm{TRG}-\mathrm{GMS}-\mathrm{TFN}-R^{2}E)/D\\ {\hat{y}}_T=(\mathrm{LFN}+\mathrm{TGS}+\mathrm{ERS}-S^{2}F-\mathrm{GRL}-\mathrm{TEN})/D\\ {\hat{z}}_T=(\mathrm{LMG}+\mathrm{TRE}+\mathrm{TFS}-\mathrm{SME}-\mathrm{RFL}-T^{2}G)/D\end{array}---\left(c\right)$

上式中D=LMN+2TRS-S²M-R²L-T²N

$L=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}(m_i^2+n_i^2),M=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}(n_i^2+l_i^2),N=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}(m_i^2+l_i^2),R=-\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{m}_i{n}_i$

$S=-\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{l}_i{n}_i,T=-\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{m}_i{l}_i,E=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}[(m_i^2+n_i^2)x_i-m_i{l}_i{y}_i-l_i{n}_i{z}_i]$

$F=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}[(l_i^2+n_i^2)y_i-m_i{l}_i{x}_i-m_i{n}_i{z}_i],G=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}[(m_i^2+l_i^2)x_i-n_i{l}_i{x}_i-m_i{n}_i{y}_i]$

式（c）所示方程的解即为目标状态估计值T。

子步骤S132：采用目标状态估计值代替最大似然估计值；

采用目标状态估计值T代替最大似然估计值，具体如下式（12）所示：

${\tilde{a}}_k\left(z_k\right)=T---\left(12\right)$

其中，为最大似然估计值。

子步骤S133：根据最大似然估计值修正原始先验概率密度函数，获取修正先验概率密度函数的均值和协方差。

根据最大似然估计值修正原始先验概率密度函数，获取修正先验概率密度函数的均值和协方差具体如下所述。

为了近似修正先验概率密度函数p₁(·)，对应作如下三个近似假设：（1）观测函数h(·)是局部线性的；（2）目标的状态分量a的边缘先验概率密度p₀(a)在区域I_a(z)是常数；（3）观测噪声v满足E[v]=0,cov[v]=R。

根据假设1，由于观测函数h(·)是局部线性的，在点对观测函数h(·)进行一阶泰勒级数展开，并令这是因为根据当前目标观测，是最可能的目标状态，最大化似然函数，可得（由于被动传感器观测方程并没有唯一的反函数，因此本发明采用上述目标状态估计值T代替最大似然估计值因此有：

$h\left(a\right)=h\left(\tilde{a}\left(z\right)\right)+\tilde{H}(a-\tilde{a}\left(z\right))$

其中 $\tilde{H}={\left[{\▿}_a{h}^T\left(a\right)\right]}^T{|}_{a=\tilde{a}\left(z\right)},$ 为雅克比矩阵在的取值。

将代入上述基本理论中的式（b）可得：

$I_a\left(z_k\right)=\left\{a_k\right|a_k={\tilde{a}}_k\left(z_k\right)-{\tilde{H}}^{-1}{w}_k,w_k\∈I_{\mathrm{\η}}\}$

接着上述式（c）转化为：

$p_1(x_k;z_k)=p_1(a_k,b_k;z_k)=\frac{1}{{\mathrm{\ϵ}}_1}{\mathrm{\χ}}_{I_a\left(z\right)}\left(a_k\right)p_0\left(b_k\right|a_k)p_0\left(a_k\right)---\left(d\right)$

由于上述假设p₀(a_k)在区域I_a(z)中为常数，因此式（d）可写为：

$p_1(a_k,b_k;z_k)=\frac{1}{{\mathrm{\ϵ}}_2}{\mathrm{\χ}}_{I_a\left(z\right)}\left(a_k\right)p_0\left(b_k\right|a_k)---\left(e\right)$

根据式（e），修正先验概率密度函数p₁(·)中状态分量a对应的均值和协方差具体如下所示：

${\mathrm{\μ}}_{a_k,1}={\tilde{a}}_k\left(z_k\right)---\left(f\right)$

${\mathrm{\Σ}}_{a_k,1}={\tilde{H}}^{-1}R{\left({\tilde{H}}^{-1}\right)}^T---\left(g\right)$

其中，a为目标的状态分量，cov[v]=R，v为观测噪声。

另外，原始先验概率密度函数p₀(x_k)=p₀(a_k,b_k)的均值和协方差P_p,0,k|k-1可以分解为：

${\hat{x}}_{p,0,k|k-1}=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{w}_i{X}_{0,k|k-1}^i=\left[\begin{array}{c}E\left[a_k\right]\\ E\left[b_k\right]\end{array}\right]=\left[\begin{array}{c}{\mathrm{\μ}}_{a_k,0}\\ {\mathrm{\μ}}_{b_k,0}\end{array}\right]---\left(h\right)$

$\begin{array}{c}{P}_{p,0,k|k-1}=Q_k+\underset{i=1}{\overset{N_s}{\mathrm{\Σ}}}{w}_i(X_{0,k|k-1}^i-{\hat{x}}_{p,0,k|k-1}){(X_{0,k|k-1}^i-{\hat{x}}_{p,0,k|k-1})}^T=\left[\begin{array}{cc}\mathrm{cov}\left[a_k\right]& \mathrm{cov}[a_k,b_k]\\ \mathrm{cov}{[a_k,b_k]}^T& \mathrm{cov}\left[b_k\right]\end{array}\right]\\ =\left[\begin{array}{cc}{\mathrm{\Σ}}_{a_k,0}& {\mathrm{\Σ}}_{a_k{b}_k,0}\\ {\left({\mathrm{\Σ}}_{a_k{b}_k,0}\right)}^T& {\mathrm{\Σ}}_{b_k,0}\end{array}\right]\end{array}---\left(i\right)$

因此根据式（f）、（g）、（h）、（i），修正先验概率密度函数p₁(·)的均值和协方差P_p,1,k|k-1具体如下式（13）、（14）所示：

${\hat{x}}_{p,1,k|k-1}=\left[\begin{array}{c}{\mathrm{\μ}}_{a_k,1}\\ {\mathrm{\μ}}_{b_k,1}\end{array}\right]---\left(13\right)$

$P_{p,1,k|k-1}=\left[\begin{array}{cc}{\mathrm{\Σ}}_{a_k,1}& {\mathrm{\Σ}}_{a_k{b}_k,1}\\ {\left({\mathrm{\Σ}}_{a_k{b}_k,1}\right)}^T& {\mathrm{\Σ}}_{b_k,1}\end{array}\right]---\left(14\right)$

其中，

${\mathrm{\μ}}_{b_k,1}={\mathrm{\μ}}_{b_k,0}+{\mathrm{\Σ}}_{a_k{b}_k,0}^T{\mathrm{\Σ}}_{a_k,0}^{-1}({\mathrm{\μ}}_{a_k,1}-{\mathrm{\μ}}_{a_k,0})$

$\begin{array}{c}{\mathrm{\Σ}}_{b_k,1}=\mathrm{\Γ}-({\mathrm{\μ}}_{b_k,1}-{\mathrm{\μ}}_{b_k,0}){({\mathrm{\μ}}_{b_k,1}-{\mathrm{\μ}}_{b_k,0})}^T+{\mathrm{\Σ}}_{a_k{b}_k,0}^T{\mathrm{\Σ}}_{a_k,0}^{-1}\\ \×[{\mathrm{\Σ}}_{a_k,1}+({\mathrm{\μ}}_{a_k,1}-{\mathrm{\μ}}_{a_k,0}){({\mathrm{\μ}}_{a_k,1}-{\mathrm{\μ}}_{a_k,0})}^T]{\left({\mathrm{\Σ}}_{a_k,0}^{-1}\right)}^T{\mathrm{\Σ}}_{a_k{b}_k,0}\end{array}$

${\mathrm{\Σ}}_{a_k{b}_k,1}={\mathrm{\Σ}}_{a_k,1}{\left({\mathrm{\Σ}}_{a_k,0}^{-1}\right)}^T{\mathrm{\Σ}}_{a_k{b}_k,0}$

$\mathrm{\Γ}={\mathrm{\Σ}}_{b_k,0}-{\mathrm{\Σ}}_{a_k{b}_k,0}^T{\mathrm{\Σ}}_{a_k,0}^{-1}{\mathrm{\Σ}}_{a_k{b}_k,0}$

即通过步骤S13将修正先验概率密度函数p₁(·)近似成一个均值为协方差为P_p,1,k|k-1的高斯概率密度函数

步骤S14：根据修正先验概率密度函数获取当前目标观测时刻目标状态的第二后验概率密度函数；

本步骤进一步基于上述修正先验概率密度函数进行观测更新以对应获取当前目标观测时刻（k时刻）目标状态的第二后验概率密度函数，请参阅图5，该步骤具体包括以下子步骤：

子步骤S141：根据均值和协方差P_p,1,k|k-1获取m个积分点；

根据修正先验概率密度函数的均值和协方差P_p,1,k|k-1获取m个积分点具体如下式（15）所示：

$X_{i,1,k|k-1}={\left(\sqrt{P_{p,1,k|k-1}}\right)}^T{\mathrm{\ξ}}_i+{\hat{x}}_{p,1,k|k-1}---\left(15\right)$

子步骤S142：将积分点代入非线性的观测函数，以获取积分点对应的m个预测值；

将积分点代入上述非线性的观测函数h(·)，以获取积分点对应的m个预测值具体如下式（16）所示：

$z_{1,k|k-1}^i=h\left(X_{i,1,k|k-1}\right)---\left(16\right)$

预测值即为积分点在k时刻对应的观测预测值。

子步骤S143：获取积分点对应的m个预测值的均值；

获取积分点对应的m个预测值的均值具体如下式（17）所示：

${\hat{z}}_{1,k|k-1}=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{w}_i{z}_{1,k|k-1}^i---\left(17\right)$

子步骤S144：根据预测值和均值获取对应的观测误差协方差以及交叉协方差；

根据预测值和均值获取对应的观测误差协方差以及交叉协方差，具体如下式（18）、（19）所示：

$P_{\mathrm{zz},1,k|k-1}=R_k+\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{w}_i(z_{1,k|k-1}^i-{\hat{z}}_{1,k|k-1}){(z_{1,k|k-1}^i-{\hat{z}}_{1,k|k-1})}^T---\left(18\right)$

$P_{\mathrm{xz},1,k|k-1}=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{w}_i({\mathrm{\χ}}_{1,k|k-1}^i-{\hat{x}}_{p,1,k|k-1}){(z_{1,k|k-1}^i-{\hat{z}}_{1,k|k-1})}^T---\left(19\right)$

其中，P_zz,1,k|k-1为观测误差协方差，P_xz,1,k|k-1为交叉协方差。

子步骤S145：根据观测误差协方差、交叉协方差以及k时刻的目标观测向量获取k时刻目标状态的第二后验概率密度函数对应的均值和协方差。

根据观测误差协方差P_zz,1,k|k-1、交叉协方差P_xz,1,k|k-1以及k时刻的目标观测向量z_k获取k时刻目标状态的第二后验概率密度函数对应的均值和协方差，具体如下式（20）、（21）所示：

${\hat{x}}_{u,1,k|k}={\hat{x}}_{p,1,k|k-1}+P_{\mathrm{xz},1,k|k-1}{P}_{\mathrm{zz},1,k|k-1}^{-1}(z_k-{\hat{z}}_{1,k|k-1})---\left(20\right)$

$P_{u,1,k|k}=P_{p,1,k|k}-P_{\mathrm{xz},1,k|k-1}{P}_{\mathrm{zz},1,k|k-1}^{-1}{P}_{\mathrm{xz},1,k|k-1}^T---\left(21\right)$

其中，为k时刻目标状态的第二后验概率密度函数对应的均值，P_u,1,k|k为k时刻目标状态的第二后验概率密度函数对应的协方差。

步骤S15：根据第一后验概率密度函数以及第二后验概率密度函数获取当前目标观测时刻目标状态的联合后验概率密度函数。

本步骤根据步骤S12获取的第一后验概率密度函数以及步骤S14获取的第二后验概率密度函数获取当前目标观测时刻目标状态的联合后验概率密度函数，请参阅图6，该步骤具体包括以下子步骤：

子步骤S151：根据第一后验概率密度函数以及第二后验概率密度函数各自对应的均值获取目标状态估计权值；

当目标观测向量z_k精度较高时，基于修正先验概率密度函数得到的目标状态的估计结果更可信；当目标观测向量z_k精度较低时，基于原始先验概率密度函数得到的目标状态的估计结果更可信。基于此，本发明通过一目标状态估计权值体现目标观测精度高低对目标状态的估计结果的影响：当目标观测向量z_k精度较高时，目标状态估计权值更接近于1；当目标观测向量z_k精度较低时，目标状态估计权值更接近于0。根据这一原则，考虑当前的估计结果和目标观测，本发明定义如下的高斯模糊隶属函数：

$\mathrm{\μ}\left(x\right)=K\·\exp (-\frac{{(z_k-h\left(x\right))}^2}{{2\mathrm{\σ}}^2})$

其中，Κ为一已知参数，σ²表示新息方差。根据上述第一后验概率密度函数对应的均值以及第二后验概率密度函数对应的均值获取目标状态估计权值，具体如下式（22）、（23）、（24）所示：

${\mathrm{\μ}}_0\left({\hat{x}}_{u,0,k|k}\right)=\frac{1}{\sqrt{\left|P_{\mathrm{zz},0,k|k-1}\right|}}\·\exp (-\frac{{(z_k-h\left({\hat{x}}_{u,0,k|k}\right))}^2}{2})---\left(22\right)$

${\mathrm{\μ}}_1\left({\hat{x}}_{u,1,k|k}\right)=\frac{1}{\sqrt{\left|P_{\mathrm{zz},1,k|k-1}\right|}}\·\exp (-\frac{{(z_k-h\left({\hat{x}}_{u,1,k|k}\right))}^2}{2})---\left(23\right)$

${\mathrm{\α}}_k=\frac{{\mathrm{\μ}}_1\left({\hat{x}}_{u,1,k|k}\right)}{{\mathrm{\μ}}_0\left({\hat{x}}_{u,0,k|k}\right)+{\mathrm{\μ}}_1\left({\hat{x}}_{u,1,k|k}\right)}---\left(24\right)$

其中，α_k为目标状态估计权值。由式（22）、（23）、（24）可以看出，当z_k与的差别越小时，表示估计结果越接近真实的目标状态，此时目标状态估计权值α_k相应变小；反之，目标状态估计权值α_k相应变大。即目标状态估计权值α_k自适应地随着目标观测的精度进行调整。

子步骤S152：根据第一后验概率密度函数、第二后验概率密度函数各自对应的均值、协方差以及目标状态估计权值获取k时刻目标状态的联合后验概率密度函数。

根据第一后验概率密度函数对应的均值协方差P_u,0,k|k、第二后验概率密度函数对应的均值协方差P_u,1,k|k以及目标状态估计权值α_k获取k时刻目标状态的联合后验概率密度函数，具体如下式（25）、（26）、（27）所示：

$p\left(x_k\right|z_{1:k})=N({\hat{x}}_{k|k},P_{k|k})---\left(25\right)$

${\hat{x}}_{k|k}={\mathrm{\α}}_k\·{\hat{x}}_{u,1,k|k}+(1-{\mathrm{\α}}_k)\·{\hat{x}}_{u,0,k|k}---\left(26\right)$

其中，p(x_k|z_1:k)为k时刻目标状态的联合后验概率密度函数，为k时刻目标状态的联合后验概率密度函数的均值，P_k|k为k时刻目标状态的联合后验概率密度函数的协方差。

即通过步骤S15将修正先验概率密度函数p₁(·)近似成一个均值为协方差为P_k|k的高斯概率密度函数p(x_k|z_1:k)，完成截断积分卡尔曼滤波过程。

上述步骤S11对应为本发明截断积分卡尔曼滤波方法的时间更新阶段，步骤S12对应为本发明截断积分卡尔曼滤波方法基于原始先验概率密度函数的观测更新阶段，步骤S13-14为本发明截断积分卡尔曼滤波方法基于修正先验概率密度函数的观测更新阶段，步骤S15为本发明截断积分卡尔曼滤波方法的联合状态更新阶段。

以下将以一个例子对本发明的TQKF方法的性能进行评估以及与现有的无迹卡尔曼滤波（UKF）、积分卡尔曼滤波(QKF)、交互多模型扩展卡尔曼滤波（IMMEKF）和多模Rao-Blackwellized粒子滤波（MMRBPF）的性能进行对比，具体如下所述。

该例子采用被动传感器阵列中两个被动传感器对单个目标进行跟踪：被动传感器观测站1的位置为(0,5km,0)，被动传感器观测站2的位置为(0,-5km,0)，MMRBPF方法中粒子数为100。

目标的实际轨迹如图7所示，目标初始位置为(2km,8km,1km)，运动速度为300m/s，高度为1.0km，目标观测向量的采样间隔T=1s。在前30个采样周期内目标作匀速直线运动；在第30个采样周期后目标以6°/s的转弯率转弯，转弯持续20个采样周期；在随后的20个采样周期内目标作匀速直线运动；在第70个采样周期后目标以4.8°/s的转弯率转弯，转弯持续25个采样周期；在随后的5个采样周期内目标作匀速直线运动。

本例子中固定观测噪声方差σ_v=1mrad，变化过程噪声方差σ_w。图8给出了当过程噪声方差σ_w=0.001时现有的QKF、MMRBPF与本发明的TQKF的均方根误差对比图（当σ_w=0.001时，UKF和IMMEKF丢失了目标，因此在图8中没有给出对应的均方根误差结果）；图9给出了当过程噪声方差σ_w=0.01时现有的UKF、QKF、MMRBPF与本发明的TQKF的均方根误差对比图（当σ_w=0.01时，IMMEKF丢失了目标，因此在图9中没有给出对应的均方根误差结果）；图10给出了当过程噪声方差σ_w=0.04时现有的UKF、QKF、MMRBPF、IMMEKF与本发明的TQKF的均方根误差对比图。

由图8、9可以看出，本发明TQKF方法的滤波性能优于现有的UKF、QKF、MMRBPF方法的滤波性能，特别是在目标机动阶段（即目标不处于匀速直线运动，例如上述在第30个采样周期后目标以6°/s的转弯率转弯），TQKF方法的滤波性能明显好于MMRBPF等方法，原因在于本发明的TQKF方法引入当前目标观测时刻的目标观测向量对原始先验概率密度函数进行了修正，避免了由于目标机动而造成目标状态先验分布方差的增大，特别是在观测比较精确时，滤波精度的效果提升越明显。由图10可以看出，当过程噪声方差增大到σ_w=0.04时，上述各方法均能够较好地对目标进行跟踪。

另外，如下所示的表1给出了现有的UKF、QKF、IMMEKF、MMRBPF和本发明TQKF五种算法独立运行100次的航迹丢失率结果。

表1各方法的航迹丢失率结果

	UKF	QKF	IMMEKF	MMRBPF	TQKF
						σw=0.001	100%	0%	100%	0%	0%
σw=0.01	0%	0%	13%	0%	0%
						σw=0.04	0%	0%	0%	0%	0%

由表1可以看出，当过程噪声方差σ_w=0.001时，UKF和IMMEKF方法的航迹丢失率达到了100%，完全不能跟踪到目标；当过程噪声方差σ_w=0.01时，IMMEKF的航迹丢失率也达到了13%，目标跟踪性能仍较差；而本发明的TQKF与QKF、MMRBPF在各种过程噪声方差情况下航迹丢失率都为0%，能够实现对各种情况下的目标进行准确跟踪。

可以理解，本发明截断积分卡尔曼滤波方法一实施方式根据高斯-厄米特积分获取当前目标观测时刻目标状态的原始先验概率密度函数，然后根据高斯-厄米特积分获取当前目标观测时刻目标状态的原始先验概率密度函数；此外还根据当前目标观测时刻的目标观测向量修正原始先验概率密度函数，以获取修正先验概率密度函数，进一步根据修正先验概率密度函数获取当前目标观测时刻目标状态的第二后验概率密度函数，最后根据第一后验概率密度函数以及第二后验概率密度函数获取当前目标观测时刻目标状态的联合后验概率密度函数。

本发明通过利用高斯-厄米特积分近似目标状态的先验和后验概率密度函数，提高系统的近似性能；根据当前目标观测向量对原始先验概率密度函数进行修正，能够减少目标状态先验分布方差，有效解决当目标发生机动时所引起的先验分布方差增大进而影响目标状态估计性能的问题；利用最小二乘交叉定位方法获取目标状态估计值，能够解决传统滤波算法进行观测更新时要求观测函数必须具有唯一反函数的问题，使得本发明能够应用于被动传感器系统的目标跟踪环境中；在联合后验概率密度函数中引入目标状态估计权值，实现自适应地根据观测信息的精度进行状态更新，从而能够有效提高滤波精度，提高对目标的跟踪性能；此外，本发明的截断积分卡尔曼滤波方法并不基于粒子滤波，计算量较简单，实用性较高。

请参阅图11，本发明实施方式还提供一种截断积分卡尔曼滤波装置，包括原始先验概率密度函数获取模块21、第一后验概率密度函数获取模块22、修正先验概率密度函数获取模块23、第二后验概率密度函数获取模块24以及联合后验概率密度函数获取模块25，各模块的功能具体如下所述。

原始先验概率密度函数获取模块21，用于根据高斯-厄米特积分获取当前目标观测时刻目标状态的原始先验概率密度函数，其中该原始先验概率密度函数获取模块21获取原始先验概率密度函数所采用的具体方法可以参阅上一实施方式中的步骤S11。

第一后验概率密度函数获取模块22，用于根据原始先验概率密度函数获取模块21所获取的原始先验概率密度函数进一步获取当前目标观测时刻目标状态的第一后验概率密度函数，其中该第一后验概率密度函数获取模块22获取第一后验概率密度函数所采用的具体方法可以参阅上一实施方式中的步骤S12。

修正先验概率密度函数获取模块23，用于根据当前目标观测时刻的目标观测向量修正原始先验概率密度函数获取模块21所获取的原始先验概率密度函数，以获取修正先验概率密度函数，其中该修正先验概率密度函数获取模块23获取修正先验概率密度函数所采用的具体方法可以参阅上一实施方式中的步骤S13。

第二后验概率密度函数获取模块24，用于根据修正先验概率密度函数获取模块23所获取的修正先验概率密度函数进一步获取当前目标观测时刻目标状态的第二后验概率密度函数，其中该第二后验概率密度函数获取模块24获取第二后验概率密度函数所采用的具体方法可以参阅上一实施方式中的步骤S14。

联合后验概率密度函数获取模块25，用于根据第一后验概率密度函数获取模块22所获取的第一后验概率密度函数以及第二后验概率密度函数获取模块24所获取的第二后验概率密度函数进一步获取当前目标观测时刻目标状态的联合后验概率密度函数，其中该联合后验概率密度函数获取模块25获取联合后验概率密度函数所采用的具体方法可以参阅上一实施方式中的步骤S15。

此外，在其他实施方式中也可采用其他模块框架结构实现本发明的截断积分卡尔曼滤波方法而不局限于本实施方式所提供的截断积分卡尔曼滤波装置对应的模块框架结构，此处不作过多限制。

本发明实施方式还提供一种目标跟踪方法，包括：

对目标进行观测以获得当前目标观测时刻的目标观测向量。

根据高斯-厄米特积分获取当前目标观测时刻目标状态的原始先验概率密度函数。

根据原始先验概率密度函数获取当前目标观测时刻目标状态的第一后验概率密度函数。

根据当前目标观测时刻的目标观测向量修正原始先验概率密度函数，以获取修正先验概率密度函数。

根据修正先验概率密度函数获取当前目标观测时刻目标状态的第二后验概率密度函数。

根据第一后验概率密度函数以及第二后验概率密度函数获取当前目标观测时刻目标状态的联合后验概率密度函数。

利用目标状态的联合后验概率密度函数对目标状态进行估计，以获得当前目标观测时刻的目标状态估计值。

输出当前目标观测时刻的目标状态估计值，以实现对飞机、航空飞行器、车辆等目标的跟踪。

请参阅图12，本发明实施方式还提供一种目标跟踪系统，该目标跟踪系统包括被动传感器阵列31以及截断积分卡尔曼滤波装置32，被动传感器阵列31连接至截断积分卡尔曼滤波装置32。

被动传感器阵列31由多个被动传感器组成，被动传感器阵列31用于对目标进行观测以获得当前目标观测时刻的目标观测向量。

截断积分卡尔曼滤波装置32处理来自被动传感器阵列31的观测数据，其处理过程可参阅上述的目标跟踪方法或截断积分卡尔曼滤波方法实施方式，在此不再赘述。

可以理解，本发明目标跟踪系统一实施方式通过被动传感器阵列对目标进行观测以获得当前目标观测时刻的目标观测向量，截断积分卡尔曼滤波装置根据目标观测向量进行滤波而输出当前目标观测时刻的目标状态估计值，能够有效减少目标状态先验分布方差，自适应地根据观测信息的精度进行状态更新，有效提高滤波精度以及目标跟踪性能，且该目标跟踪系统的实用性较高。

以上所述仅为本发明的实施方式，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。

Claims (9)

1.一种目标跟踪方法，其特征在于，包括：

对目标进行观测以获得当前目标观测时刻的目标观测向量；

根据高斯-厄米特积分获取当前目标观测时刻目标状态的原始先验概率密度函数；

根据所述原始先验概率密度函数获取当前目标观测时刻目标状态的第一后验概率密度函数；

根据所述当前目标观测时刻的目标观测向量修正所述原始先验概率密度函数，以获取修正先验概率密度函数；

根据所述修正先验概率密度函数获取当前目标观测时刻目标状态的第二后验概率密度函数；

根据所述第一后验概率密度函数以及第二后验概率密度函数获取当前目标观测时刻目标状态的联合后验概率密度函数；

利用所述目标状态的联合后验概率密度函数对目标状态进行估计，以获得当前目标观测时刻的目标状态估计值；

输出所述当前目标观测时刻的目标状态估计值，以实现对目标的跟踪。

2.一种目标跟踪系统，其特征在于，包括：

被动传感器阵列以及截断积分卡尔曼滤波装置，所述被动传感器阵列连接至截断积分卡尔曼滤波装置，所述被动传感器阵列由多个被动传感器组成，所述被动传感器阵列用于对目标进行观测以获得当前目标观测时刻的目标观测向量；

所述截断积分卡尔曼滤波装置包括：

原始先验概率密度函数获取模块，用于根据高斯-厄米特积分获取当前目标观测时刻目标状态的原始先验概率密度函数；

第一后验概率密度函数获取模块，用于根据所述原始先验概率密度函数获取当前目标观测时刻目标状态的第一后验概率密度函数；

修正先验概率密度函数获取模块，用于根据所述当前目标观测时刻的目标观测向量修正所述原始先验概率密度函数，以获取修正先验概率密度函数；

第二后验概率密度函数获取模块，用于根据所述修正先验概率密度函数获取当前目标观测时刻目标状态的第二后验概率密度函数；

联合后验概率密度函数获取模块，用于根据所述第一后验概率密度函数以及第二后验概率密度函数获取当前目标观测时刻目标状态的联合后验概率密度函数；

目标状态估计模块，用于利用所述目标状态的联合后验概率密度函数对目标状态进行估计，以获得当前目标观测时刻的目标状态估计值；

目标状态估计值输出模块，用于输出所述当前目标观测时刻的目标状态估计值。

3.一种截断积分卡尔曼滤波方法，其特征在于，包括：

根据高斯-厄米特积分获取当前目标观测时刻目标状态的原始先验概率密度函数；

根据所述原始先验概率密度函数获取当前目标观测时刻目标状态的第一后验概率密度函数；

根据当前目标观测时刻的目标观测向量修正所述原始先验概率密度函数，以获取修正先验概率密度函数；

根据所述修正先验概率密度函数获取当前目标观测时刻目标状态的第二后验概率密度函数；

根据所述第一后验概率密度函数以及第二后验概率密度函数获取当前目标观测时刻目标状态的联合后验概率密度函数，完成截断积分卡尔曼滤波过程。

4.根据权利要求3所述的方法，其特征在于，所述根据高斯-厄米特积分获取当前目标观测时刻目标状态的原始先验概率密度函数的步骤具体包括：

根据高斯-厄米特积分获取m个积分点具体如下式所示：

$X_{i,k-1|k-1}={\left(\sqrt{P_{k-1|k-1}}\right)}^T{\mathrm{\ξ}}_i+{\hat{x}}_{k-1|k-1}---\left(1\right)$

其中，为均值，P_k-1|k-1为协方差，ξ_i表示高斯-厄米特积分点，i=1,2,...,m；

将所述积分点代入非线性的过程函数，以获取积分点对应的m个预测值具体如下式所示：

$X_{0,k|k-1}^i=f\left(X_{i,k-1/k-1}\right)---\left(2\right)$

其中，f(·)为所述非线性的过程函数；

根据所述积分点对应的m个预测值获取k时刻目标状态的原始先验概率密度函数对应的均值和协方差，具体如下所示：

${\hat{x}}_{p,0,k|k-1}=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{w}_i{X}_{0,k|k-1}^i---\left(3\right)$

$P_{p,0,k|k-1}=Q_k+\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{w}_i(X_{0,k|k-1}^i-{\hat{x}}_{p,0,k|k-1}){(X_{0,k|k-1}^i-{\hat{x}}_{p,0,k|k-1})}^T---\left(4\right)$

其中，为积分点对应的权值，Q_k为过程噪声向量的协方差，k时刻表示当前目标观测时刻，为k时刻目标状态的原始先验概率密度函数的均值，P_p,0,k|k-1为k时刻目标状态的原始先验概率密度函数的协方差。

5.根据权利要求4所述的方法，其特征在于，所述根据所述原始先验概率密度函数获取当前目标观测时刻目标状态的第一后验概率密度函数的步骤具体包括：

根据所述均值和协方差P_{p，0，k|k-1}获取m个积分点具体如下式所示：

$X_{i,0,k|k-1}={\left(\sqrt{P_{p,0,k|k-1}}\right)}^T{\mathrm{\ξ}}_i+{\hat{x}}_{p,0,k|k-1}---\left(5\right)$

将所述积分点代入非线性的观测函数，以获取积分点对应的m个预测值具体如下式所示：

$z_{0,k|k-1}^i=h\left(X_{i,0,k|k-1}\right)---\left(6\right)$

其中，h(·)为所述非线性的观测函数；

获取所述积分点对应的m个预测值的均值具体如下式所示：

${\hat{z}}_{0,k|k-1}=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{w}_i{z}_{0,k|k-1}^i---\left(7\right)$

根据所述预测值和均值获取对应的新息协方差以及交叉协方差，具体如下所示：

$P_{\mathrm{zz},0,k|k-1}=R_k+\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{w}_i(z_{0,k|k-1}^i-{\hat{z}}_{0,k|k-1}){(z_{0,k|k-1}^i-{\hat{z}}_{0,k|k-1})}^T---\left(8\right)$

$P_{\mathrm{xz},0,k|k-1}=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{w}_i({\mathrm{\χ}}_{0,k|k-1}^i-{\hat{x}}_{p,0,k|k-1}){({\mathrm{\χ}}_{0,k|k-1}^i-{\hat{x}}_{p,0,k|k-1})}^T---\left(9\right)$

其中，P_zz,0,k|k-1为所述新息协方差，P_xz,0,k|k-1为所述交叉协方差，R_k为观测噪声协方差；

根据所述新息协方差P_zz,0,k|k-1、交叉协方差P_xz,0,k|k-1以及k时刻的目标观测向量获取k时刻目标状态的第一后验概率密度函数对应的均值和协方差，具体如下所示：

${\hat{x}}_{u,0,k|k}={\hat{x}}_{p,0,k|k-1}+P_{\mathrm{xz},0,k|k-1}{P}_{\mathrm{zz},0,k|k-1}^{-1}(z_k-{\hat{z}}_{0,k|k-1})---\left(10\right)$

$P_{u,0,k|k}=P_{p,0,k|k-1}-P_{\mathrm{xz},0,k|k-1}{P}_{\mathrm{zz},0,k|k-1}^{-1}{P}_{\mathrm{xz},0,k|k-1}^T---\left(11\right)$

其中，为所述k时刻目标状态的第一后验概率密度函数对应的均值，P_u,0,k|k为所述k时刻目标状态的第一后验概率密度函数对应的协方差，z_k为k时刻的目标观测向量。

6.根据权利要求5所述的方法，其特征在于，所述根据当前目标观测时刻的目标观测向量修正所述原始先验概率密度函数，以获取修正先验概率密度函数的步骤具体包括：

根据k时刻的目标观测向量利用最小二乘交叉定位方法获取k时刻的目标状态估计值T；

采用所述目标状态估计值T代替最大似然估计值，具体如下式所示：

${\tilde{a}}_k\left(z_k\right)=T---\left(12\right)$

其中，为所述最大似然估计值；

根据所述最大似然估计值修正原始先验概率密度函数，获取修正先验概率密度函数的均值和协方差，具体如下所示：

${\hat{x}}_{p,1,k|k-1}=\left[\begin{array}{c}{\mathrm{\μ}}_{a_k,1}\\ {\mathrm{\μ}}_{b_k,1}\end{array}\right]---\left(13\right)$

$P_{p,1,k|k-1}=\left[\begin{array}{cc}{\mathrm{\Σ}}_{a_k,1}& {\mathrm{\Σ}}_{a_k{b}_k,1}\\ {\left({\mathrm{\Σ}}_{a_k{b}_k,1}\right)}^T& {\mathrm{\Σ}}_{b_k,1}\end{array}\right]---\left(14\right)$

其中，

其中，为所述修正先验概率密度函数的均值，P_p,1,k|k-1为所述修正先验概率密度函数的协方差，a为目标的状态分量，cov[v]=R，v为观测噪声。

7.根据权利要求6所述的方法，其特征在于，所述根据所述修正先验概率密度函数获取当前目标观测时刻目标状态的第二后验概率密度函数的步骤具体包括：

根据所述均值和协方差P_p,1,k|k-1获取m个积分点具体如下式所示：

$X_{i,1,k|k-1}={\left(\sqrt{P_{p,1,k|k-1}}\right)}^T{\mathrm{\ξ}}_i+{\hat{x}}_{p,1,k|k-1}---\left(15\right)$

将所述积分点代入非线性的观测函数h(·)，以获取积分点对应的m个预测值具体如下式所示：

$z_{1,k|k-1}^i=h\left(X_{i,1,k|k-1}\right)---\left(16\right)$

获取所述积分点对应的m个预测值的均值具体如下式所示：

${\hat{z}}_{1,k|k-1}=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{w}_i{z}_{1,k|k-1}^i---\left(17\right)$

根据所述预测值和均值获取对应的观测误差协方差以及交叉协方差，具体如下所示：

$P_{\mathrm{zz},1,k|k-1}=R_k+\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{w}_i(z_{1,k|k-1}^i-{\hat{z}}_{1,k|k-1}){(z_{1,k|k-1}^i-{\hat{z}}_{1,k|k-1})}^T---\left(18\right)$

$P_{\mathrm{xz},1,k|k-1}=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{w}_i({\mathrm{\χ}}_{1,k|k-1}^i-{\hat{x}}_{p,1,k|k-1}){(z_{1,k|k-1}^i-{\hat{z}}_{1,k|k-1})}^T---\left(19\right)$

其中，P_zz,1,k|k-1为所述观测误差协方差，P_xz,1,k|k-1为所述交叉协方差；

根据所述观测误差协方差P_zz,1,k|k-1、交叉协方差P_xz,1,k|k-1以及k时刻的目标观测向量获取k时刻目标状态的第二后验概率密度函数对应的均值和协方差，具体如下所示：

${\hat{x}}_{u,1,k|k}={\hat{x}}_{p,1,k|k-1}+P_{\mathrm{xz},1,k|k-1}{P}_{\mathrm{zz},1,k|k-1}^{-1}(z_k-{\hat{z}}_{1,k|k-1})---\left(20\right)$

$P_{u,1,k|k}=P_{p,1,k|k}-P_{\mathrm{xz},1,k|k-1}{P}_{\mathrm{zz},1,k|k-1}^{-1}{P}_{\mathrm{xz},1,k|k-1}^T---\left(21\right)$

其中，为所述k时刻目标状态的第二后验概率密度函数对应的均值，P_u,1,k|k为所述k时刻目标状态的第二后验概率密度函数对应的协方差。

8.根据权利要求7所述的装置，其特征在于，所述根据所述第一后验概率密度函数以及第二后验概率密度函数获取当前目标观测时刻目标状态的联合后验概率密度函数的步骤具体包括：

根据所述第一后验概率密度函数以及第二后验概率密度函数各自对应的均值获取目标状态估计权值，具体如下所示：

${\mathrm{\μ}}_0\left({\hat{x}}_{u,0,k|k}\right)=\frac{1}{\sqrt{\left|P_{\mathrm{zz},0,k|k-1}\right|}}\·\exp (-\frac{{(z_k-h\left({\hat{x}}_{u,0,k|k}\right))}^2}{2})---\left(22\right)$

${\mathrm{\μ}}_1\left({\hat{x}}_{u,1,k|k}\right)=\frac{1}{\sqrt{\left|P_{\mathrm{zz},1,k|k-1}\right|}}\·\exp (-\frac{{(z_k-h\left({\hat{x}}_{u,1,k|k}\right))}^2}{2})---\left(23\right)$

${\mathrm{\α}}_k=\frac{{\mathrm{\μ}}_1\left({\hat{x}}_{u,1,k|k}\right)}{{\mathrm{\μ}}_0\left({\hat{x}}_{u,0,k|k}\right)+{\mathrm{\μ}}_1\left({\hat{x}}_{u,1,k|k}\right)}---\left(24\right)$

其中，α_k为所述目标状态估计权值；

根据所述第一后验概率密度函数、第二后验概率密度函数各自对应的均值、协方差以及目标状态估计权值α_k获取k时刻目标状态的联合后验概率密度函数，具体如下所示：

$p\left(x_k\right|z_{1:k})=N({\hat{x}}_{k|k},P_{k|k})---\left(25\right)$

${\hat{x}}_{k|k}={\mathrm{\α}}_k\·{\hat{x}}_{u,1,k|k}+(1-{\mathrm{\α}}_k)\·{\hat{x}}_{u,0,k|k}---\left(26\right)$

$P_{k|k}={\mathrm{\α}}_k\·[P_{u,1,k|k}+({\hat{x}}_{u,1,k|k}-{\hat{x}}_{k|k}){({\hat{x}}_{u,1,k|k}-{\hat{x}}_{k|k})}^T]+(1-{\mathrm{\α}}_k)\·[P_{u,0,k|k}+({\hat{x}}_{u,0,k|k}-{\hat{x}}_{k|k}){({\hat{x}}_{u,0,k|k}-{\hat{x}}_{k|k})}^T]---\left(27\right)$

其中，p(x_k|z_1:k)为所述k时刻目标状态的联合后验概率密度函数，为所述k时刻目标状态的联合后验概率密度函数的均值，P_k|k为所述k时刻目标状态的联合后验概率密度函数的协方差。

9.一种截断积分卡尔曼滤波装置，其特征在于，包括：

原始先验概率密度函数获取模块，用于根据高斯-厄米特积分获取当前目标观测时刻目标状态的原始先验概率密度函数；

第一后验概率密度函数获取模块，用于根据所述原始先验概率密度函数获取当前目标观测时刻目标状态的第一后验概率密度函数；

修正先验概率密度函数获取模块，用于根据当前目标观测时刻的目标观测向量修正所述原始先验概率密度函数，以获取修正先验概率密度函数；

第二后验概率密度函数获取模块，用于根据所述修正先验概率密度函数获取当前目标观测时刻目标状态的第二后验概率密度函数；

联合后验概率密度函数获取模块，用于根据所述第一后验概率密度函数以及第二后验概率密度函数获取当前目标观测时刻目标状态的联合后验概率密度函数。