CN113204038B - 基于时域与频域的卡尔曼平滑滤波方法及平滑滤波器 - Google Patents

Abstract

本发明涉及信号平滑滤波技术领域，为基于时域与频域的卡尔曼平滑滤波方法及平滑滤波器，包括获取待滤波信号；采用泰勒级数低阶近似模型，根据信号采样频率和模型维数，确定平滑滤波器的状态方程和量测方程；根据卡尔曼平滑滤波算法，确定平滑滤波器的频率响应函数，进而可得到其幅频特性函数；根据所设计平滑滤波器的幅频特性参数，估算系统噪声和量测噪声的统计特性参数；根据卡尔曼滤波模型及其噪声统计特性参数，采用卡尔曼平滑滤波算法，对所述待滤波信号进行平滑滤波处理。由于采用双向卡尔曼滤波，估值精度优于传统的卡尔曼低通滤波器。在频率域，可同传统的FIR低通滤波器一样，使用方便，且对于低信噪比测量信号，可以获得更高的滤波精度。

Description

基于时域与频域的卡尔曼平滑滤波方法及平滑滤波器

技术领域

本发明涉及信号平滑滤波技术领域，具体涉及基于时域与频域的卡尔曼平滑滤波方法及平滑滤波器。

背景技术

卡尔曼滤波技术广泛应用于信号测量、信号传输、智能控制、 GPS动态数据处理和惯性导航等领域。由于外界环境、动态变化等因素的影响，经传感器获得的测量信号、以及经数据链路传输的测量信号通常包含大量噪声。噪声通常以高频为主，需要采用平滑滤波技术处理，消除高频噪声干扰，以提取信号中低频中的有效成分信息。特别是，当测量信号的信噪比较低时，采用平滑滤波技术对信号进行滤波处理，十分必要。

卡尔曼滤波应用需要建立准确的数学模型，且要求噪声的统计特性是已知的。而实际情况并非如此，模型的偏差及噪声的不确定性, 都会对滤波结果和滤波精度产生影响(甚至发散)。

发明内容

本发明提供了基于时域与频域的卡尔曼平滑滤波方法及平滑滤波器，解决了噪声统计特性参数未知的问题，提出了基于频域的噪声统计特性参数估算方法，使得所设计的卡尔曼平滑滤波器，不仅在时域，也可在频域，对测量信号进行滤波处理。

本发明提供了基于时域与频域的卡尔曼平滑滤波方法，包括：

S1，获取待滤波信号；

S2，采用泰勒级数低阶近似模型，根据信号采样频率和模型维数，确定平滑滤波器的状态方程和量测方程；

S3，根据卡尔曼平滑滤波算法，确定平滑滤波器的频率响应函数，进而可得到其幅频特性函数；根据所设计平滑滤波器的幅频特性参数，估算系统噪声和量测噪声的统计特性参数；

S4，根据卡尔曼滤波模型及其噪声特性参数，采用卡尔曼平滑滤波算法，对所述待滤波信号进行平滑滤波处理。

可选的，采用泰勒级数低阶近似模型，滤波器的状态方程为：

X_k＝ΦX_k-1+ΓW_k-1 (1)

式中，

W_k＝[W_k]

X_k为t_k时刻的状态向量,Φ为t_k-1时刻至t_k时刻的一步转移矩阵,Γ为系统噪声耦合矩阵,W_k为零均值白噪声或高斯白噪声，其协方差矩阵为Q_k；

其中，X_k中包含x_k和x_k的m阶导数(m＝1,2,…,n-1)的状态信息序列，例：

表示x_k的一阶导数，t_s为采样间隔，n为模型维数。

可选的，采用泰勒级数低阶近似模型，滤波器的量测方程为

Z_k＝HX_k+V_k (2)

式中，Z_k＝[x′_k]，H＝[1 0 0 ... 0]，V_k＝[V_k]，Z_k为t_k时刻的量测向量，x′_k为待滤波信号，H为量测矩阵，V_k为零均值白噪声或高斯白噪声，其协方差矩阵为R_k。

可选的，所述卡尔曼平滑滤波算法包括正向滤波算法，正向滤波算法如下：

状态一步预测

状态估计

滤波增益K_k＝P_k/k-1H^T(HP_k/k-1H^T+R_k)^-1 (5a)

或

一步预测均方误差P_k/k-1＝ΦP_k-1Φ^T+ΓQ_k-1Γ^T (6)

估计均方误差

或P_k＝(I-K_kH)P_k/k-1 (7b)

或

式中,I为单位矩阵。

给定初始状态向量

和初始估计均方误差矩阵P₀，按(3)～(7)式， (7)式包括(7a)、(7b)及(7c)，其他的标号也一样，由量测向量{Z_k}(k＝1,2,…,M)，递推得到状态向量估计

表示滤波器X_k的先验估计，

表示滤波器X_k-1的后验估计，Z_k为待滤波信号，K_k表示卡尔曼滤波增益，Φ为t_k-1时刻至t_k时刻的一步转移矩阵。

可选的，所述卡尔曼平滑滤波算法还包括反向滤波算法，反向滤波算法如下：

平滑方程:

平滑均方误差:

令

在正向滤波基础上，按(8)式和(9)式，由状态向量估计

递推得到平滑后的状态向量估计

可选的，所述S3具体包括：输入信号为Z_k(x′_k)，输出信号为

即

根据卡尔曼平滑滤波器结构和平滑滤波算法，导出的平滑滤波器频率响应函数为：

其中，I为单位矩阵，j为虚数单位，ω为圆频率，Φ为t_k-1时刻至t_k时刻的一步转移矩阵。

可选的，在稳态时，根据滤波算法导出 P＝(I-PH^TR^-1H)(ΦPΦ^T+ΓQΓ^T)；其中，P表示滤波状态估计误差的协方差矩阵。

可选的，按照平滑滤波算法，在稳态条件下，频率响应函数表示为：

将包含P矩阵代数方程的解代入频率响应函数H(e^jω)，并以此为基础，导出滤波器的幅频特性函数|H(f)|(ω＝2πf),在噪声参数确定的条件下，可求出滤波器对应的截止频率f_c。

可选的，在量测噪声方差R估算基础上，根据滤波模型、滤波器的频率响应函数H(e^jω)和滤波器截止频率f_c，估算系统噪声方差Q。

本发明还提供了用于基于时域与频域的卡尔曼平滑滤波方法的平滑滤波器，包括：

待滤波数据获取模块，用于获取待滤波信号及数据采样间隔t_s参数；

卡尔曼滤波模型和滤波参数确定模块，用于确定系统状态方程、量测方程和滤波器的截止频率；

预测方程和更新方程确定模块，用于根据卡尔曼平滑滤波算法, 确定出平滑滤波器的预测方程、滤波更新方程和平滑更新方程；

噪声统计特性参数确定模块，用于根据卡尔曼平滑滤波算法，确定平滑滤波器的频率响应函数，进而得到其幅频特性函数，并以此为基础，由滤波器截止频率，确定量测噪声序列方差R和系统噪声序列方差Q。

有益效果：本发明提供了基于时域与频域的卡尔曼平滑滤波方法及平滑滤波器，包括：获取待滤波信号；采用泰勒级数低阶近似模型，根据信号采样频率和模型维数，确定平滑滤波器的状态方程和量测方程；根据卡尔曼平滑滤波算法，确定平滑滤波器的频率响应函数，进而可得到其幅频特性函数；根据所设计平滑滤波器的幅频特性参数，估算系统噪声和量测噪声的统计特性参数；根据滤波模型及其噪声特性参数，采用卡尔曼平滑滤波算法，对所述待滤波信号进行平滑滤波处理。通过采用固定区间平滑滤波算法，利用在整个区间内采样得到的所有测量值，得到每次采样时的最优估计。由于采用双向卡尔曼滤波，估值精度优于传统的卡尔曼低通滤波器。且在频率域，同传统的 FIR低通滤波器一样，使用方便，且对于低信噪比测量信号，可以获得更高的滤波精度。

上述说明仅是本发明技术方案的概述，为了能够更清楚了解本发明的技术手段，并可依照说明书的内容予以实施，以下以本发明的较佳实施例并配合附图详细说明如后。本发明的具体实施方式由以下实施例及其附图详细给出。

附图说明

此处所说明的附图用来提供对本发明的进一步理解，构成本申请的一部分，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1为本发明基于时域与频域的卡尔曼平滑滤波方法及滤波器的信号处理流程图；

图2为本发明基于时域与频域的卡尔曼平滑滤波方法及滤波器的卡尔曼平滑滤波器结构示意图；

图3为本发明基于时域与频域的卡尔曼平滑滤波方法及滤波器的卡尔曼平滑滤波算法(循环计算)示意图；

图4为本发明基于时域与频域的卡尔曼平滑滤波方法及滤波器的噪声统计特性参数估算框图示意图；

图5为本发明基于时域与频域的卡尔曼平滑滤波方法及滤波器的卡尔曼平滑滤波器模块结构示意图。

具体实施方式

以下结合附图对本发明的原理和特征进行描述，所举实例只用于解释本发明，并非用于限定本发明的范围。在下列段落中参照附图以举例方式更具体地描述本发明。根据下面说明和权利要求书，本发明的优点和特征将更清楚。需说明的是，附图均采用非常简化的形式且均使用非精准的比例，仅用以方便、明晰地辅助说明本发明实施例的目的。

需要说明的是，当组件被称为“固定于”另一个组件，它可以直接在另一个组件上或者也可以存在居中的组件。当一个组件被认为是“连接”另一个组件，它可以是直接连接到另一个组件或者可能同时存在居中组件。当一个组件被认为是“设置于”另一个组件，它可以是直接设置在另一个组件上或者可能同时存在居中组件。本文所使用的术语“垂直的”、“水平的”、“左”、“右”以及类似的表述只是为了说明的目的。

除非另有定义，本文所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本文中在本发明的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本发明。本文所使用的术语“及/或”包括一个或多个相关的所列项目的任意的和所有的组合。

如图1所述，本发明提供了基于时域与频域的卡尔曼平滑滤波方法及平滑滤波器，包括：S1，获取待滤波信号；

S2，采用泰勒级数低阶近似模型，根据信号采样频率和模型维数，确定平滑滤波器的状态方程和量测方程；

S3，根据卡尔曼平滑滤波算法，确定平滑滤波器的频率响应函数，进而可得到其幅频特性函数；根据所述平滑滤波器的幅频特性参数，估算系统噪声和量测噪声的统计特性参数；

S4，根据所述卡尔曼滤波模型及其噪声特性参数，采用卡尔曼平滑滤波算法，对所述待滤波信号进行平滑滤波处理。通过采用固定区间平滑滤波算法，利用在整个区间内采样得到的所有测量值，得到每次采样时的最优估计。由于采用平滑双向卡尔曼滤波，估值精度优于传统的卡尔曼低通滤波器。该卡尔曼平滑滤波器不仅可在时域，也可在频域对测量信号进行平滑滤波处理。且在频率域，同传统的FIR低通滤波器一样，使用方便，且对于低信噪比测量信号，可以获得更高的滤波精度。

该方法用于对信号进行平滑滤波。滤波方法首先确定信号的采样间隔和平滑滤波器的维数，并给出系统的状态方程和更新方程；再根据平滑滤波器的截止频率和频率响应函数，确定系统噪声和量测噪声的统计特性参数；最后，按卡尔曼平滑滤波算法对待滤波信号进行滤波。其中，本文所采用的滤波器即为卡尔曼平滑滤波器。具体地，该滤波方法工作原理如下：

S1，获取待滤波信号。

S2，采用泰勒级数低阶近似模型，根据信号采样频率和模型维数，确定滤波器的状态方程和量测方程。

S3，根据卡尔曼平滑滤波算法，确定平滑滤波器的频率响应函数，进而得到其幅频特性函数。以此为基础，由平滑低通滤波器截止频率，确定量测噪声序列方差R和系统噪声序列方差Q。

S4，根据卡尔曼滤波模型及其噪声特性参数，采用卡尔曼平滑滤波算法，对所述待滤波信号进行平滑滤波处理。

本发明采用双向卡尔曼滤波即卡尔曼平滑滤波，滤波精度优于传统的卡尔曼滤波；在量测噪声方差R估算基础上(FIR滤波结果作为近似标准)，根据滤波模型、平滑滤波器的频率响应函数H(e^jω)和平滑滤波器截止频率f_c,估算系统噪声方差Q；平滑滤波器不仅在时域，也可在频率域对信号进行滤波处理。特别是，同传统的FIR平滑滤波器一样，在频率域，可根据低通平滑滤波器的截止频率f_c，对待滤波信号进行滤波处理，且滤波精度高，使用方便。

该方案采用的泰勒级数低阶近似模型的卡尔曼平滑滤波方法具体包括以下几个步骤：

步骤1：获取待滤波信号。

步骤2:由信号采样间隔t_s，根据选定模型维数n，确定系统状态方程和量测方程。

步骤3:根据卡尔曼平滑滤波算法，确定出平滑滤波器的预测方程、滤波更新方程和平滑更新方程。

步骤4：根据卡尔曼平滑滤波算法，确定平滑滤波器的频率响应函数，进而得到其幅频特性函数。以此为基础，由滤波器截止频率，确定量测噪声序列方差R和系统噪声序列方差Q。

步骤5：根据上述确定的滤波模型及确定的噪声特性参数，采用卡尔曼平滑滤波算法，对所述待滤波信号进行平滑滤波处理。

下面针对以上步骤进行细化解释说明：

步骤1具体包括：获取待滤波数据获取模块，并确定数据采样间隔t_s等参数；

步骤2具体包括:

根据待滤波信号确定采样间隔t_s，选取模型维数n，确定系统状态方程和量测方程。(采用泰勒级数低阶近似模型)

滤波器的状态方程为：X_k＝ΦX_k-1+ΓW_k-1 (1)

式中，

其中，Z_k为待滤波信号，K_k表示卡尔曼滤波增益。滤波器的量测方程为Z_k＝HX_k+V_k(2)

式中，Z_k＝[x′_k]，H＝[1 0 0 ... 0]，V_k＝[V_k]。Z_k为t_k时刻的量测向量(x′_k待滤波信号)，H为量测矩阵，V_k为零均值白噪声或高斯白噪声，其协方差矩阵为R_k。

步骤3具体包括：根据卡尔曼平滑滤波算法，确定出平滑滤波器的预测方程、滤波更新方程和平滑更新方程。

卡尔曼平滑滤波采用双向卡尔曼滤波，包括正向滤波(常规卡尔曼滤波)和反向滤波(平滑滤波)。

正向滤波(常规卡尔曼滤波)算法的更新方程为：

状态一步预测

状态估计

滤波增益

或

一步预测均方误差P_k/k-1＝ΦP_k-1Φ^T+ΓQ_k-1Γ^T (6)

估计均方误差

或P_k＝(I-K_kH)P_k/k-1 (7b)

或

式中,I为单位矩阵。

给定初始状态向量

和初始估计均方误差矩阵P₀,按(3)～(7)式,由量测向量{Z_k}(k＝1,2,…,M),递推得到状态向量估计

反向滤波(平滑滤波)算法：

平滑方程:

式中

平滑均方误差:

令

在正向滤波基础上,按(8)式和(9)式,由状态向量估计

递推得到平滑后的状态向量估计

对于测量信号滤波处理来讲，输入信号为Z_k(x′_k)，输出信号为

即

步骤4具体包括根据卡尔曼平滑滤波算法，确定平滑滤波器的频率响应函数，得到其幅频特性函数。以此为基础，由滤波器截止频率，确定噪声统计特性参数，其包括量测噪声序列方差R和系统噪声序列方差Q。

根据卡尔曼平滑低通平滑滤波器结构图(图2)，可导出的平滑滤波器频率响应函数为：

其中，I为单位矩阵，j为虚数单位，ω为圆频率。

如图4所示，在稳态时，根据滤波算法可导出 P＝(I-PH^TR^-1H)(ΦPΦ^T+ΓQΓ^T)；其中，P表示滤波状态估计误差的协方差矩阵。

按照平滑滤波算法，在稳态条件下，频率响应函数亦可表示为：

将包含P矩阵代数方程的解代入频率响应函数H(e^jω)，并以此为基础，导出平滑滤波器的幅频特性函数|H(f)|(ω＝2πf),可求出平滑滤波器对应的截止频率f_c。

与上述过程相反，在量测噪声方差R估算基础上(FIR滤波结果作为近似标准),根据滤波模型、平滑滤波器的频率响应函数H(e^jω)和平滑滤波器截止频率f_c,估算系统噪声方差Q。

步骤5具体包括：根据滤波模型及确定的噪声特性参数，采用卡尔曼平滑滤波算法，对所述待滤波信号进行平滑滤波处理。

为实现上述目的，本发明还提供了平滑滤波器，本实施例的滤波器为卡尔曼平滑滤波器，这里只针对卡尔曼平滑滤波器进行举例说明，其他类型的平滑滤波器不做赘述。图5为卡尔曼平滑滤波器模块结构示意图，具体包括：

待滤波数据获取模块，用于获取待滤波信号及参数(采样间隔t_s)。

滤波模型及滤波参数确定模块，用于确定系统状态方程、量测方程和滤波器的截止频率；

预测方程和校正方程确定模块，用于确定卡尔曼平滑滤波的预测方程和更新(校正)方程，其过程如图3所示。

平滑滤波器噪声参数确定模块，用于确定系统噪声和量测噪声统计特性参数，其过程如图4所示。

平滑滤波模块，用于根据滤波模型、滤波参数、噪声参数、所述预测方程和所述校正方程，采用卡尔曼平滑滤波算法，对待滤波信号进行平滑滤波处理，其过程如图5所示。

本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

最后应当说明的是：以上实施例仅用以说明本发明的技术方案而非对其限制，尽管参照上述实施例对本发明进行了详细的说明，所属领域的普通技术人员应当理解：依然可以对本发明的具体实施方式进行修改或者等同替换，而未脱离本发明精神和范围的任何修改或者等同替换，其均应涵盖在本发明的权利要求保护范围之内。

Claims (6)

1.基于时域与频域的卡尔曼平滑滤波方法，其特征在于，包括：

S1，获取待滤波信号；

S2，采用泰勒级数低阶近似模型，根据信号采样频率和模型维数，确定平滑滤波器的状态方程和量测方程；采用泰勒级数低阶近似模型，滤波器的量测方程为Z_k＝HX_k+V_k (2)

式中，Z_k＝[x′_k]，H＝[1 0 0 ... 0]，V_k＝[V_k]，Z_k为t_k时刻的量测向量，x′_k为待滤波信号，H为量测矩阵，V_k为零均值白噪声或高斯白噪声，其协方差矩阵为R_k；

S3，根据卡尔曼平滑滤波算法，确定平滑滤波器的频率响应函数，进而可得到其幅频特性函数；根据所设计平滑滤波器的幅频特性参数，估算系统噪声和量测噪声的统计特性参数；所述卡尔曼平滑滤波算法包括正向滤波算法，正向滤波算法如下：

状态一步预测

状态估计

滤波增益K_k＝P_k/k-1H^T(HP_k/k-1H^T+R_k)^-1 (5a)

或

一步预测均方误差P_k/k-1＝ΦP_k-1Φ^T+ΓQ_k-1Γ^T (6)

估计均方误差

或P_k＝(I-K_kH)P_k/k-1 (7b)

或

式中,I为单位矩阵，给定初始状态向量

和初始估计均方误差矩阵P₀，按(3)～(7)式，(7)式包括(7a)、(7b)及(7c)，其他的标号也一样，由量测向量{Z_k}(k＝1,2,…,M)，递推得到状态向量估计

表示滤波器X_k的先验估计，

表示滤波器X_k-1的后验估计，Z_k为待滤波信号，K_k表示卡尔曼滤波增益，Φ为t_k-1时刻至t_k时刻的一步转移矩阵；

所述卡尔曼平滑滤波算法还包括反向滤波算法，反向滤波算法如下：

平滑方程:

式中

平滑均方误差:

令

在正向滤波基础上，按(8)式和(9)式，由状态向量估计

递推得到平滑后的状态向量估计

所述S3具体包括：输入信号为Z_k(x′_k)，输出信号为

即

根据卡尔曼平滑滤波器结构和平滑滤波算法，导出的平滑滤波器频率响应函数为：

其中，I为单位矩阵，j为虚数单位，ω为圆频率，Φ为t_k-1时刻至t_k时刻的一步转移矩阵

S4，根据卡尔曼滤波模型及其噪声统计特性参数，采用卡尔曼平滑滤波算法，对所述待滤波信号进行平滑滤波处理。

2.根据权利要求1所述的基于时域与频域的卡尔曼平滑滤波方法，其特征在于，采用泰勒级数低阶近似模型，滤波器的状态方程为：

X_k＝ΦX_k-1+ΓW_k-1 (1)

式中，

W_k＝[W_k]

X_k为t_k时刻的状态向量,Φ为t_k-1时刻至t_k时刻的一步转移矩阵,Γ为系统噪声耦合矩阵,W_k为零均值白噪声或高斯白噪声，其协方差矩阵为Q_k；

其中，X_k中包含x_k和x_k的m阶导数(m＝1,2,…,n-1)的状态信息序列，

表示x_k的一阶导数，t_s为采样间隔，n为模型维数。

3.根据权利要求1所述的基于时域与频域的卡尔曼平滑滤波方法，其特征在于，在稳态时，根据滤波算法导出P＝(I-PH^TR^-1H)(ΦPΦ^T+ΓQΓ^T)；其中，P表示滤波状态估计误差的协方差矩阵，Q表示估算系统噪声方差。

4.根据权利要求3所述的基于时域与频域的卡尔曼平滑滤波方法，其特征在于，按照平滑滤波算法，在稳态条件下，频率响应函数表示为：

将包含P矩阵代数方程的解代入频率响应函数H(e^jω)，并以此为基础，导出滤波器的幅频特性函数|H(f)|(ω＝2πf),在噪声参数确定的条件下，可求出滤波器对应的截止频率f_c。

5.根据权利要求4所述的基于时域与频域的卡尔曼平滑滤波方法，其特征在于，在量测噪声方差R估算基础上，根据滤波模型、滤波器的频率响应函数H(e^jω)和滤波器截止频率f_c，估算系统噪声方差Q。

6.用于如权利要求1至5任一项所述的基于时域与频域的卡尔曼平滑滤波方法的平滑滤波器，其特征在于，包括：

待滤波数据获取模块，用于获取待滤波信号及数据采样间隔t_s参数；

卡尔曼滤波模型和滤波参数确定模块，用于确定系统状态方程、量测方程和滤波器的截止频率；

预测方程和更新方程确定模块，用于根据卡尔曼平滑滤波算法,确定出平滑滤波器的预测方程、滤波更新方程和平滑更新方程；

噪声统计特性参数确定模块，用于根据卡尔曼平滑滤波算法，确定平滑滤波器的频率响应函数，进而得到其幅频特性函数，并以此为基础，由滤波器截止频率，确定量测噪声序列方差R和系统噪声序列方差Q。

Images