CN103955892B - 一种目标跟踪方法及扩展截断无迹卡尔曼滤波方法、装置 - Google Patents

Abstract

本发明公开了一种目标跟踪方法、系统及扩展截断无迹卡尔曼滤波方法、装置，该扩展截断无迹卡尔曼滤波方法包括根据无迹变换获取原始先验概率密度函数；根据原始先验概率密度函数获取第一后验概率密度函数；根据统计线性回归理论以及当前目标观测时刻的目标观测向量修正原始先验概率密度函数，以获取修正先验概率密度函数；根据修正先验概率密度函数获取第二后验概率密度函数；根据原始先验概率密度函数以及第二后验概率密度函数获取联合后验概率密度函数。本发明能够解决观测函数不具有唯一反函数的问题，有效减少目标状态先验分布方差，自适应地根据观测信息的精度进行状态更新，有效提高滤波精度且实用性较高。

Description

一种目标跟踪方法及扩展截断无迹卡尔曼滤波方法、装置

技术领域

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

背景技术

在导航和制导系统、跟踪快速变化的无线信道的信道状态信息、跟踪飞机的实时位置等科学领域中经常需要使用滤波技术以实现对目标的实时跟踪。现有技术中常采用如下几种滤波方法：第一种为由卡尔曼于1960年提出的卡尔曼滤波（KF），该方法可以处理具有高斯分布噪声的线性系统以得到系统状态的最小均方差估计RMMSE；第二种为使用序贯蒙特卡洛方法（SMC），例如粒子滤波（PF）；第三种为卡尔曼滤波类型的算法，例如扩展卡尔曼滤波、无迹卡尔曼滤波、截断无迹卡尔曼滤波、积分卡尔曼滤波等。

本申请发明人在长期研发中发现，现有技术中第一种KF方法只能处理线性系统，而在现实情况中，一个动态系统的真实的过程和观测模型通常都是非线性的，且过程和观测噪声都是非高斯的；第二种例如粒子滤波方法其计算量随着采样粒子数的增加而剧增，一般很难进行实际应用；第三种方法中的扩展卡尔曼滤波会引入较大的近似误差以及引起发散现象，对于截断无迹卡尔曼滤波方法则由于要求当前时刻的观测函数是双向单射的，即要求观测函数具有唯一的反函数，而在实际情况中观测函数通常都不具有唯一的反函数，因此难以进行实际应用；此外扩展卡尔曼滤波、无迹卡尔曼滤波、积分卡尔曼滤波等方法在先验分布方差增大时，非线性的影响增加，观测函数的非线性更加明显，此时滤波性能受到较大影响而使得滤波精度较低。

发明内容

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

为解决上述技术问题，本发明的第一方面是：提供一种目标跟踪方法，包括：对目标进行感测以获得当前目标观测时刻的目标观测向量；根据无迹变换获取当前目标观测时刻目标状态的原始先验概率密度函数；根据原始先验概率密度函数获取当前目标观测时刻目标状态的第一后验概率密度函数；根据统计线性回归理论以及当前目标观测时刻的目标观测向量修正原始先验概率密度函数，以获取修正先验概率密度函数；根据修正先验概率密度函数获取当前目标观测时刻目标状态的第二后验概率密度函数；根据第一后验概率密度函数以及第二后验概率密度函数获取当前目标观测时刻目标状态的联合后验概率密度函数；利用目标状态的联合后验概率密度函数对目标状态进行估计，以获得当前目标观测时刻的目标状态估计值；输出当前目标观测时刻的目标状态估计值，以实现对目标的跟踪。

为解决上述技术问题，本发明的第二方面是：提供一种目标跟踪系统，包括：传感器以及扩展截断无迹卡尔曼滤波装置，传感器连接至扩展截断无迹卡尔曼滤波装置，传感器用于对目标进行感测以获得当前目标观测时刻的目标观测向量；扩展截断无迹卡尔曼滤波装置包括：原始先验概率密度函数获取模块，用于根据无迹变换获取当前目标观测时刻目标状态的原始先验概率密度函数；第一后验概率密度函数获取模块，用于根据原始先验概率密度函数获取当前目标观测时刻目标状态的第一后验概率密度函数；修正先验概率密度函数获取模块，用于根据统计线性回归理论以及当前目标观测时刻的目标观测向量修正原始先验概率密度函数，以获取修正先验概率密度函数；第二后验概率密度函数获取模块，用于根据修正先验概率密度函数获取当前目标观测时刻目标状态的第二后验概率密度函数；联合后验概率密度函数获取模块，用于根据第一后验概率密度函数以及第二后验概率密度函数获取当前目标观测时刻目标状态的联合后验概率密度函数；目标状态估计模块，用于利用目标状态的联合后验概率密度函数对目标状态进行估计，以获得当前目标观测时刻的目标状态估计值；目标状态估计值输出模块，用于输出当前目标观测时刻的目标状态估计值。

为解决上述技术问题，本发明的第三方面是：提供一种扩展截断无迹卡尔曼滤波方法，包括：根据无迹变换获取当前目标观测时刻目标状态的原始先验概率密度函数；根据原始先验概率密度函数获取当前目标观测时刻目标状态的第一后验概率密度函数；根据统计线性回归理论以及当前目标观测时刻的目标观测向量修正原始先验概率密度函数，以获取修正先验概率密度函数；根据修正先验概率密度函数获取当前目标观测时刻目标状态的第二后验概率密度函数；根据原始先验概率密度函数以及第二后验概率密度函数获取当前目标观测时刻目标状态的联合后验概率密度函数，完成扩展截断无迹卡尔曼滤波过程。

其中，根据无迹变换获取当前目标观测目标状态的原始先验概率密度函数的步骤具体包括：根据无迹变换获取N个Sigma点其中i＝1,2,...,N；

将Sigma点代入非线性状态转移函数，以获取Sigma点对应的N个预测值具体如下式所示：

${\mathrm{\χ}}_{0,k|k-1}^i=f\left({\mathrm{\χ}}_0^i\right)---\left(1\right)$

其中，f(·)为非线性状态转移函数；

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

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

$P_{p,0,k|k-1}=Q_k+\underset{i=1}{\overset{N}{\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(3\right)$

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

其中，根据原始先验概率密度函数获取当前目标观测时刻目标状态的第一后验概率密度函数的步骤具体包括：将预测值代入非线性观测函数，以获取预测值对应的N个观测值具体如下式所示：

$z_{0,k|k-1}^i=h\left({\mathrm{\χ}}_{0,k|k-1}^i\right)---\left(4\right)$

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

获取N个观测值的均值具体如下式所示：

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

根据均值获取对应的新息协方差P_zz,0,k|k-1以及交叉协方差P_xz,0,k|k-1，具体如下所示：

$P_{\mathrm{zz},0,k|k-1}=R_k+\underset{i=1}{\overset{N}{\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(6\right)$

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

其中，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(8\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(9\right)$

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

其中，根据统计线性回归理论以及当前目标观测时刻的目标观测向量修正原始先验概率密度函数，以获取修正先验概率密度函数的步骤具体包括：根据k时刻目标状态的原始先验概率密度函数获取误差协方差，具体如下式所示：

$P_{\mathrm{xx},0,k|k-1}=\underset{i=1}{\overset{N}{\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(10\right)$

其中，P_xx,0,k|k-1为误差协方差；

根据统计线性回归理论以及误差协方差P_xx,0,k|k-1获取非线性观测函数的线性回归参数H_k、d_k，具体如下所示：

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

$d_k={\hat{z}}_{0,k|k-1}-H_k{\hat{x}}_{p,0,k|k-1}---\left(12\right)$

根据线性回归参数H_k、d_k以及k时刻的目标观测向量z_k获取修正先验概率密度函数的均值和协方差，具体如下所示：

μ_a,1＝H_k*(z_k-d_k)    （13）

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

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

${\mathrm{\Σ}}_{a,1}=\∫(a_k-{\mathrm{\μ}}_1){(a_k-{\mathrm{\μ}}_1)}^T{p}_1(a_k;z_k)d{a}_k={\tilde{H}}^{-1}R{\left({\tilde{H}}^{-1}\right)}^T---\left(16\right)$

${\mathrm{\μ}}_{b,1}={\mathrm{\μ}}_{b,0}+{\mathrm{\Σ}}_{\mathrm{ab},0}^T{\mathrm{\Σ}}_{a,0}^{-1}({\mathrm{\μ}}_{a,1}-{\mathrm{\μ}}_{a,0})---\left(17\right)$

${\mathrm{\Σ}}_{\mathrm{ab},1}={\mathrm{\Σ}}_{a,1}{\left({\mathrm{\Σ}}_{a,0}^{-1}\right)}^T{\mathrm{\Σ}}_{\mathrm{ab},0}---\left(18\right)$

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

$\mathrm{\Γ}={\mathrm{\Σ}}_{b,0}-{\mathrm{\Σ}}_{\mathrm{ab},0}^T{\mathrm{\Σ}}_{a,0}^{-1}{\mathrm{\Σ}}_{\mathrm{ab},0}---\left(20\right)$

${\hat{x}}_{p,0,k|k-1}=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_i{\mathrm{\χ}}_{0,k|k-1}^i=\left[\begin{array}{c}{\mathrm{\μ}}_{a,0}\\ {\mathrm{\μ}}_{b,0}\end{array}\right]---\left(21\right)$

$\begin{array}{c}{P}_{p,0,k|k-1}=Q_k+\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_i({\mathrm{\χ}}_{0,k|k-1}^i-{\hat{x}}_{p,0,k|k-1}){\left({\mathrm{\χ}}_{0,k|k-1}^i{\hat{x}}_{p,0,k|k-1}\right)}^T\\ =\left[\begin{array}{cc}{\mathrm{\Σ}}_{a,0}& {\mathrm{\Σ}}_{\mathrm{ab},0}\\ {\left({\mathrm{\Σ}}_{\mathrm{ab},0}\right)}^T& {\mathrm{\Σ}}_{b,0}\end{array}\right]\end{array}---\left(22\right)$

其中，为修正先验概率密度函数的均值，P_p,1,k|k-1为修正先验概率密度函数的协方差，a为目标的状态分量。

其中，根据修正先验概率密度函数获取当前目标观测时刻目标状态的第二后验概率密度函数的步骤具体包括：

根据无迹变换以及修正先验概率密度函数的均值和协方差P_p,1,k|k-1获取N个Sigma点

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

$z_{1,k|k-1}^i=h\left({\mathrm{\χ}}_{1,k|k-1}^i\right)---\left(23\right)$

获取N个观测值的均值具体如下式所示：

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

根据均值获取对应的新息协方差P_zz,1,k|k-1以及交叉协方差P_xz,1,k|k-1，具体如下所示：

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

$P_{\mathrm{xz},1,k|k-1}=\underset{i=1}{\overset{N}{\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(26\right)$

根据新息协方差P_zz,1,k|k-1、交叉协方差P_xz,1,k|k-1以及k时刻的目标观测向量z_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(27\right)$

$P_{u,1,k|k}=P_{p,1,k|k-1}-P_{\mathrm{xz},1,k|k-1}{P}_{\mathrm{zz},1,k|k-1}^{-1}{P}_{\mathrm{xz},1,k|k-1}^T---\left(28\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(29\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(30\right)$

$\mathrm{\α}=\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(31\right)$

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

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

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

${\hat{x}}_{k|k}={\mathrm{\α}}_k\·{\hat{x}}_{u,1,k|k}+(1-{\mathrm{\α}}_k)\·{\hat{x}}_{u,0,k|k}---\left(33\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(34\right)$

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

为解决上述技术问题，本发明的第四方面是：提供一种扩展截断无迹卡尔曼滤波装置，包括：原始先验概率密度函数获取模块，用于根据无迹变换获取当前目标观测时刻目标状态的原始先验概率密度函数；第一后验概率密度函数获取模块，用于根据原始先验概率密度函数获取当前目标观测时刻目标状态的第一后验概率密度函数；修正先验概率密度函数获取模块，用于根据统计线性回归理论以及当前目标观测时刻的目标观测向量修正原始先验概率密度函数，以获取修正先验概率密度函数；第二后验概率密度函数获取模块，用于根据修正先验概率密度函数获取当前目标观测时刻目标状态的第二后验概率密度函数；联合后验概率密度函数获取模块，用于根据第一后验概率密度函数以及第二后验概率密度函数获取当前目标观测时刻目标状态的联合后验概率密度函数。

本发明的有益效果是：区别于现有技术的情况，本发明根据无迹变换获取当前目标观测时刻目标状态的原始先验概率密度函数；根据原始先验概率密度函数获取当前目标观测时刻目标状态的第一后验概率密度函数；根据统计线性回归理论以及当前目标观测时刻的目标观测向量修正原始先验概率密度函数，以获取修正先验概率密度函数，能够解决观测函数不具有唯一反函数的问题，有效减少目标状态先验分布方差；根据修正先验概率密度函数获取当前目标观测时刻目标状态的第二后验概率密度函数；根据原始先验概率密度函数以及第二后验概率密度函数获取当前目标观测时刻目标状态的联合后验概率密度函数，实现自适应地根据观测信息的精度进行状态更新，有效提高滤波精度且实用性较高。

附图说明

图1是本发明扩展截断无迹卡尔曼滤波方法一实施方式的流程图；

图2是本发明扩展截断无迹卡尔曼滤波方法一实施方式中根据无迹变换获取当前目标观测时刻目标状态的原始先验概率密度函数的流程图；

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

图4是本发明扩展截断无迹卡尔曼滤波方法一实施方式中根据统计线性回归理论以及当前目标观测时刻的目标观测向量修正原始先验概率密度函数，以获取修正先验概率密度函数的流程图；

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

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

图7是本发明扩展截断无迹卡尔曼滤波方法一实施方式中EKF、UKF、QKF、PF和ETUKF的均方根误差对比图；

图8是本发明扩展截断无迹卡尔曼滤波方法一实施方式中EKF、UKF、QKF、PF和ETUKF在不同观测噪声方差下的均方根误差对比图；

图9是本发明扩展截断无迹卡尔曼滤波装置一实施方式的原理框图；

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

具体实施方式

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

本发明的扩展截断无迹卡尔曼滤波ETUKF为从一系列的不完全及包含噪声的目标观测向量中，对目标在某一时刻的状态进行估计。本发明扩展截断无迹卡尔曼滤波方法的基本理论具体如下所述：

假设k时刻目标的状态向量为x_k＝[a_k ^T,b_k ^T]^T，并且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的后验概率密度函数对应为：

$\begin{array}{c}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\left(z_k\right|x_k)\·p_1(x_k;z_k)\end{array}$

其中：

$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：根据无迹变换获取当前目标观测时刻目标状态的原始先验概率密度函数；

无迹变换（Unscented Transformation，UT）是用于计算经过非线性变换的随机变量统计的一种方法，无迹变换不需要对非线性状态和测量模型进行线性化，而是对状态向量的概率密度函数进行近似化，近似化后的概率密度函数仍然是高斯的，但它表现为一系列选取好的Sigma采样点，其获取Sigma点及Sigma点对应权值的具体方法为：

假设x_k|k为一个n_x维随机向量，x_k|k的均值和协方差分别为P_k|k，根据下述公式获取(2n_a+1)个Sigma点χ_i和χ_i对应的权值w_i：

$\left\{\begin{array}{c}{\mathrm{\χ}}_0={\hat{x}}_{k|k},i=0\\ {\mathrm{\χ}}_i={\hat{x}}_{k|k}+{\left(\sqrt{(n_a+\mathrm{\κ})P_{k|k}}\right)}_i,i=1,...,n_a\\ {\mathrm{\χ}}_{i+n_a}={\hat{x}}_{k|k}-{\left(\sqrt{(n_a+\mathrm{\κ})P_{k|k}}\right)}_i,i=1,...,n_a\end{array}\right.$

$\left\{\begin{array}{cc}{w}_0=\mathrm{\κ}/(n_a+\mathrm{\κ})& i=0\\ w_i=1/\left[2(n_a+\mathrm{\κ})\right]& i=1,...,n_a\\ w_{i+n_x}=1/\left[2(n_a+\mathrm{\κ})\right]& i=1,...,n_a\end{array}\right.$

其中，κ是一个尺度参数，κ为满足(n_a+κ)≠0的任何数值，是均方根矩阵(n_a+κ)P_k|k的第i行或第j列，n_a为状态向量的维数。

本步骤根据无迹变换获取当前目标观测时刻目标状态的原始先验概率密度函数，请参阅图2，具体包括以下子步骤：

子步骤S111：根据无迹变换获取N个Sigma点；

假设在上一目标观测时刻（k-1时刻）系统的初始状态均值以及协方差P_k-1|k-1为已知，即k-1时刻目标状态的联合后验概率密度函数的均值和协方差P_k-1|k-1为已知，基于上述均值和协方差P_k-1|k-1根据无迹变换获取N个Sigma点其中i＝1,2,...,N，N＝2n_a+1，n_a＝n_x+n_v+n_w，n_x表示目标状态x的维数，n_v表示观测噪声v的维数，n_w表示过程噪声w的维数。

子步骤S112：将Sigma点代入非线性状态转移函数，以获取Sigma点对应的N个预测值；

将上述N个Sigma点分别代入非线性状态转移函数，以获取N个Sigma点对应的N个预测值具体如下式（1）所示：

${\mathrm{\χ}}_{0,k|k-1}^i=f\left({\mathrm{\χ}}_0^i\right)---\left(1\right)$

其中，f(·)为非线性状态转移函数。

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

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

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

$P_{p,0,k|k-1}=Q_k+\underset{i=1}{\overset{N}{\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(3\right)$

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

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

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

子步骤S121：将预测值代入非线性观测函数，以获取预测值对应的N个观测值；

将上述子步骤S112获取的预测值代入非线性观测函数，以获取N个预测值对应的N个观测值具体如下式（4）所示：

$z_{0,k|k-1}^i=h\left({\mathrm{\χ}}_{0,k|k-1}^i\right)---\left(4\right)$

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

子步骤S122：获取N个观测值的均值；

获取N个观测值的均值具体如下式（5）所示：

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

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

根据观测值的均值获取对应的新息协方差P_zz,0,k|k-1以及交叉协方差P_xz,0,k|k-1，具体如下式（6）、（7）所示：

$P_{\mathrm{zz},0,k|k-1}=R_k+\underset{i=1}{\overset{N}{\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(6\right)$

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

其中，R_k为观测噪声协方差。

子步骤S124：根据新息协方差、交叉协方差以及k时刻目标状态的原始先验概率密度函数获取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(8\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(9\right)$

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

步骤S13：根据统计线性回归理论以及当前目标观测时刻的目标观测向量修正原始先验概率密度函数，以获取修正先验概率密度函数；

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

根据假设1，由于观测函数h(·)是局部线性的，在点对观测函数h(·)进行一阶泰勒级数展开，定义为a_k的最大似然估计，并令这是因为根据当前目标观测，是最可能的目标状态，最大化似然函数可得到a，因此有：

$h\left(a\right)=h\left(\tilde{a}\left(z\right)\right)+\tilde{H}(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对应的均值μ_a,1和协方差Σ_a,1可以近似如下：

${\mathrm{\μ}}_{a,1}={\∫a}_k{p}_1(a_k;z_k){\mathrm{da}}_k={\tilde{a}}_k\left(z_k\right)$

${\mathrm{\Σ}}_{a,1}=\∫(a_k-{\mathrm{\μ}}_1){(a_k-{\mathrm{\μ}}_1)}^T{p}_1(a_k;z_k)d{a}_k={\tilde{H}}^{-1}R{\left({\tilde{H}}^{-1}\right)}^T$

其中，a为目标的状态分量。

另外，原始先验概率密度函数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{N}{\mathrm{\Σ}}}{w}_i{\mathrm{\χ}}_{0,k|k-1}^i=\left[\begin{array}{c}{\mathrm{\μ}}_{a,0}\\ {\mathrm{\μ}}_{b,0}\end{array}\right]$

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

因此修正先验概率密度函数p₁(·)的均值和协方差P_p,1,k|k-1分别如下所示：

${\hat{x}}_{p,1,k|k-1}=\left[\begin{array}{c}{\mathrm{\μ}}_{a,1}\\ {\mathrm{\μ}}_{b,1}\end{array}\right]$

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

由上述推导过程可以看出，要获取修正先验概率密度函数p₁(·)，观测函数h(·)必须为双向单射的，即要求观测函数h(·)具有唯一的反函数，而在实际情况中观测函数h(·)通常不具有唯一的反函数。针对上述观测函数为非双向单射的问题，本发明根据统计线性回归理论获取修正先验概率密度函数，请参阅图4，其具体包括以下子步骤：

子步骤S131：根据k时刻目标状态的原始先验概率密度函数获取误差协方差；

根据k时刻目标状态的原始先验概率密度函数获取误差协方差，具体如下式（10）所示：

$P_{\mathrm{xx},0,k|k-1}=\underset{i=1}{\overset{N}{\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(10\right)$

其中，P_xx,0,k|k-1为误差协方差。

子步骤S132：根据统计线性回归理论以及误差协方差获取非线性观测函数的线性回归参数；

为了获取上述反函数h^-1(z_k)，目标为将非线性函数近似成如下线性估计的形式其中H_k、d_k分别为由最小均方根误差准则确定的线性回归参数。本实施方式根据统计线性回归理论以及误差协方差P_xx,0,k|k-1获取非线性观测函数的线性回归参数H_k、d_k的过程具体如下所述。

对于线性回归参数H_k、d_k有：

{H_k,d_k}＝argminE(e_k ^Te_k)    （f）

其中，e_k为线性化误差，将e_k代入式（f），对d_k求导并令其为0，得到：

$\begin{array}{c}(-2)E(z_k-H_k{x}_k-d_k)=0\\ \⇔d_k={\stackrel{\‾}{z}}_k-H_k{\stackrel{\‾}{x}}_k\end{array}$

其中， ${\stackrel{\‾}{x}}_k=E\left(x_k\right)={\hat{x}}_{p,0,k|k-1},{\stackrel{\‾}{z}}_k=E\left(z_k\right)={\hat{z}}_{0,k|k-1}.$ 将d_k代入式（f），得到：

$e^{T}e={[(z_k-{\stackrel{\‾}{z}}_k)-H_k(x_k-{\stackrel{\‾}{x}}_k)]}^T[(z_k-{\stackrel{\‾}{z}}_k)-H_k(x_k-{\stackrel{\‾}{x}}_k)]$

对H_k求偏导并令其为0，得到：

$(-2)E\left\{\right[(z_k-{\stackrel{\‾}{z}}_k)-H_k(x_k-{\stackrel{\‾}{x}}_k)\left]\right[z_k-{\stackrel{\‾}{z}}_k\left]\right)\}=0$

进一步可得线性回归参数H_k、d_k，具体如下式（11）、（12）所示：

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

$d_k={\hat{z}}_{0,k|k-1}-H_k{\hat{x}}_{p,0,k|k-1}---\left(12\right)$

其中，P_xz,0,k|k-1即为上述公式（7）中的交叉协方差，P_xx,0,k|k-1即为上述公式（10）中的误差协方差，即为上述公式（5）中的均值，即为上述公式（2）中的均值。

子步骤S133：根据线性回归参数以及k时刻的目标观测向量获取修正先验概率密度函数的均值和协方差。

由上述论述可知：

μ_a,1＝∫a_kp₁(a_k;z_k)da_k＝h^-1(z_k)

进一步根据线性回归参数H_k、d_k以及k时刻的目标观测向量z_k获取修正先验概率密度函数的均值和协方差，具体如下式（13-22）所示：

μ_a,1＝H_k*(z_k-d_k)    （13）

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

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

${\mathrm{\Σ}}_{a,1}=\∫(a_k-{\mathrm{\μ}}_1){(a_k-{\mathrm{\μ}}_1)}^T{p}_1(a_k;z_k)d{a}_k={\tilde{H}}^{-1}R{\left({\tilde{H}}^{-1}\right)}^T---\left(16\right)$

${\mathrm{\μ}}_{b,1}={\mathrm{\μ}}_{b,0}+{\mathrm{\Σ}}_{\mathrm{ab},0}^T{\mathrm{\Σ}}_{a,0}^{-1}({\mathrm{\μ}}_{a,1}-{\mathrm{\μ}}_{a,0})---\left(17\right)$

${\mathrm{\Σ}}_{\mathrm{ab},1}={\mathrm{\Σ}}_{a,1}{\left({\mathrm{\Σ}}_{a,0}^{-1}\right)}^T{\mathrm{\Σ}}_{\mathrm{ab},0}---\left(18\right)$

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

$\mathrm{\Γ}={\mathrm{\Σ}}_{b,0}-{\mathrm{\Σ}}_{\mathrm{ab},0}^T{\mathrm{\Σ}}_{a,0}^{-1}{\mathrm{\Σ}}_{\mathrm{ab},0}---\left(20\right)$

${\hat{x}}_{p,0,k|k-1}=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_i{\mathrm{\χ}}_{0,k|k-1}^i=\left[\begin{array}{c}{\mathrm{\μ}}_{a,0}\\ {\mathrm{\μ}}_{b,0}\end{array}\right]---\left(21\right)$

$\begin{array}{c}{P}_{p,0,k|k-1}=Q_k+\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_i({\mathrm{\χ}}_{0,k|k-1}^i-{\hat{x}}_{p,0,k|k-1}){\left({\mathrm{\χ}}_{0,k|k-1}^i{\hat{x}}_{p,0,k|k-1}\right)}^T\\ =\left[\begin{array}{cc}{\mathrm{\Σ}}_{a,0}& {\mathrm{\Σ}}_{\mathrm{ab},0}\\ {\left({\mathrm{\Σ}}_{\mathrm{ab},0}\right)}^T& {\mathrm{\Σ}}_{b,0}\end{array}\right]\end{array}---\left(22\right)$

其中，为修正先验概率密度函数的均值，P_p,1,k|k-1为修正先验概率密度函数的协方差，a为目标的状态分量。

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

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

子步骤S141：根据无迹变换以及修正先验概率密度函数的均值和协方差获取N个Sigma点；

根据无迹变换以及修正先验概率密度函数的均值和协方差P_p,1,k|k-1获取N个Sigma点

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

将Sigma点代入非线性观测函数h(·)，以获取Sigma点对应的N个观测值具体如下式（23）所示：

$z_{1,k|k-1}^i=h\left({\mathrm{\χ}}_{1,k|k-1}^i\right)---\left(23\right)$

子步骤S143：获取N个观测值的均值；

获取N个观测值的均值具体如下式（24）所示：

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

其中，公式（24）中的w_i同样指代上述公式（2）、（3）等所示的权值w_i。

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

根据均值获取对应的新息协方差P_zz,1,k|k-1以及交叉协方差P_xz,1,k|k-1，具体如下式（25）、（26）所示：

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

$P_{\mathrm{xz},1,k|k-1}=\underset{i=1}{\overset{N}{\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(26\right)$

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

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

${\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(27\right)$

$P_{u,1,k|k}=P_{p,1,k|k-1}-P_{\mathrm{xz},1,k|k-1}{P}_{\mathrm{zz},1,k|k-1}^{-1}{P}_{\mathrm{xz},1,k|k-1}^T---\left(28\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})$

其中，Κ为一已知参数，σ²表示新息方差。

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

${\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(29\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(30\right)$

$\mathrm{\α}=\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(31\right)$

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

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

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

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

${\hat{x}}_{k|k}={\mathrm{\α}}_k\·{\hat{x}}_{u,1,k|k}+(1-{\mathrm{\α}}_k)\·{\hat{x}}_{u,0,k|k}---\left(33\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(34\right)$

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

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

以下将以一个例子对本发明的ETUKF方法的性能进行评估以及与现有的扩展卡尔曼滤波（EKF）、无迹卡尔曼滤波（UKF）、积分卡尔曼滤波(QKF)和粒子滤波（PF）的性能进行对比，具体如下所述。

该例子考虑单变量非平稳增长模型（UNGM），该模型的过程模型是高度非线性的，观测模型是非平稳的。非线性过程模型和观测模型可以写为：

$x_k=\mathrm{\α}{x}_{k-1}+\mathrm{\β}\frac{x_{k-1}}{1+x_{k-1}^2}+\mathrm{\γ}\cos \left(1.2(k-1)\right)+n_k$

$z_k=\left\{\begin{array}{cc}{\mathrm{\φ}}_2{x}_k^2+v_k& k\≤30\\ {\mathrm{\φ}}_1{x}_k^3-2+v_k& k>30\end{array}\right.$

其中n_k是均值为零方差为1的高斯噪声，v_k是均值为零方差为0.01的高斯噪声，β＝25、α＝0.5、γ＝8、φ₁＝0.2、φ₂＝0.05。在每次蒙特卡洛仿真中，假设状态x₀的初始分布式是[0 1]上的均匀分布。

图7给出了本发明的ETUKF与现有的EKF、UKF、QKF、PF的均方根误差对比结果。从图7中可以看出ETUKF的性能明显优于EKF、UKF和QKF，ETUKF的性能与PF接近。导致EKF、UKF和QKF的性能较差的一个原因是高度非线性的过程模型和非平稳的观测模型所产生的近似误差的增加。

图8给出了不同观测噪声方差下本发明的ETUKF与现有的EKF、UKF、QKF、PF的均方根误差对比结果。由图8中可以看出，当观测信息量非常丰富或不丰富时，本发明的ETUKF在所有情况下都具有鲁棒性。ETUKF的性能明显优于EKF、UKF和QKF，ETUKF的性能与PF接近。

此外，下表1给出了上述各个滤波方法的计算时间。

表1 不同滤波方法计算时间的对比

上述各个方法各执行了100次蒙特卡洛仿真。由表1可以看出，对于一维UNGM，PF的计算时间比EKF、UKF、QKF和ETUKF高得多，ETUKF和QKF所用时间接近。然而，在四维BOT的情况下，QKF的计算时间（117.847s）比ETUKF高得多（30.913s），造成该情况的一个主要原因是：QKF遭受维数灾难，其高斯-厄米特积分点的数目随着状态向量的维数的增加呈指数增长。在各个方法中，EKF计算最快。但是，对照上表1和图7或图8，可以发现在计算效率这方面本发明的ETUKF优于现有的EKF、UKF、QKF、PF。

可以理解，本发明扩展截断无迹卡尔曼滤波方法一实施方式根据当前目标观测向量对原始先验概率密度函数进行修正，能够减少目标状态先验分布方差，有效解决观测函数的非线性对滤波精度的影响，提高滤波精度；根据统计线性回归理论而获取修正先验概率密度函数，能够解决传统滤波算法进行观测更新时要求观测函数必须具有唯一反函数的问题，能够进行实际应用；在联合后验概率密度函数中引入目标状态估计权值，实现自适应地根据观测信息的精度进行状态更新，能够有效提高滤波精度，提高对目标的跟踪性能；此外，本发明的扩展截断无迹卡尔曼滤波方法并不基于粒子滤波，计算量较简单，实用性较高。

请参阅图9，本发明实施方式还提供一种扩展截断无迹卡尔曼滤波装置，该扩展截断无迹卡尔曼滤波装置具体包括以下各个模块：

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

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

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

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

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

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

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

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

根据无迹变换获取当前目标观测时刻目标状态的原始先验概率密度函数。

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

根据统计线性回归理论以及当前目标观测时刻的目标观测向量修正原始先验概率密度函数，以获取修正先验概率密度函数。

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

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

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

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

请参阅图10，本发明实施方式还提供一种目标跟踪系统，该目标跟踪系统包括传感器31以及扩展截断无迹卡尔曼滤波装置32，传感器31连接至扩展截断无迹卡尔曼滤波装置32。

传感器31用于对目标进行感测以获得当前目标观测时刻的目标观测向量，其中传感器31具体可为红外、雷达等。

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

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

Claims (8)

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

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

根据无迹变换获取当前目标观测时刻目标状态的原始先验概率密度函数；

将预测值代入非线性观测函数，以获取预测值对应的N个观测值其中，h(·)为所述非线性观测函数；

获取所述N个观测值的均值 ${\hat{z}}_{0,k|k-1}=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_i{z}_{0,k|k-1}^i;$

根据所述均值获取对应的新息协方差P_zz,0,k|k-1以及交叉协方差P_xz,0,k|k-1；其中， $P_{\mathrm{zz},0,k|k-1}=R_k+\underset{i=1}{\overset{N}{\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,$ $P_{\mathrm{xz},0,k|k-1}=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_i({\mathrm{\χ}}_{0,k|k-1}^i-{\hat{x}}_{p,0,k|k-1}){(z_{0,k|k-1}^i-{\hat{z}}_{0,k|k-1})}^T,$ R_k为观测噪声协方差，w_i为(2n_a+1)个Sigma点χ_i和χ_i对应的权值；

根据所述新息协方差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})$ 以及协方差 $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;$ 其中，z_k为k时刻的目标观测向量，为k时刻目标状态的原始先验概率密度函数对应的均值；

根据统计线性回归理论以及所述当前目标观测时刻的目标观测向量修正所述原始先验概率密度函数，以获取修正先验概率密度函数；

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

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

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

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

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

传感器以及扩展截断无迹卡尔曼滤波装置，所述传感器连接至扩展截断无迹卡尔曼滤波装置，所述传感器用于对目标进行感测以获得当前目标观测时刻的目标观测向量；

所述扩展截断无迹卡尔曼滤波装置包括：

原始先验概率密度函数获取模块，用于根据无迹变换获取当前目标观测时刻目标状态的原始先验概率密度函数；

第一后验概率密度函数获取模块，用于将预测值代入非线性观测函数，以获取预测值对应的N个观测值其中，h(·)为所述非线性观测函数；获取所述N个观测值的均值根据所述均值获取对应的新息协方差P_zz,0,k|k-1以及交叉协方差P_xz,0,k|k-1；其中，

$P_{\mathrm{zz},0,k|k-1}=R_k+\underset{i=1}{\overset{N}{\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,$

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

R_k为观测噪声协方差，w_i为(2n_a+1)个Sigma点χ_i和χ_i对应的权值；以及根据所述新息协方差P_zz,0,k|k-1、交叉协方差P_xz,0,k|k-1以及k时刻目标状态的原始先验概率密度函数获取k时刻目标状态的第一后验概率密度函数

对应的均值x ${\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})$ 以及协方差 $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;$ 其中，z_k为k时刻的目标观测向量，

为k时刻目标状态的原始先验概率密度函数对应的均值；

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

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

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

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

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

3.一种扩展截断无迹卡尔曼滤波方法，其特征在于，包括：

根据无迹变换获取当前目标观测时刻目标状态的原始先验概率密度函数；

将预测值代入非线性观测函数，以获取预测值对应的N个观测值其中，h(·)为所述非线性观测函数；

获取所述N个观测值的均值 ${\hat{z}}_{0,k|k-1}=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_i{z}_{0,k|k-1}^i;$

根据所述均值获取对应的新息协方差P_zz,0,k|k-1以及交叉协方差P_xz,0,k|k-1；其中， $P_{\mathrm{zz},0,k|k-1}=R_k+\underset{i=1}{\overset{N}{\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,$ $P_{\mathrm{xz},0,k|k-1}=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_i({\mathrm{\χ}}_{0,k|k-1}^i-{\hat{x}}_{p,0,k|k-1}){(z_{0,k|k-1}^i-{\hat{z}}_{0,k|k-1})}^T,$ R_k为观测噪声协方差，w_i为(2n_a+1)个Sigma点χ_i和χ_i对应的权值；

根据所述新息协方差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})$ 以及协方差 $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;$ 其中，z_k为k时刻的目标观测向量，为k时刻目标状态的原始先验概率密度函数对应的均值；

根据统计线性回归理论以及当前目标观测时刻的目标观测向量修正所述原始先验概率密度函数，以获取修正先验概率密度函数；

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

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

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

根据无迹变换获取N个Sigma点其中i＝1,2,…,N；

将所述Sigma点代入非线性状态转移函数，以获取Sigma点对应的N个预测值具体如下式所示：

${\mathrm{\χ}}_{0,k|k-1}^i=f\left({\mathrm{\χ}}_0^i\right)---\left(1\right)$

其中，f(·)为所述非线性状态转移函数；

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

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

$P_{p,0,k|k-1}=Q_k+\underset{i=1}{\overset{N}{\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(3\right)$

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

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

根据所述k时刻目标状态的原始先验概率密度函数获取误差协方差，具体如下式所示：

$P_{\mathrm{xx},0,k|k-1}=\underset{i=1}{\overset{N}{\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(10\right)$

其中，P_xx,0,k|k-1为所述误差协方差；

根据统计线性回归理论以及误差协方差P_xx,0,k|k-1获取非线性观测函数的线性回归参数H_k、d_k，具体如下所示：

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

$d_k={\hat{z}}_{0,k|k-1}-H_k{\hat{x}}_{p,0,k|k-1}---\left(12\right)$

根据所述线性回归参数H_k、d_k以及k时刻的目标观测向量z_k获取修

正先验概率密度函数的均值和协方差，具体如下所示：

μ_a,1＝H_k*(z_k-d_k)  (13)

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

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

${\mathrm{\Σ}}_{a,1}=\∫(a_k-{\mathrm{\μ}}_1){(a_k-{\mathrm{\μ}}_1)}^T{p}_1(a_k;z_k){\mathrm{da}}_k={\tilde{H}}^{-1}R{\left({\tilde{H}}^{-1}\right)}^T---\left(16\right)$

${\mathrm{\μ}}_{b,1}={\mathrm{\μ}}_{b,0}+{\mathrm{\Σ}}_{\mathrm{ab},0}^T{\mathrm{\Σ}}_{a,0}^{-1}({\mathrm{\μ}}_{a,1}-{\mathrm{\μ}}_{a,0})---\left(17\right)$

${\mathrm{\Σ}}_{\mathrm{ab},1}={\mathrm{\Σ}}_{a,1}{\left({\mathrm{\Σ}}_{a,0}^{-1}\right)}^T{\mathrm{\Σ}}_{\mathrm{ab},0}---\left(18\right)$

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

$\mathrm{\Γ}={\mathrm{\Σ}}_{b,0}-{\mathrm{\Σ}}_{\mathrm{ab},0}^T{\mathrm{\Σ}}_{a,0}^{-1}{\mathrm{\Σ}}_{\mathrm{ab},0}---\left(20\right)$

${\hat{x}}_{p,0,k|k-1}=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_i{\mathrm{\χ}}_{0,k|k-1}^i=\left[\begin{array}{c}{\mathrm{\μ}}_{a,0}\\ {\mathrm{\μ}}_{b,0}\end{array}\right]---\left(21\right)$

$\begin{array}{c}{P}_{p,0,k|k-1}=Q_k+\underset{i=1}{\overset{N}{\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[\begin{array}{cc}{\mathrm{\Σ}}_{a,0}& {\mathrm{\Σ}}_{\mathrm{ab},0}\\ {\left({\mathrm{\Σ}}_{\mathrm{ab},0}\right)}^T& {\mathrm{\Σ}}_{b,0}\end{array}\right]\end{array}---\left(22\right)$

其中，为所述修正先验概率密度函数的均值，P_p,1,k|k-1为所述修正先验概率密度函数的协方差，a为目标的状态分量，a_k为目标的状态分量，z_k为k时刻的目标观测向量，μ_a,1和∑_a,1的分别为修正先验概率密度函数p₁(·)中目标的状态分量a对应的均值和协方差，μ_b,0和∑b,0分别为修正先验概率密度函数p₁(·)中目标的状态分量b对应的均值和协方差，μ_a,0和∑a,0分别为原始先验概率密度函数p₀(x_k)＝p₀(a_k,b_k)中目标的状态分量a对应的均值和协方差，μ_b,1和∑b,1分别为修正先验概率密度函数p₁(·)中目标的状态分量b对应的均值和协方差μ_ab,1和∑ab,1分别为修正先验概率密度函数p₁(·)中目标的状态分量a、b对应的均值和协方差。

6.根据权利要求5所述的方法，其特征在于，所述根据所述修正先

验概率密度函数获取当前目标观测时刻目标状态的第二后验概率

密度函数的步骤具体包括：

根据无迹变换以及修正先验概率密度函数的均值和协方差P_p,1,k|k-1获取N个Sigma点

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

$z_{1,k|k-1}^i=h\left({\mathrm{\χ}}_{1,k|k-1}^i\right)---\left(23\right)$

获取所述N个观测值的均值具体如下式所示：

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

根据所述均值获取对应的新息协方差P_zz,1,k|k-1以及交叉协方差P_xz,1,k|k-1，具体如下所示：

$P_{\mathrm{zz},1,k|k-1}=R_k+\underset{i=1}{\overset{N}{\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(25\right)$

$P_{\mathrm{xz},1,k|k-1}=\underset{i=1}{\overset{N}{\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(26\right)$

根据所述新息协方差P_zz,1,k|k-1、交叉协方差P_xz,1,k|k-1以及k时刻的目标观测向量z_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(27\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(28\right)$

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

7.根据权利要求6所述的方法，其特征在于，所述根据所述原始先

验概率密度函数以及第二后验概率密度函数获取当前目标观测时

刻目标状态的联合后验概率密度函数的步骤具体包括：

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

${\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(29\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(30\right)$

$\mathrm{\α}=\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(31\right)$

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

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

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

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

其中，p(x_k|z_1:k)为所述k时刻目标状态的联合后验概率密度函数，为所述k时刻目标状态的联合后验概率密度函数的均值，P_k|k为所述k时刻目标状态的联合后验概率密度函数的协方差，和P_k|k分别为n_x维随机向量x_k|k的均值和协方差，P_u,0,k|k为k时刻目标状态的第一后验概率密度函数对应的协方差，P_u,1,k|k为k时刻目标状态的第二后验概率密度函数对应的协方差。

8.一种扩展截断无迹卡尔曼滤波装置，其特征在于，包括：

原始先验概率密度函数获取模块，用于根据无迹变换获取当前目标观测时刻目标状态的原始先验概率密度函数；

第一后验概率密度函数获取模块，用于将预测值代入非线性观测函数，以获取预测值对应的N个观测值其中，h(·)为所述非线性观测函数；获取所述N个观测值的均值 获取对应的新息协方差P_zz,0,k|k-1以及交叉协方差P_xz,0,k|k-1；其中，

$P_{\mathrm{zz},0,k|k-1}=R_k+\underset{i=1}{\overset{N}{\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,$

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

R_k为观测噪声协方差，w_i为(2n_a+1)个Sigma点χ_i和χ_i对应的权值；以及根据所述新息协方差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})$ 以及协方差 $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;$ 其中，z_k为k时刻的目标观测向量，为k时刻目标状态的原始先验概率密度函数对应的均值；

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

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

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