CN112632454A - 一种基于自适应卡尔曼滤波算法的mems陀螺滤波方法 - Google Patents

Abstract

本发明涉及一种基于自适应卡尔曼滤波算法的MEMS陀螺滤波方法，包括以下步骤：步骤一：建立卡尔曼滤波器的状态方程和测量方程；步骤二：根据MEMS陀螺的静态角速度数据计算得到卡尔曼滤波算法的测量噪声方差阵，及迭代计算得到卡尔曼滤波算法的过程噪声方差阵；步骤三：利用卡尔曼滤波算法对MEMS陀螺的数据进行滤波；步骤四：根据滤波结果计算过程噪声方差阵的迭代系数，返回步骤二，对过程噪声方差阵进行更新迭代，直至完成MEMS陀螺的数据滤波。本发明的滤波方法对模型精度要求不高，可以自主迭代计算过程噪声方差阵，能够有效降低MEMS陀螺的随机噪声，实时提高MEMS陀螺动态数据输出精度，从而提高卫星的姿态稳定度。

Description

一种基于自适应卡尔曼滤波算法的MEMS陀螺滤波方法

技术领域

本发明涉及信号处理技术领域，特别是涉及一种基于自适应卡尔曼滤波算法的MEMS陀螺滤波方法。

背景技术

MEMS陀螺由于成本低、尺寸小、重量轻、可靠性高等优点，近年来在微纳卫星上的应用越来越广泛。然而受到现阶段工艺、材料等因素的制约，MEMS陀螺仪的输出精度较低。MEMS陀螺的误差源中随机噪声含量较大,并且容易受环境影响，具有慢时变、非平稳的特点。因此，如何对MEMS陀螺仪输出数据进行有效处理以提高其测量精度，已经成为商业航天领域重要研究方向。

目前针对陀螺随机噪声处理广泛采用的方法是基于时间序列分析法等方法对陀螺进行建模，然后使用卡尔曼滤波算法进行数字滤波。

然而传统卡尔曼滤波对MEMS陀螺模型精度、过程噪声方差阵精度和测量噪声方差阵精度要求很高，而基于地面测试数据获得的陀螺模型和过程噪声方差阵随着卫星在轨环境变化、运行时间推移将不再准确，所以将导致算法精度降低甚至发散，从而降低MEMS陀螺动态数据输出精度。

发明内容

为解决现有技术中传统卡尔曼滤波算法存在对MEMS陀螺模型精度、过程噪声方差阵精度和测量噪声方差阵精度依赖过高，算法精度容易降低甚至发散的问题，本发明提供一种基于自适应卡尔曼滤波算法的MEMS陀螺滤波方法。

为解决上述问题，本发明采取如下的技术方案：

一种基于自适应卡尔曼滤波算法的MEMS陀螺滤波方法，包括以下步骤：

步骤一：根据MEMS陀螺的数据采集周期建立卡尔曼滤波器的状态方程和测量方程；

步骤二：获取所述MEMS陀螺的静态角速度数据，根据所述静态角速度数据计算得到卡尔曼滤波算法的测量噪声方差阵R_k，以及使用公式

进行迭代计算得到卡尔曼滤波算法的过程噪声方差阵Q_k，其中

为过程噪声方差阵Q_k在k时刻的估计值，α_k为迭代系数，在第一次运算时

给任意值，α₀取1；

步骤三：利用所述卡尔曼滤波算法对所述MEMS陀螺的数据进行滤波；

步骤四：根据步骤三得到的滤波结果计算所述过程噪声方差阵Q_k的迭代系数，然后返回步骤二，对所述过程噪声方差阵Q_k进行更新迭代，直至完成所述MEMS陀螺的数据滤波。

与现有技术相比，本发明具有以下有益效果：

本发明所提出的一种基于自适应卡尔曼滤波算法的MEMS陀螺滤波方法对模型精度要求不高，可以自主迭代计算过程噪声方差阵，能够有效降低MEMS陀螺的随机噪声，实时提高MEMS陀螺动态数据输出精度，从而提高卫星的姿态稳定度，同时该滤波方法结构简单、易于实现，可应用于航空航天、惯性导航等领域，对于实际工程应用具有很大的现实意义。

附图说明

图1为本发明实施例提供的一种基于自适应卡尔曼滤波算法的MEMS陀螺滤波方法流程示意图；

图2为MEMS陀螺的静态角速度数据示意图；

图3为实施例二滤波前后MEMS陀螺角速度对比图；

图4为实施例二过程噪声方差阵更新结果示意图；

图5为实施例三滤波前后MEMS陀螺角速度对比图；

图6为实施例三过程噪声方差阵更新结果示意图。

具体实施方式

下面将结合附图及较佳实施例对本发明的技术方案进行详细描述。

实施例一

在本实施例中，如图1所示，本发明提供一种基于自适应卡尔曼滤波算法的MEMS陀螺滤波方法，该方法包括以下步骤：

步骤一(S1)：根据MEMS陀螺的数据采集周期建立卡尔曼滤波器的状态方程和测量方程；

步骤二(S2)：获取MEMS陀螺的静态角速度数据，根据静态角速度数据计算得到卡尔曼滤波算法的测量噪声方差阵R_k，以及使用公式

进行迭代计算得到卡尔曼滤波算法的过程噪声方差阵Q_k，其中

为过程噪声方差阵Q_k在k时刻的估计值，α_k为迭代系数，在第一次运算时

给任意值，α₀取1；

步骤三(S3)：利用卡尔曼滤波算法对MEMS陀螺的数据进行滤波；

步骤四(S4)：根据步骤三得到的滤波结果计算过程噪声方差阵Q_k的迭代系数，然后返回步骤二，对过程噪声方差阵Q_k进行更新迭代，直至完成MEMS陀螺的数据滤波。

具体地，步骤S1，建立卡尔曼滤波器的状态方程和测量方程：

利用MEMS陀螺的角速度和角加速度关系建立状态方程：

其中，w_k是MEMS陀螺在k时刻的角速度，ΔT为MEMS陀螺的采样周期，δ_k是MEMS陀螺在k时刻的角加速度，ν_1,k、ν_2,k是零均值的高斯白噪声信号，用于表征系统过程噪声，其对应过程噪声方差阵为

Q_11,k为ν_1,k的方差，Q_22,k为ν_2,k的方差。

定义卡尔曼滤波器k时刻的状态量X_k＝[w_k α_k]^T，则卡尔曼滤波器的状态方程如下：

X_k＝φ_k|k-1X_k-1+ν_k

其中，

为一步状态转移矩阵，ν_k＝[ν_1,k ν_2,k]^T为过程噪声向量。选择系统的测量为陀螺实际测量输出，即Z_k＝w_k，所以卡尔曼滤波器的测量方程为：

Z_k＝H_kX_k+ε_k

其中，H_k＝[1 0]为系统观测矩阵，ε_k是与ν_1,k、ν_2,k不相关的零均值高斯白噪声信号，用于表征系统测量噪声，其方差构成R_k。

步骤S2，确定测量噪声方差阵R_k和过程噪声方差阵Q_k：

对于测量噪声方差阵R_k，直接选取一段零均值静态数据，求其方差即可。具体地，通过获取MEMS陀螺的静态角速度数据，根据静态角速度数据计算得到卡尔曼滤波算法的测量噪声方差阵R_k。

对于过程噪声方差阵Q_k，由于建模的不准确以及在轨环境的变化无法实时准确获取，本发明采用如下公式进行迭代计算得到：

其中，

为过程噪声方差阵Q_k在k时刻的估计值，α_k为迭代系数，在第一次运算时

给任意值，α₀取1。

步骤3，使用卡尔曼滤波算法对MEMS陀螺数据进行滤波：

根据如下卡尔曼滤波原理进行滤波：

其中，

为卡尔曼滤波状态一步预测量，P_k|k-1为卡尔曼一步预测误差方差阵，K_k为卡尔曼滤波器增益矩阵，P_k为滤波误差方差阵，

为k时刻状态估计量。

步骤S4，计算过程噪声方差阵Q_k的迭代系数，完成算法迭代；

根据步骤S3得到的滤波结果计算过程噪声方差阵Q_k的迭代系数α_k，然后返回步骤S2，对过程噪声方差阵Q_k进行更新迭代，重复步骤S2～S4,直至完成MEMS陀螺的数据滤波。

可选地，根据步骤S3得到的滤波结果计算过程噪声方差阵的迭代系数包括以下步骤：

步骤四一：计算更新序列：

其中，Z_k为卡尔曼滤波器的测量方程，H_k为观测矩阵，

为卡尔曼滤波状态一步预测量。

步骤四二：通过一阶低通滤波器对更新序列的平方和进行滤波，得到

其中，b＝2·π·f_c·ΔT，ΔT为数据采集周期，f_c为一阶低通滤波器的截止频率，f_c的取值范围界于系统带宽和陀螺奈奎斯特频率之间。

步骤四三：计算过程噪声方差阵Q_k的迭代系数，计算公式如下：

其中，trace为求矩阵的迹。在完成以上运算后，重新运行步骤2～4，完成算法迭代。

本实施例所提出的一种基于自适应卡尔曼滤波算法的MEMS陀螺滤波方法对模型精度要求不高，可以自主迭代计算过程噪声方差阵，能够有效降低MEMS陀螺的随机噪声，实时提高MEMS陀螺动态数据输出精度，从而提高卫星的姿态稳定度，同时该滤波方法结构简单、易于实现，可应用于航空航天、惯性导航等领域，对于实际工程应用具有很大的现实意义。

接下来结合具体实施例对本发明的技术方案作进一步说明。

实施例二

步骤一，根据MEMS陀螺数据采集周期建立卡尔曼滤波器的状态方程和测量方程，此实施例中MEMS陀螺的采集周期为0.01s，所以滤波器的状态方程转移矩阵

观测矩阵H_k＝[1 0]，状态量X_k＝[w_k δ_k]^T。

步骤二，获取MEMS陀螺的静态角速度数据，剔除均值和趋势项，得到如图2所示数据，求取静态角速度数据方差作为后续滤波使用的测量噪声方差阵，例如在施例二中选择的某一款MEMS陀螺的噪声方差为4.9963e-04，所以选择的测量噪声方差阵R_k＝5e-04。

考虑到过程模型存在误差，尤其角加速度更新过程误差更大，所以本实施例选择的过程噪声方差阵初值(第一次运算时)为

迭代系数初值取1，后续每一步过程噪声方差阵使用公式

计算，其中

为过程噪声方差阵Q_k在k时刻的估计值，α_k为迭代系数。

步骤三，利用卡尔曼滤波原理进行卡尔曼滤波，其中滤波用到的滤波噪声方差阵初始值P₀设置为单位阵，即

状态变量初始值X₀中角速度信息使用第一拍测量值，角加速度信息置0，即X₀＝[Z₀ 0]^T。

步骤四，首先计算滤波更新序列

然后通过一阶低通滤波器对更新序列的平方和进行滤波，本实施例滤波选择的滤波器截止频率为0.5Hz，所以滤波器系数中b＝0.0314。然后根据公式

计算噪声方差阵的迭代系数。由于

和

计算结果为单维变量，所以矩阵的迹等于数据本身，所以运算量及其简化。最终，在完成迭代系数计算后，重新进行步骤三和步骤四的运算。图3为采用本实施例的滤波方法滤波前后MEMS陀螺角速度对比图，图4为过程噪声方差阵更新结果示意图。

实施例三

实施例三的实施过程和参数设置与实施例二基本一致，区别在于陀螺数据在前100s的噪声特性与实施例二一致，在后100s时陀螺的随机噪声幅值增大2倍。图5为采用本实施例的滤波方法滤波前后MEMS陀螺角速度对比图，图6为过程噪声方差阵更新结果示意图。在实施过程中只需要同步将测量噪声方差阵R_k放大四倍，可依然保持与100s前近似的滤波效果。

以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。

Claims (3)

1.一种基于自适应卡尔曼滤波算法的MEMS陀螺滤波方法，其特征在于，包括以下步骤：

步骤一：根据MEMS陀螺的数据采集周期建立卡尔曼滤波器的状态方程和测量方程；

步骤二：获取所述MEMS陀螺的静态角速度数据，根据所述静态角速度数据计算得到卡尔曼滤波算法的测量噪声方差阵R_k，以及使用公式

进行迭代计算得到卡尔曼滤波算法的过程噪声方差阵Q_k，其中

为过程噪声方差阵Q_k在k时刻的估计值，α_k为迭代系数，在第一次运算时

给任意值，α₀取1；

步骤三：利用所述卡尔曼滤波算法对所述MEMS陀螺的数据进行滤波；

步骤四：根据步骤三得到的滤波结果计算所述过程噪声方差阵Q_k的迭代系数，然后返回步骤二，对所述过程噪声方差阵Q_k进行更新迭代，直至完成所述MEMS陀螺的数据滤波。

2.根据权利要求1所述的一种基于自适应卡尔曼滤波算法的MEMS陀螺滤波方法，其特征在于，根据步骤三得到的滤波结果计算所述过程噪声方差阵的迭代系数包括以下步骤：

步骤四一：计算更新序列：

其中，Z_k为卡尔曼滤波器的测量方程，H_k为观测矩阵，

为卡尔曼滤波状态一步预测量；

步骤四二：通过一阶低通滤波器对所述更新序列的平方和进行滤波，得到

其中，b＝2·π·f_c·ΔT，ΔT为数据采集周期，f_c为一阶低通滤波器截止频率，取值范围界于系统带宽和陀螺奈奎斯特频率之间；

步骤四三：计算所述过程噪声方差阵Q_k的迭代系数，计算公式如下：

其中，trace为求矩阵的迹，P_k|k-1为卡尔曼一步预测误差方差阵。

3.根据权利要求1或2所述的一种基于自适应卡尔曼滤波算法的MEMS陀螺滤波方法，其特征在于：

利用所述MEMS陀螺的角速度和角加速度关系建立状态方程：

其中，w_k是所述MEMS陀螺在k时刻的角速度，ΔT为所述MEMS陀螺的采样周期，δ_k是所述MEMS陀螺在k时刻的角加速度，ν_1,k、ν_2,k是零均值的高斯白噪声信号，用于表征过程噪声；

定义卡尔曼滤波器k时刻的状态量X_k＝[w_k a_k]^T，则卡尔曼滤波器的状态方程如下：

X_k＝φ_k|k-1X_k-1+ν_k

其中，

为一步状态转移矩阵，ν_k＝[ν_1,k ν_2,k]^T为过程噪声向量；

卡尔曼滤波器的测量方程为：

Z_k＝H_kX_k+ε_k

其中，H_k＝[1 0]为系统观测矩阵，ε_k是与ν_1,k、ν_2,k不相关的零均值高斯白噪声信号，用于表征测量噪声。

Images