具体实施方式

现在将参考附图更全面地描述示例实施方式。然而，示例实施方式 能够以多种形式实施，且不应被理解为限于在此阐述的实施方式；相反， 提供这些实施方式使得本公开将全面和完整，并将示例实施方式的构思 全面地传达给本领域的技术人员。图中相同的附图标记表示相同或类似 的结构，因而将省略它们的详细描述。此外，附图仅为本公开的示意性 图解，并非一定是按比例绘制。

虽然本说明书中使用相对性的用语，例如“上”“下”来描述图标 的一个组件对于另一组件的相对关系，但是这些术语用于本说明书中仅 出于方便，例如根据附图中所述的示例的方向。能理解的是，如果将图 标的装置翻转使其上下颠倒，则所叙述在“上”的组件将会成为在“下” 的组件。当某结构在其它结构“上”时，有可能是指某结构一体形成于 其它结构上，或指某结构“直接”设置在其它结构上，或指某结构通过 另一结构“间接”设置在其它结构上。

用语“一个”、“一”、“该”、“所述”和“至少一个”用以表 示存在一个或多个要素/组成部分/等；用语“包括”和“具有”用以表 示开放式的包括在内的意思并且是指除了列出的要素/组成部分/等之外 还可存在另外的要素/组成部分/等；用语“第一”、“第二”和“第三”等仅作为标记使用，不是对其对象的数量限制。

由于数据截断可以视为一种特殊的非线性，因此可以考虑一些非线 性递归滤波器如EKF、UKF和PF等。

EKF在工作点附近对非线性系统进行线性化，EKF的做法有两点： 第一，在工作点附近对系统进行线性近似化；第二，在线性系统近似下， 把噪声项和状态都当成高斯分布，这样只要估计它们的均值和协方差矩 阵就可以描述状态了，后续工作和卡尔曼滤波是一样的。即EKF给出的 公式和卡尔曼是一致的，用线性化之后的矩阵去代替卡尔曼滤波器的转 移矩阵和观测矩阵。但是由于在截断区域观测方程具有明显的不连续性， 在不连续性处不存在梯度，因此EKF无法有效处理截断数据问题。

UKF摒弃了对非线性函数进行线性化的传统做法，采用卡尔曼线性 滤波框架，对于一步预测方程，使用无迹变换(UT)来处理均值和协方 差的非线性传递问题。UKF算法是对非线性函数的概率密度分布进行近 似，用一系列确定样本来逼近状态的后验概率密度，而不是对非线性函 数进行近似，不需要对Jacobian矩阵进行求导。UKF没有把高阶项忽略，因此对于非线性分布的统计量有较高的计算精度。UKF虽然是一种计算 量较小的方法，然而当测量值接近截断区域时，该方法不鲁棒。

PF采用了一种用大量采样点去描述这个分布的方法，就是一个不断 采样——算权重——重采样的过程，越是符合观测的粒子拥有约大的权 重，而权重越大就越容易在重采样时被采到。PF在计算上比EKF和UKF 的代价多得多，因为它需要使用一组称为粒子的加权样本来生成后验分 布。此外，Tobit模型中的不连续性意味着需要大量粒子才能对该区域中 的系统进行充分建模，导致采样所需的粒子数量、随分布呈指数增长， 所以仅限于低维的问题，高维的基本就没办法了。

针对上述问题，本公开实施方式提供一种处理截断数据的卡尔曼滤 波方法，该滤波方法能够适用于非线性系统存在数据截断的情况。图1 所示为本公开处理截断数据的卡尔曼滤波方法的流程图，该方法包括如 下步骤：

S110、使用无迹卡尔曼滤波UKF算法计算待测系统的一步预测状态 值和协方差；

S120、使用修正观测方程基于待测系统的一步预测状态值确定待测 系统的预测观测值；

S130、基于待测系统的预测观测值和协方差确定待测系统的卡尔曼 增益；

S140、基于卡尔曼增益对待测系统进行状态更新和测量值更新。

本公开滤波方法，通过对待测系统使用UKF算法得到待测系统的一 步预测状态值和协方差，在一步预测状态值的基础上可获取到待测系统 的新的采样点，将这些采样点代入本公开提供的修正观测方程可计算得 到待测系统的预测观测值，然后根据待测系统的预测观测值和待测系统 的协方差可计算得到待测系统的卡尔曼增益，再使用卡尔曼增益可实现 对待测系统进行状态更新和测量值更新。本公开方法通过对观测方程进 行修正，修正后的观测方程将截断融入统计特性中，噪声不再是高斯分 布，而是截断的高斯分布，因此观测值也服从截断的高斯分布。从而使 得本公开方法能够适用于非线性系统，且能够有效处理非线性系统的数 据截断问题。下面对本示例性实施例中的各步骤进行具体介绍。

在步骤S110中，是要通过现有的UKF滤波算法来计算待测系统的 一步预测状态值和协方差。待测系统可以为待追踪的目标物体，具体根 据使用场景进行确定。无迹卡尔曼滤波UKF是一种非线性滤波方法。它 使用无迹变换来处理均值和协方差的非线性传递问题。图2所示为无迹 变换的原理的示意图，其基本原理为：结合原始状态分布特征按某一规则选取一些采样点，即Sigma点集，使这些采样点的均值和协方差等于 原状态分布的均值和协方差。将这些点代入非线性函数中，相应得到非 线性函数值点集，通过这些点集即可求取变换后的均值和协方差。因此 无迹变换使得非线性问题在处理上变得更简单直观而且保持了相对EKF 等非线性卡尔曼滤波器较好的精度。图3示出了使用UKF算法计算待测系统的一步状态预测值和协方差的具体步骤，包括：

S310、使用无迹变换获取待测系统的多个第一采样点，并计算各第 一采样点的权重；

S320、计算各第一采样点的一步预测状态值；

S330、基于各第一采样点的一步预测状态值以及各第一采样点的权 重确定待测系统的一步预测状态值和协方差。

在步骤S310中，是要通过UT变换得到一组sigma点集，即多个第 一采样点，并计算各第一采样点的权重，应当理解的是，这里的权重包 括待测系统的状态均值权重和协方差权重。本示例性实施例中，可采用 性能较好的比例对称采样方法进行UT变换得到待测系统的sigma点集。 若n为状态向量的维数，则可以获取到如下所示的2n+1个sigma采样点：

式中：为第i个第一采样点；为系统的状态均值；i为矩 阵的列数；为矩阵平方根的第i列。

并可以基于如下公式计算待测系统的各sigma采样点的权重，各 sigma采样点的权重用于后续步骤计算系统的卡尔曼增益。本步骤中的 各sigma采样点的权重包括系统状态权重和协方差权重，计算公式如下：

式中：为系统状态权重，为为协方差权重，上标为 第几个采样点；参数λ＝a²(n+k)-n是一个缩放比例参数，用来降低总的 预算误差，a的选取控制了采样点的分布状态，k为待选参数；待选参数 β为非负的权系数。

在步骤S320中，是要计算各第一采样点的一步预测状态值，以通过 sigma点集的一步预测状态值预测系统的下一状态。各第一采样点的一 步预测状态值可表示为：

式中：为一步预测状态值，为上一状态值，f(·)为系统的 状态更新函数。

在步骤S330中，是要进一步确定待测系统的一步状态预测值和协方 差。具体可通过如下公式基于各第一采样点的一步预测状态值以及各第 一采样点的权重计算待测系统的一步预测状态值：

式中：为系统的一步预测状态值，ω_m ⁽ⁱ⁾为第i个采样点的权重， 为第i个采样点的一步预测状态值。

以及，使用如下公式基于各第一采样点的一步预测状态值以及各第 一采样点的权重计算待测系统的协方差矩阵：

式中：P_k|k-1为待测系统的协方差矩阵，ω_c ⁽ⁱ⁾为第i个采样点的权重， 为系统的一步预测状态值，为第i个采样点的一步预测状态值， Q为常量。

在经过步骤S310～S330后，实现了根据UKF算法对待测系统进行 了一步状态预测，以为后续步骤进行系统状态更新提供基础。

在步骤S120中，是要在系统状态的一步预测基础上，通过UT变换， 得到新的sigma点集，即多个第二采样点，然后将多个第二采样点代入 本公开提供的修正观测方程而得到待测系统的预测观测值，相当于在本 步骤实现了将测量观测值用于卡尔曼滤波，得到预测观测值。值得注意 的是，本公开正是通过对系统的观测方程进行修正，使用修正后的观测方程能够有效处理非线性系统的数据截断问题，实现了对现有的UKF滤 波算法的功能扩展。图4示出了确定待测系统的预测观测值的流程图， 包括：

S410、对待测系统的一步预测状态值使用无迹变换获取待测系统的 多个第二采样点；

S420、使用修正观测方程基于多个第二采样点确定待测系统的预测 观测值。

其中，在步骤S410中，是要使用UT变换对得到的待测系统的一步 预测状态进行再次采样，得到新的sigma点集，即多个第二采样点。应 当理解的是，本步骤同样可以采用采样性能较好的比例对称采样法获取 得到多个第二采样点。所得到的多个第二采样点可通过下式表示：

式中：为第i个第二采样点；为系统的一步预测的状态均 值；i为矩阵的列数；为矩阵平方根的第i列。

在步骤S420中，具体是通过将步骤S410中产生的多个第二采样点 代入本公开提供的修正观测方程，以得到待测系统的预测观测值。

对于左截断类型的Tobit模型引入UKF算法的基本方程后，其非线 性系统可表示为：

由此，本公开的修正观测方程可表示为：

Y_k＝p_k(h(x_k)+v_k)+(I_m×m-p_k)τ (38)

式中：Y_k为系统的观测测量值，p_k为伯努利随机变量矩阵，h(x_k)+v_k为系统的原观测方程，(I_m×m-p_k)τ为截断的高斯噪声。

在将第二采样点代入上述修正观测方程(38)后，可得到如下计算 待测系统的预测观测值的表达式：

式中：为系统的观测测量值，p_k为伯努利随机变量矩阵， 为系统的原观测方程，(I_m×m-p_k)τ为截断的高斯噪声。

下面对本公开中的修正观测方程的推导过程进行介绍。卡尔曼滤波 的基本方程为：

状态方程：x_k＝f(x_k-1)+w_k-1 (2)

观测方程：y_k＝h(x_k)+v_k (3)

其中，x是状态值，y是测量值，角标k表示递归顺序(或时间顺序)； f(·)是状态更新函数，h(·)是观测函数；w是服从零均值高斯分布的状态 噪声，其方差为Q，v是服从零均值高斯分布观测噪声，其方差为R。

在面对截断数据问题时，可通过引入Tobit模型进行解决。为表述方 便，下面仅以左截断类型的Tobit模型为例进行介绍，其它Tobit类型可 类比左截断Tobit模型进行推导。左截断类型的Tobit模型的一般描述如 下：

其中，是没有截断的观测值，y_t是被截断的真实观测值，τ是数 据截断阈值，其值由具体环境决定。

可以看到，由Tobit模型引起的截断是一种独特的非线性类型，在截 断区域的斜率为零，且在截断点存在不连续性。将Tobit模型引入卡尔曼 滤波的基本方程后得到新的方程组如下：

状态方程：x_k＝f(x_k-1)+w_k-1 (6)

观测方程：

左截断类型的Tobit模型：

由于数据截断，在截断区域观测噪声不再服从高斯分布，而且由于 状态值决定着截断现象的发生，因此噪声与状态值相关。在数据截断的 情况下违背了卡尔曼滤波的噪声与状态值无关的假设，直接使用卡尔曼 滤波器处理截断数据将不再是无偏的。可根据Tobit模型对卡尔曼滤波进 行观测方程修正，具体地，为了表示截断测量y_k∈R^m×1，可引入伯努利随 机变量矩阵p_k∈R^m×m，其中：

基于公式上述公式(6)～(9)，可得到卡尔曼滤波的修正观测方程：

y_k＝p_k(h(x_k)+v_k)+(I_m×m-p_k)τ (10)

将公式(10)所示的修正观测方程应用于公式(17)所示的非线性 系统，可得到如公式(38)所示的非线性系统的修正观测方程。

本公开通过对观测方程进行修正，修正后的观测方程将截断融入统 计特性中，噪声不再是高斯分布，而是截断的高斯分布，因此观测值也 服从截断的高斯分布，从而可用于处理截断数据。

在步骤S130中，是要确定待测系统的卡尔曼增益，图5示出了卡尔 曼增益的确定方法流程图，包括：

S510、基于待测系统的预测观测值和协方差确定待测系统的预测量 均值；

S520、基于待测系统的预测量均值和协方差确定待测系统的预测量 方差；

S530、基于待测系统的预测量方差确定待测系统的卡尔曼增益。

其中，在步骤S510中，可通过如下公式计算待测系统的预测量均值：

式中：为系统的预测量均值，ω_m ⁽ⁱ⁾为第i个采样点的均值权重， 为系统的观测测量值。

在步骤S520中，可通过如下公式计算待测系统的预测量方差，包括 预测量方差P_yy和预测量协方差P_xy和：

式中：P_yy为预测量方差，为系统的观测测量值，为系统的 预测量均值，P_xy为预测量协方差，为第i个第二采样点，为系 统的状态均值，ω_c ⁽ⁱ⁾为第i个采样点的协方差权重。

上述公式(26)中，为观测噪音的期望，其中p_k为伯努利 随机变量，v_k∈R^m×1是零均值高斯白噪声，其协方差分别为R∈R^m×m，假设 过程的状态噪声和观测噪声相互独立，状态和噪声相互独立，状态噪声 和观测噪声分别在时间上也是独立的。伯努利随机变量可根据如下公式 计算：

E(p_k(l))就是真实观测值落入未截断区域的概率，假设观测噪声是对 角形式，则有：

需要说明的是，在步骤S520是在UKF算法的基础上，通过引入Tobit 模型实现了对测量观测值和观测噪声协方差的统计特性的修正，观测噪 声的统计特性的改变可以近似等效为噪声前乘了一个“截断的观测矩阵”。 从而使用截断值中存在的信息使得真实值得以很好地收敛，提高了估计 性能，在模型中包含截断信息可以很好的改善状态估计。

在得到预测量的协方差P_xy和方差P_yy后，在步骤S530中，可根据如 下公式计算待测系统的卡尔曼增益：

在得到卡尔曼增益后，在步骤S140中，可通过如下公式对待测系统 进行状态更新：

以及使用如下公式基于卡尔曼增益对待测系统进行测量值更新：

式中：为系统的预测状态，为系统的上一预测状态，K_k为卡 尔曼增益，为系统的预测量值，为系统的上一预测量值；P_k|k为更 新后的协方差矩阵，P_k|k-1为待测系统的协方差矩阵，P_ykyk为系统的预测量 方差。

本公开提供的滤波方法在检测范围受限的目标跟踪的问题中能够表 现出比经典UKF更良好的性能，该滤波方法受非线性较强且截断区域的 影响较小，能够对目标保持比较好的跟踪能力。下面结合一具体示例对 本公开滤波方法的应用效果作进一步介绍。

示例性的，在一雷达追踪示例中，假设质点M在二维平面x-y内做 匀加速运动。其在某一时刻k的位置、速度和加速度可用矢量 表示。水平方向(x轴方向)和垂直方向(y轴方 向)两方向上运动都具有加性系统噪声W(k)，则在笛卡儿坐标系下该质 点的运动状态方程为：

X(k+1)＝ΦX(k)+W(k) (31)

式中：

假设坐标位置在原点的雷达对质点M进行跟踪，雷达能够有效检测 目标的范围为半径为D的圆域。雷达和质点M之间的距离记为r_k，质点 M相对于雷达的角度为实际测量中雷达具有加性测量噪声V(k)，在 以雷达为中心的坐标系下，观测方程为：

测量值为：

仿真中假设雷达检测范围D为5200m，系统噪声W(k)具有协方差阵 Q_k，V(k)具有协方差阵R_k，分别如下所示：

W、V二者不相关，观测次数N＝120，采样时间为T＝0.5s。初始状态 X(0)＝[1000；4500；40；150；2；-10]^T，初始状态协方差矩阵为：

图6示出了本公开滤波方法与经典UKF滤波方法对目标的跟踪位置 误差对比，图中，横坐标表示观测次数，纵坐标表示距离偏差，第一曲 线701为本公开滤波方法对目标的跟踪曲线，第二曲线702为是用经典 UKF滤波方法对目标的跟踪曲线，由仿真结果可得，使用本公开提供的 滤波算法在对检测范围受限的目标进行追踪时，其在非线性比较强的截断区域附件能够依然保持比较好的跟踪能力，其受非线性强且截断区域 的影响较小，其在截断区域的跟踪偏差最大不超过25m，而传统的UKF 滤波方法在非线性比较强的截断区域附件表现出明显的不稳定性，其在 截断区域的跟踪偏差最大可大90m，且表现出明显的不稳定性。此外， 使用本公开滤波算法对目标进行跟踪时，每一次对目标的追踪结果都优 于经典UKF滤波方法对目标的追踪结果。可以看出，使用本公开提供的 滤波方法在检测范围受限的目标追踪时具有更好的性能。

此外，在本公开的示例性实施例中，还提供了一种能够实现上述方 法的卡尔曼滤波器。

下面参照图7来描述根据本发明的这种实施例的卡尔曼滤波器900。 图7显示的卡尔曼滤波器900仅仅是一个示例，不应对本发明实施例的 功能和使用范围带来任何限制。

如图7所示，卡尔曼滤波器900以通用计算设备的形式表现。卡尔 曼滤波器900的组件可以包括但不限于：上述至少一个处理单元910、 上述至少一个存储单元920、连接不同系统组件(包括存储单元920和 处理单元910)的总线930、显示单元940。

其中，所述存储单元存储有程序代码，所述程序代码可以被所述处 理单元910执行，使得所述处理单元910执行本说明书上述“示例性方法” 部分中描述的根据本发明各种示例性实施例的步骤。

存储单元920可以包括易失性存储单元形式的可读介质，例如随机 存取存储单元(RAM)921和/或高速缓存存储单元922，还可以进一步 包括只读存储单元(ROM)923。

存储单元920还可以包括具有一组(至少一个)程序模块925的程 序/实用工具924，这样的程序模块925包括但不限于：操作系统、一个 或者多个应用程序、其它程序模块以及程序数据，这些示例中的每一个 或某种组合中可能包括网络环境的实现。

总线930可以为表示几类总线结构中的一种或多种，包括存储单元 总线或者存储单元控制器、外围总线、图形加速端口、处理单元或者使 用多种总线结构中的任意总线结构的局域总线。

卡尔曼滤波器900也可以与一个或多个外部设备1100(例如键盘、 指向设备、蓝牙设备等)通信，还可与一个或者多个使得用户能与该卡 尔曼滤波器900交互的设备通信，和/或与使得该卡尔曼滤波器900能与 一个或多个其它计算设备进行通信的任何设备(例如路由器、调制解调 器等等)通信。这种通信可以通过输入/输出(I/O)接口950进行。并且，卡尔曼滤波器900还可以通过网络适配器960与一个或者多个网络(例 如局域网(LAN)，广域网(WAN)和/或公共网络，例如因特网)通信。 如图所示，网络适配器940通过总线930与卡尔曼滤波器900的其它模 块通信。应当明白，尽管图中未示出，可以结合卡尔曼滤波器900使用 其它硬件和/或软件模块，包括但不限于：微代码、设备驱动器、冗余处 理单元、外部磁盘驱动阵列、RAID系统、磁带驱动器以及数据备份存 储系统等。

通过以上的实施例的描述，本领域的技术人员易于理解，这里描述 的示例实施例可以通过软件实现，也可以通过软件结合必要的硬件的方 式来实现。因此，根据本公开实施例的技术方案可以以软件产品的形式 体现出来，该软件产品可以存储在一个非易失性存储介质(可以是 CD-ROM，U盘，移动硬盘等)中或网络上，包括若干指令以使得一台 计算设备(可以是个人计算机、服务器、终端装置、或者网络设备等) 执行根据本公开实施例的方法。

需要说明的是，尽管在附图中以特定顺序描述了本公开中处理截断 数据的卡尔曼滤波方法的各个步骤，但是，这并非要求或者暗示必须按 照该特定顺序来执行这些步骤，或是必须执行全部所示的步骤才能实现 期望的结果。附加的或备选的，可以省略某些步骤，将多个步骤合并为 一个步骤执行，以及/或者将一个步骤分解为多个步骤执行等。

本领域技术人员在考虑说明书及实践这里公开的发明后，将容易想 到本公开的其它实施方案。本申请旨在涵盖本公开的任何变型、用途或 者适应性变化，这些变型、用途或者适应性变化遵循本公开的一般性原 理并包括本公开未公开的本技术领域中的公知常识或惯用技术手段。说 明书和实施例仅被视为示例性的，本公开的真正范围和精神由所附的权 利要求指出。