CN112946641B - 一种基于卡尔曼滤波新息与残差相关的数据滤波方法 - Google Patents

Abstract

本发明是一种基于卡尔曼滤波新息与残差相关的数据滤波方法，属于计算机数据信号处理领域。本发明解决标准卡尔曼滤波对线性系统滤波估计时量测噪声方差阵未知，出现信号滤波效果不好，甚至出现滤波发散的问题。本发明针对离散线性系统模型，采集信号处理系统的量测序列构建滤波新息序列和残差序列；挖掘量测噪声方差阵与新息序列和残差序列的协方差矩阵的关系，通过计算新息序列和残差序列的协方差矩阵实时估计量测噪声方差阵，再带入标准卡尔曼滤波器进行实时滤波。本发明方法具有对实际噪声统计特性发生变化的跟踪能力，不易受外界量测扰动的影响，保证了量测噪声方差阵估计时的非负定性，消除了因噪声方差阵估计负定所引起的滤波发散现象。

Description

一种基于卡尔曼滤波新息与残差相关的数据滤波方法

技术领域

本发明属于计算机数据信号处理领域，涉及自适应卡尔曼滤波技术领域，具体涉及一种对计算机信号数据基于滤波新息与残差相关的量测噪声方差阵估计的改进卡尔曼滤波进行处理的方法。

背景技术

数据滤波是去除噪声还原真实数据的一种数据处理技术。卡尔曼滤波作为一种线性递推最小方差估计具有易于计算机在线计算、存储量小等特点，因此自1960年提出之初便受到工程界的广泛关注与应用。由于它便于计算机编程实现，并能够对现场采集的数据进行实时的更新和处理，Kalman滤波是目前应用最为广泛的滤波方法，在通信，导航，制导与控制等多领域得到了较好的应用。在数据通信领域，卡尔曼滤波被应用到对信道的实时跟踪上；在导航领域，卡尔曼滤波技术被应用到雷达系统中的目标跟踪、惯性与卫星组合导航以及多传感器的数据融合中；在制导与控制领域，卡尔曼滤波技术被应用到对弹道轨迹的实时估计中。

卡尔曼滤波通过利用状态空间模型的方法描述系统，其中描述状态量自身随时间变化的模型称之为系统模型，描述传感器量测信息与系统状态关系的模型称之为量测模型。在线性系统滤波中，卡尔曼滤波理论在完全已知系统的状态空间模型及其统计特性的前提下，可以获得线性最小方差估计准则下的最优解。

在标准卡尔曼滤波方法中，量测噪声方差阵是滤波获得最优解不可或缺的重要参数之一。量测噪声方差阵表征了量测模型中量测量不确定性动态误差的统计特性。量测噪声方差阵越大表明由传感器所获知的量测量精确度越低，在卡尔曼滤波中所给予量测量的权重也就越低；量测噪声方差阵越小表明由传感器所获知的量测量精确度越高，在卡尔曼滤波中所给予量测量的权重也就越高。然而，在工程实践中量测噪声方差阵的大小通常会因外界环境的影响而变化。此时，先验的量测噪声方差阵将会因为与实际噪声的方差阵不匹配而破坏标准卡尔曼滤波的最优性，甚至引起滤波的发散致使标准卡尔曼滤波无法正常工作。

标准卡尔曼滤波无法在线辨识量测噪声方差阵，因此在对采集的信号数据进行滤波时，将给予错误的权重，使得信号滤波效果不好，甚至出现滤波发散的现象致使无法得到合理的滤波解，因此设计具有自适应能力的卡尔曼滤波方法是工程实践中亟需解决的问题。

发明内容

为了解决标准卡尔曼滤波对线性系统滤波估计时量测噪声方差阵未知，出现信号滤波效果不好，甚至出现滤波发散的问题，本发明提供了一种基于卡尔曼滤波新息与残差相关的数据滤波方法，有效估计量测噪声方差阵，以提高信号滤波效果。

本发明的一种基于卡尔曼滤波新息与残差相关的数据滤波方法，将信号处理系统通过卡尔曼滤波器描述，建立离散线性系统模型，如下：

其中，X(k)为信号处理系统的输入状态向量，Z(k)为传感器的量测状态向量；Φ(k,k-1)与H(k)分别为状态转移矩阵与量测矩阵；Γ(k,k-1)为系统噪声驱动阵；W(k-1)与V(k)为相互独立的零均值白噪声序列，分别被称为系统噪声与量测噪声，W(k-1)与V(k)的噪声方差阵分别用Q(k-1)与R(k)表示。

针对上述离散线性系统模型，本发明的数据滤波方法包括以下步骤：

步骤一：利用量测序列{Z(k)}构建滤波新息序列{ε(k)}，如下：

其中，

为k-1时刻卡尔曼滤波值；中间量

步骤二：利用量测序列{Z(k)}构建滤波残差序列{η(k)}，如下：

其中，

为k时刻卡尔曼滤波值；中间量

步骤三：检测滤波新息序列{ε(k)}与残差序列{η(k)}的平稳性，若都满足平稳性条件，继续下一步骤，否则，获得下一时刻的量测序列，转步骤一执行；

当满足下面三个不等式条件时，认为滤波新息序列{ε(k)}或残差序列{η(k)}具备平稳性；

其中，标记ξ(k)为滤波新息ε(k)或残差η(k)，E()为随机变量均值运算符，Var()为随机变量方差运算符，Cov[]为随机变量协方差运算符；l为计算协方差时的固定步长，取值范围为10～25；ξ_threshold、ξ_{var_threshold}与ξ_{cov_threshold}为检测阈值，取值范围分别为10～25，3～4.5与3～5。

步骤四：利用开窗法计算序列{ε(k)}与{η(k)}的协方差矩阵Cov[ε(k),η(k)]，如下：

其中，M为开窗估计的窗口长度，上角标T表示转置。

步骤五：利用量测噪声方差阵R(k)与序列{ε(k)}、{η(k)}的协方差矩阵Cov[ε(k),η(k)]间的代数关系，计算量测噪声方差阵估计序列

由于所述的R(k)与协方差矩阵Cov[ε(k),η(k)]存在如下物理代数关系：

其中，P(k)为状态估计协方差矩阵，K(k)为卡尔曼滤波增益矩阵；

存在关系式K(k)＝P(k)H^T(k)R^-1(k)；

因此，实时估计k时刻的量测噪声方差阵

步骤六：将量测噪声方差阵的估计值

带入到标准卡尔曼滤波器中，实时计算状态的滤波估计值及状态估计协方差矩阵，实时进行卡尔曼滤波。

与现有技术相比，本发明的优点和积极效果在于：

(1)本发明解决了现有标准卡尔曼滤波所存在的问题，即由于标准卡尔曼滤波在滤波过程中采用的是先验的量测噪声协方差阵，因此当实际噪声统计特性发生变化时滤波结果会因失配的噪声统计参数而变得不稳定。本发明滤波方法具有对实际噪声统计特性发生变化的跟踪能力，不易受外界量测扰动的影响，滤波结果具有更强的鲁棒性。

(2)本发明打破目前对量测噪声方差阵R(k)采用先验经验获取的思路，而是基于卡尔曼滤波新息与残差相关估计获得。量测噪声方差阵的估计形式简洁，即为滤波新息与残差间的协方差矩阵，保证了量测噪声方差阵估计时的非负定性，消除了因噪声方差阵估计负定所引起的滤波发散现象。

(3)与标准卡尔曼滤波算法相比，本发明降低了对量测噪声统计特性参数的要求，能够有效地解决量测噪声方差阵时变时的滤波估计问题，进一步扩大了卡尔曼滤波的应用范围。同时，本发明方法获取量测噪声方差阵的计算量适中，具有良好的实时性，利于工程实践中实际系统的数据滤波应用。

附图说明

图1是本发明基于卡尔曼滤波新息与残差相关的数据滤波方法的流程图；

图2是采用本发明方法估计的量测噪声方差与量测噪声方差真值的对比图。

具体实施方式

下面将结合实施例对本发明作进一步的详细说明。

本发明实施例中，以对目标跟踪系统采集的信号数据进行处理来说明，采集到的位置量测信号时间序列输入至标准卡尔曼滤波器中。为了方便描述本发明基于卡尔曼滤波新息及残差相关的数据滤波方法，首先给出了本发明所针对的空间模型及其前提条件。

本发明方法的卡尔曼滤波器建立的离散线性系统模型为：

式中，X(k)为系统状态向量，在本实施例的目标跟踪系统中系统状态包括目标物体的位置、速度以及加速度；Z(k)为量测状态向量，在本实施例中量测状态指对目标物体的位置量测值；Φ(k,k-1)与H(k)分别为状态转移矩阵与量测矩阵；Γ(k,k-1)为系统噪声驱动矩阵；W(k-1)与V(k)为相互独立的零均值白噪声序列，在卡尔曼滤波中分别被称为系统噪声与量测噪声，其噪声方差阵分别用Q(k-1)与R(k)表示。k表示k时刻。

量测噪声方差阵作为表征量测模型中量测量不确定性动态误差的重要统计量，是滤波获得最优解不可或缺的重要参数之一。先验的量测噪声方差阵将会因为与实际量测噪声方差阵不匹配而破坏标准卡尔曼滤波的最优性，甚至引起滤波的发散致使标准卡尔曼滤波无法正常工作。本发明对标准卡尔曼滤波器改进，所解决的是标准卡尔曼滤波在针对线性系统滤波估计时量测噪声方差阵未知的问题，针对量测噪声方差阵R(k)在卡尔曼滤波过程中为时变的情形，利用新息与残差相关信息来估计量测噪声方差阵，再进而实现数据滤波，如图1所示，本发明包括如下六个步骤。

步骤一：利用量测序列{Z(k)}构建滤波新息序列{ε(k)}，k时刻的滤波新息ε(k)如下：

其中，

为k-1时刻卡尔曼滤波值；

为卡尔曼滤波状态一步预测值，即

中间量

步骤二：利用量测序列{Z(k)}构建滤波残差序列{η(k)}，k时刻的滤波残差η(k)如下：

其中，

为卡尔曼滤波状态估计值，中间量

步骤三：检测滤波新息序列{ε(k)}与残差序列{η(k)}的平稳性。

滤波新息序列与残差序列的平稳性可以通过下面三个条件进行判别：

(1)滤波新息序列或残差序列与上一时刻序列期望之差与期望之比的绝对值小于等于变化率阈值，即

其中，{ξ(k)}为k时刻的新息序列或残差序列，E(·)为随机变量均值运算符，ξ_threshold为检测阈值，取值范围为10～25。在检测过程中，针对两种不同序列，检测阈值ξ_threshold设置一致。

(2)滤波新息序列或残差序列与其期望之差的平方再与上一时刻序列方差作差与上一时刻序列方差之比的绝对值小于等于变化率阈值，即

其中，Var()为随机变量方差运算符，ξ_{var_threshold}为检测阈值，取值范围为3～4.5。在检测过程中，针对两种不同序列，检测阈值ξ_{var_threshold}设置一致。

(3)滤波新息序列或残差序列协方差的变化率小于等于协方差变化率阈值，即

其中，Cov[]为随机变量协方差运算符；l为计算协方差时的固定步长，取值范围为10～25；ξ_{cov_threshold}为检测阈值，取值范围为3～5。在检测过程中，针对两种不同序列，检测阈值ξ_{cov_threshold}设置一致。

当上述三个条件同时满足时即认为滤波新息序列或残差序列满足平稳性条件，当滤波新息序列和残差序列都满足平稳性条件后，进行步骤四；否则，返回步骤一，继续获得下一时刻的量测状态，计算新息序列和残差序列并进行平稳性判别。

步骤四：计算序列{ε(k)}与{η(k)}的协方差矩阵Cov[ε(k),η(k)]，如下：

式(6)中利用到卡尔曼滤波中新息与残差为零均值的特性，即E(ε(k))＝0与E(η(k))＝0。上角标T表示转置。

将式(1)与式(2)代入到式(6)中计算可得：

式中，

E[V(k)V^T(k)]为定义的量测噪声方差阵，有E[V(k)V^T(k)]＝R(k)。

与

可由下式计算：

式中，I为相应维数的单位矩阵，K(k)为卡尔曼滤波增益矩阵，P(k/k-1)为状态一步预测协方差矩阵，P(k)为状态估计协方差矩阵。

将式(8)与式(9)代入式(7)整理可得

其中，上角标“-1”表示求逆矩阵。

由数理统计知识可知，序列{ε(k)}与{η(k)}的协方差矩阵Cov[ε(k),η(k)]的开窗估计公式为：

式中，M为开窗估计时的窗口长度，取值为正整数。

步骤五：通过量测噪声方差阵R(k)与序列{ε(k)}、{η(k)}的协方差矩阵Cov[ε(k),η(k)]间的代数关系，计算量测噪声方差阵估计序列

从式(10)与式(11)间的代数关系

从而可以获取量测噪声方差阵实时估计序列

计算得到k时刻的量测噪声方差阵估计值

如下：

在估计之初，滤波新息或残差序列长度N会存在小于窗口长度M的情况，如图1所示，利用滤波新息或残差序列的所有数据估计量测噪声方差阵，如下：

式中，N<M，N为当前新息或残差序列长度最大值。

步骤六：将量测噪声方差阵的估计值

带入到标准卡尔曼滤波器中，实时计算状态的滤波估计值及其协方差矩阵。

由上述步骤可以获取量测噪声方差阵实时估计序列

然后将量测噪声方差阵的估计值

带入到标准卡尔曼滤波中，实时计算状态的滤波估计值及状态估计协方差矩阵。具体处理过程类似标准卡尔曼滤波过程，如下所示：

通过上述步骤，便可实现对量测噪声方差阵的在线估计，并利用所获得的

实时进行卡尔曼滤波。

为了能够验证本发明所述量测噪声方差阵估计方法的有效性，本发明通过MATLAB对本发明方法进行仿真实验，仿真结果表明了本发明方法对量测噪声方差阵估计的准确性，从而实现对状态的实时滤波。

实验中，某一维目标跟踪系统的离散线性系统状态方程如下：

式中，

为系统状态量，x_1,k,x_2,k,x_3,k分别表示目标物体的位置、速度以及加速度；T为采样时间，取值为T＝0.1s；

为零均值白噪声，其方差阵为

采集位置信号的时间序列输入卡尔曼滤波器中，量测方程为

式中，z_k为位置量测量；v_k为零均值量测白噪声。

在仿真过程中，量测噪声v_k的方差阵是有变化的，即有在仿真前5000步量测噪声方差阵R＝0.04，后4000步量测噪声方差阵变为R＝0.4。在卡尔曼滤波过程中，除量测噪声方差阵是未知需要待估计的外，其他模型参数均是已知的。

将利用本发明方法对量测噪声方差阵估计值与真值的对比如图2所示。由图2可知，本发明方法能够有效跟踪量测噪声方差阵的变化。在仿真前5000步，本发明的量测噪声方差阵估计值在真值0.04附近波动；随后随着量测噪声方差阵真值的变化，本发明所估计的量测噪声方差值在真值0.4附近波动。

由上述仿真及仿真结果表明了本发明方法实时对量测噪声方差阵估计的有效性，解决了现有标准卡尔曼滤波采用先验的量测噪声方差阵，当实际噪声统计特性发生变化时滤波结果会因失配的噪声统计参数而变得不稳定的问题，具有一定的工程实践价值。

Claims (3)

1.一种基于卡尔曼滤波新息与残差相关的数据滤波方法，利用卡尔曼滤波器对信号处理系统建立离散线性系统模型，设信号处理系统在k时刻的输入状态向量为X(k)，传感器量测状态向量为Z(k)；其特征在于，所述方法包括如下步骤：

步骤1：获取量测序列{Z(k)}，构建滤波新息序列{ε(k)}和滤波残差序列{η(k)}；

其中，H(k)为量测矩阵，Φ(k,k-1)为状态转移矩阵，

分别为k-1时刻、k时刻的卡尔曼滤波值；V(k)为量测噪声，为零均值白噪声序列，量测噪声方差阵为R(k)；中间量

中间量

步骤2：检测滤波新息序列{ε(k)}与残差序列{η(k)}的平稳性，若都满足平稳性条件，继续下一步骤，否则，获得下一时刻的量测序列，转步骤1执行；

进行平稳性判断包括：标记k时刻的滤波新息ε(k)或残差η(k)为ξ(k)，当同时满足下面三个不等式条件时，则序列满足平稳性条件；

条件1：计算上一时刻ξ(k-1)的期望E(ξ(k-1))，设置检测阈值ξ_threshold为10～25；

条件2：计算上一时刻ξ(k-1)的方差Var(ξ(k-1))，计算当前k时刻ξ(k)的期望E(ξ(k))，设置检测阈值ξ_{var_threshold}为3～4.5；

条件3：设置固定步长l为10～25；

计算k时刻ξ(k)与k-l时刻的ξ(k-l)的协方差Cov[ξ(k),ξ(k-l)]；

计算k-1时刻ξ(k-1)与k-1-l时刻ξ(k-1-l)的协方差Cov[ξ(k-1),ξ(k-1-l)]；

设置检测阈值ξ_{cov_threshold}为3～5；

步骤3：利用开窗法计算序列{ε(k)}与{η(k)}的协方差矩阵Cov[ε(k),η(k)]；

步骤4：根据量测噪声方差阵R(k)与协方差矩阵Cov[ε(k),η(k)]的代数关系，实时估计量测噪声方差阵

所述的R(k)与协方差矩阵Cov[ε(k),η(k)]存在如下代数关系：

其中，E(·)为求取均值，上角标T表示转置，P(k)为状态估计协方差矩阵，K(k)为卡尔曼滤波增益矩阵，存在关系式K(k)＝P(k)H^T(k)R^-1(k)；

步骤5：将量测噪声方差阵的估计值

带入到卡尔曼滤波器中，实时计算状态的滤波估计值及状态协方差矩阵，实时进行卡尔曼滤波。

2.根据权利要求1所述的方法，其特征在于，所述的步骤3中，利用开窗法估计协方差矩阵Cov[ε(k),η(k)]，如下：

其中，M为开窗法估计时的窗口长度。

3.根据权利要求1所述的方法，其特征在于，所述的步骤3中，设k时刻获取的滤波新息序列{ε(k)}与残差序列{η(k)}的长度最大值为N，当N<M时，M为开窗法估计时的窗口长度，则根据按照窗口长度N计算协方差矩阵Cov[ε(k),η(k)]。

Images