CN104596514B - 加速度计和陀螺仪的实时降噪系统和方法 - Google Patents

Abstract

本发明公开了一种加速度计和陀螺仪的实时降噪系统，包括惯性测量单元、信息处理单元、导航计算机，惯性测量单元提取加速度计和陀螺仪的量测信息并输送给信息处理单元进行实时降噪处理，最后由导航计算机对经过处理的量测信息加以利用输出导航信息。本发明还公开了一种加速度计和陀螺仪的实时降噪方法，通过建立关于加速度计和陀螺仪的单输入单输出卡尔曼滤波状态方程和观测方程，并根据机动情况在线调整卡尔滤波增益阵和观测噪声方差阵实现了利用卡尔曼滤波器的工作特性对加速度计和陀螺仪的量测信息进行降噪处理，有效降低量测随机噪声，通过在线调整滤波参数达到动、静态环境中加速度计和陀螺仪都有很好的量测精度。

Description

加速度计和陀螺仪的实时降噪系统和方法

技术领域

本发明涉及加速度计和陀螺仪降噪，尤其涉及一种加速度计和陀螺仪的实时降噪系统和方法。

背景技术

惯性测量单元被广泛的使用在惯性导航系统中，其中，惯性测量单元的零偏稳定性、常值误差、标度系数误差、非线性误差以及随机噪声对惯性导航系统的对准精度、导航解算精度都起着决定性作用。因此，提高惯性测量单元的量测精度和可靠性显得至关重要。提高惯性测量单元的量测精度的一种有效方法就是降低惯性测量单元的量测噪声，提高信噪比。

早期，针对惯性测量单元降噪的最简单的方法主要集中在低通滤波器的设计，但低通滤波器只对固定带宽的噪声有很好的滤波效果，所以只能针对特定环境下的特定信息进行有效处理，而不适用于任意机动环境且低通滤波器存在时间延迟的问题较为严重。

随着计算机性能的提高，离散小波变换技术也被广泛的应用于惯性测量单元的降噪处理，该方法在静态环境中的运用得到了很好的效果，但是在动态环境中该方法却失效且该方法也会引入时间延迟，降低信息的跟踪能力。

卡尔曼滤波可以有效处理系统过程随机噪声和量测随机噪声的能力，因此可以将卡尔曼滤波运用于惯性测量单元的随机噪声处理。现有的自适应卡尔曼滤波降噪方法虽然可以适应机动变化但是计算量较大，在高采样频率的实时环境中使用存在一定的困难。

发明内容

为了解决在动、静态环境中皆能有效滤除加速度计和陀螺仪随机量测噪声的问题，本发明提供了一种加速度计和陀螺仪的实时降噪系统及实时降噪方法，以达到减少滤波时延，降低计算量，提高加速度计和陀螺仪量测精度的效果。

为了解决上述问题，本发明一方面提供了一种加速度计和陀螺仪的实时降噪系统，包括：

惯性测量单元，用于提取加速度计和陀螺仪的量测信息，所述量测信息为被测载体的线加速度和角速度信息；

信息处理单元，用于对所述量测信息进行卡尔曼滤波并在线调整卡尔曼滤波器参数得到降噪后的量测信息；

导航计算机，用于利用所述降噪后的量测信息进行导航解算，输出所述被测载体的导航信息。

进一步地，所述信息处理单元包括：

建立方程模块，用于建立关于所述加速度计和陀螺仪的单输入单输出卡尔曼滤波状态方程和观测方程；

迭代计算模块，用于进行卡尔曼滤波迭代计算，并根据残差χ²检测法得到检测函数值γ_k，并将所述检测函数值γ_k与预先设定的阈值进行比较得出所述被测载体的机动变化情况；

在线调整模块，用于根据所述被测载体的机动变化情况调整卡尔曼滤波器的滤波增益矩阵K的值和观测噪声方差矩阵R的值，并将调整后的值反馈给迭代计算模块。

另一方面，本发明还提供了一种加速度计和陀螺仪的实时降噪方法，包括如下步骤：

提取加速度计和陀螺仪的量测信息，所述量测信息为被测载体的线加速度和角速度信息；

对所述量测信息进行卡尔曼滤波并在线调整卡尔曼滤波器参数得到降噪后的量测信息；

利用所述降噪后的量测信息进行导航解算，输出所述被测载体的导航信息。

进一步地，所述在线调整卡尔曼滤波器参数的具体步骤为：

S1、建立关于加速度计和陀螺仪的单输入单输出卡尔曼滤波状态方程和观测方程；

S2、进行卡尔曼滤波迭代计算，并根据残差χ²检测法得到检测函数值γ_k，并将所述检测函数值γ_k与预先设定的阈值进行比较得出所述被测载体的机动变化情况；

S3、根据所述机动变化情况在线调整卡尔曼滤波器的滤波增益矩阵K的值和观测噪声方差矩阵R的值。

为了较准确地反映被测载体的实际运动情况，所述步骤S1中建立关于加速度计和陀螺仪的单输入单输出卡尔曼滤波状态方程和观测方程的具体步骤为：

将扰动信息作为系统随机噪声，且在采样周期内设线加速度的变化率为0，以所述被测载体的线速度、线加速度为状态量建立所述加速度计的连续状态方程，以所述被测载体的线加速度和量测噪声建立所述加速度计的连续观测方程；

将扰动信息作为系统随机噪声，且在采样周期内设角加速度的变化率为0，以所述被测载体的角速度、角加速度为状态量建立所述陀螺仪的连续状态方程，以所述被测载体的角速度和量测噪声建立所述陀螺仪的连续观测方程；

分别对所述加速度计的连续状态方程和连续观测方程，所述陀螺仪的连续状态方程和连续观测方程进行离散化处理，得到所述加速度计的离散卡尔曼滤波状态方程和观测方程以及所述陀螺仪的离散卡尔曼滤波状态方程和观测方程。

为了更好地适应被测载体的实际运动情况，尤其是机动状态突变的情况，所述步骤S2中获取所述被测载体的机动变化情况的具体步骤为：

将所述检测函数值γ_k与所述阈值比较，若所述检测函数值γ_k大于所述阈值，则判决被所述被测载体处于快速机动变化状态；

若所述检测函数值γ_k小于阈值，则判决所述被测载体处于机动变化缓慢或匀速运动或静止的状态。

为了提高降噪性能，所述步骤S3中在线调整卡尔曼滤波的滤波增益矩阵K的值和观测噪声方差矩阵R的值的具体步骤为：

设置滤波初始值，包括：加速度计的观测噪声方差阵R_a、过程噪声方差阵Q_a、卡尔曼滤波的估计误差方差阵P_a和系统状态X_a，陀螺仪的观测噪声方差阵R_g、过程噪声方差阵Q_g、卡尔曼滤波的估计误差方差阵P_g和系统状态X_g；

当判决所述被测载体处于快速机动变化状态时，对当前迭代计算中的滤波增益矩阵K乘以一个大于1的加权系数并保持所述加速度计的观测噪声方差阵R_a和所述陀螺仪的观测噪声方差阵R_g的值不变，将得到的矩阵值用于后续迭代计算；

当判决所述被测载体处于机动变化缓慢或静止或匀速机动状态时，对当前迭代计算中的所述加速度计的观测噪声方差阵R_a和所述陀螺仪的观测噪声方差阵R_g的值分别乘以一个远大于1的加权系数且保持滤波增益阵K的值不变，将得到的矩阵值用于后续迭代计算。

为了保证加速计和陀螺测量值的滤波结果能够实时跟随载体的机动，需要适当将R_g和R_a初始值设置的小一些，所述加速度计的滤波初始值和所述陀螺仪的滤波初始值分别设置为：

X_g＝[0 0 0]^T，P_g＝diag[10 10 10]，

R_g＝0.00001，Q_g＝diag[3×10^-8 3×10^-8]，

X_a＝[0 0 0]^T，P_a＝diag[10 10 10]，

R_a＝0.00001，Q_a＝diag[5×10^-4 5×10^-4]。

本发明与现有技术相比，包括如下优点：

1.本发明针对加速度计和陀螺仪的特性分别建立了单输入单输出的卡尔曼滤波状态方程和观测方程，滤波系统将加速度计和陀螺仪的量测噪声作为卡尔曼滤波系统的观测噪声处理，利用卡尔曼滤波的工作特性，有效降低了加速度计和陀螺仪量测数据中的随机噪声。

2.本发明利用残差χ²检测法对载体机动变化情况进行实时监测，残差χ²检测法是一种无法确定故障原因、但能实时确定一组量测信息有效性的检测方法，常常被用于突变型故障检测，实现了对载体机动变化情况进行实时监测的功能。

3.本发明根据机动情况在线调整卡尔曼滤波的滤波增益阵K的值和观测噪声方差阵R阵的值。加速度计和陀螺仪一般运用在动态环境中，为了保证加速计和陀螺量测值经过滤波后的结果能够实时跟随载体的机动，需要适当将R_g和R_a的初始值设置的小一些。当系统被判为处于快速机动变化的阶段时，为了使滤波结果能够更快的跟踪载体的机动，可以保持R_g和R_a不变，同时，在当前卡尔曼滤波迭代计算的出的滤波增益阵K前乘以一个大于1的加权系数，然后在后续迭代计算中使用加权以后的K阵；当系统被判为处于缓慢机动变化或者静止或匀速机动的阶段时，为了增加系统的降噪性能，可以直接利用当前卡尔曼滤波迭代计算出的滤波增益阵K，但需要在R_g和R_a前分别乘以一个远远大于1的加权系数。基于此，本发明能够适应动、静态环境下的加速度计和陀螺仪的随机噪声处理。

附图说明

图1是本发明所述的加速度计和陀螺仪的实时降噪系统结构图；

图2是本发明所述的加速度计和陀螺仪的实时降噪方法在惯性测量单元天向轴从静止到转动到静止的陀螺仪输出滤波前后数据对比图；

图3是图2的局部放大图；

图4是本发明所述的加速度计和陀螺仪的实时降噪方法在惯性测量单元天向轴从静止到转动到静止的加速度计输出滤波前后数据对比图；

图5是图4的局部放大图；

图6是本发明所述的加速度计和陀螺仪的实时降噪方法在单轴摇摆情况下陀螺仪输出滤波前后数据对比图；

图7是图6的局部放大图；

图8是本发明所述的加速度计和陀螺仪的实时降噪方法在单轴摇摆情况下加速度计输出滤波前后数据对比图；

图9是图8的局部放大图。

具体实施方式

如图1所示，加速度计和陀螺仪的实时降噪系统，包括：惯性测量单元、信息处理单元、导航计算机，信息处理单元包括：建立方程模块、迭代计算模块、在线调整模块，惯性测量单元中加速度计检测载体在载体坐标系中的线加速度信息，陀螺仪检测载体在惯性空间中的角速度信息，并将加速度计和陀螺仪检测到的模拟信息转换成数字信息传输给信息处理单元，信息处理单元通过在线调节卡尔曼滤波器参数对检测信息进行处理得到量测信息，并将其输出至导航计算机；最终由导航计算机利用经过降噪处理的量测信息进行导航解算并输出导航信息。

信息处理单元在线调整卡尔曼滤波器参数的具体步骤为：建立方程模块建立关于加速度计和陀螺仪的单输入单输出卡尔曼滤波状态方程和观测方程；迭代计算模块利用残差χ²检测法对系统的机动变化情况进行实时检测；在线更新模块根据机动情况在线调整卡尔曼滤波的滤波增益阵K的值和观测噪声方差阵R阵的值，并将调整的结果用于后续迭代计算。

建立关于加速度计和陀螺仪的单输入单输出卡尔曼滤波状态方程和观测方程，具体步骤为：

对于加速度计，在加速度的测量过程中，由于加速度计实时测量载体的机动，较难确定扰动信息和控制信息，因此可以将系统模型中的控制量输入忽略，将扰动信息作为系统的随机噪声；同时，在一个短暂的采样周期内认为加速度为常值，加速度的变化率为0，因此，加速度计的状态方程和观测方程可以表示为：

Z(t)＝[0 1]·X(t)+v(t)＝C_a·X(t)+v(t)

其中，V为线速度，a为线加速度，w(t)为t时刻2×1维的系统过程噪声向量，通常表示为白噪声，Z(t)为t时刻加速度计量测信息，v(t)为t时刻1×1的量测噪声向量，通常也表示为白噪声。

对加速度计的状态方程和观测方程进行离散化处理，

Φ_a≈I+T·F，G_a≈T·B_a，H_a＝C_a

其中T为采样时间间隔。

得到离散化后的加速度计的系统状态方程和观测方程为：

X_k+1＝Φ_a·X_k+G_a·w_k

Z_k+1＝H_a·X_k+1+v_k+1

对于陀螺仪，假设在一个短暂的采样周期内，载体角速率可表示为一个定值，那么陀螺仪的状态方程和观测方程可以表示为：

Z(t)＝[1 0]·X(t)+v(t)＝C_g·X(t)+v(t)

其中，ω为角速率，ε为角加速度，w(t)为t时刻2×1维的系统过程噪声向量，通常表示为白噪声，Z(t)为t时刻陀螺仪的量测信息，v(t)为t时刻1×1的量测噪声，通常也表示为白噪声。

对陀螺仪的状态方程和观测方程进行离散化处理，

Φ_g≈I+T·F，G_g≈T·B_g，H_g＝C_g

其中T为采样时间间隔。

得到陀螺仪离散化后的系统状态方程和观测方程为：

X_k+1＝Φ_g·X_k+G_g·w_k

Z_k+1＝H_g·X_k+1+v_k+1

残差χ²检测法是一种无法确定故障原因、但能实时确定一组量测信息有效性的检测方法，常常被用于突变型故障检测，本实施例将其用于对被测载体机动变化情况进行实时检测，具体步骤为：

求取检测函数

其中，

P_k＝(I-K_kH_K)P_k/k-1，

r_k＝Z_k-H_kΦ_k/k-1X_k-1。

Φ_k/k-1为状态一步转移阵，在陀螺仪滤波系统中即对应Φ_g，在加速度计滤波系统中即对应Φ_a。R为系统量测噪声序列方差阵，Q为子系统过程噪声序列方差阵，下标k表示第k步递推时刻；H_k为k时刻系统观测矩阵，P_k/k-1为一步预测误差方差阵，P_k为估计误差方差阵，K_k为滤波增益矩阵，r_k为残差，γ_k表示检测函数值，下标k表示第k步递推时刻。

将检测函数值与阈值比较，若检测函数值大于阈值，则说明系统处于快速机动变化的阶段；

将检测函数值与阈值比较，若检测函数值小于阈值，则说明系统处于机动变化缓慢或者处于匀速运动或静止的运动状态。

根据机动情况在线调整卡尔曼滤波的滤波增益阵K的值和观测噪声方差阵R阵的值，其具体步骤为：

首先根据所选用的加速度计和陀螺仪厂商提供的仪表技术手册中对加速度计和陀螺仪的零偏、随机漂移等特性的说明，分别设置加速度计和陀螺仪的卡尔曼滤波中的估计误差方差阵P、系统量测噪声序列方差阵R、系统过程噪声序列方差阵Q的初始参数，以及对滤波状态初值赋0。

在动态环境中，加速度计和陀螺仪需及时测量载体的机动情况。因此为了保证加速计和陀螺测量值的滤波结果能够实时跟随载体的机动，需要适当将R_g和R_a初始值设置的小一些。

本实施例中滤波参数初值设置如下：

X_g＝[0 0 0]^T，P_g＝diag[10 10 10]，

R_g＝0.00001，Q_g＝diag[3×10^-8 3×10^-8]，

X_a＝[0 0 0]^T，P_a＝diag[10 10 10]，

R_a＝0.00001，Q_a＝diag[5×10^-4 5×10^-4]，

其中，X_g表示陀螺仪卡尔曼滤波系统的状态初值，P_g表示陀螺仪的卡尔曼滤波估计误差方差阵初值，R_g和Q_g分别表示陀螺仪滤波系统的量测噪声方差阵和过程噪声方差阵。X_a表示加速度计的卡尔曼滤波系统的状态初值，P_a表示加速度计的卡尔曼滤波估计误差方差阵初值，R_a和Q_a分别表示陀螺仪滤波系统的量测噪声方差阵和过程噪声方差阵。

当系统被判为处于快速机动变化的阶段时，为了使滤波结果能够更快的跟踪载体的机动，保持R_g和R_a不变，同时，在当前卡尔曼滤波迭代计算出的滤波增益阵K前乘以一个大于1的加权系数，然后在后续迭代计算中使用加权以后的K阵，本实施例中K阵的加权系数1.2。

当系统被判为处于静止或匀速机动的阶段时，为了增加系统的降噪性能，可以直接利用当前卡尔曼滤波迭代计算的出的滤波增益阵K，在R_g和R_a前分别乘以一个远远大于1的加权系数，本实施例中k_g＝1000，k_a＝10000。

本发明的重要特征在于：首先，建立关于加速度计和陀螺仪的单输入单输出卡尔曼滤波状态方程和观测方程；然后，利用残差χ²检测法对系统的机动变化情况进行实时检测；接着，根据机动情况在线调整卡尔曼滤波的滤波增益阵K的值和观测噪声方差阵R阵的值，实现任意机动环境中的加速度计和陀螺仪的实时降噪。

实验时将包含有三个陀螺和三个加速度计的惯性测量单元固定在转台正中心。其中加速度计和陀螺仪的性能指标如下：加速度计零偏不大于50μg(g＝9.8m²/s)，随机偏差不大于50μg；陀螺仪的常值漂移为0.006°/h，随机漂移不大于0.006°/加速度计零偏不大于50μg(g＝9.8m²/s)，随机偏差不大于50μg。

图2和图4中的机动方式为水平轴固定，天向轴先静止，然后以10°/s的速度转动，最后停止。其中实线为滤波后的仪表数据，虚线为滤波前传感器的输出数据。通过图2和图3可以看出静止状态下，本发明提出的加速度计和陀螺仪的实时降噪方法能够有效滤除陀螺仪量测信息中的随机噪声。绕天向轴旋转过程中，本发明提出的在线调节滤波参数的方法能够在滤除天向轴陀螺的随机噪声同时使滤波信息有效跟踪原始信息。通过图4和图5可以看出静止状态下，本发明提出的加速度计和陀螺仪的实时降噪方法能够有效滤除加速度计量测量中的随机噪声。

图6和图8中的机动方式为转台按照一定频率一定幅值摇摆，其中实线为滤波后的仪表数据，虚线为滤波前传感器的输出数据。通过图6和图7可以看出摇摆状态下，本发明提出的加速度计和陀螺仪的实时降噪方法在有效滤除陀螺仪量测信息中的随机噪声同时，还能较好保持滤波处理后的信息有效跟踪原始信息。通过图8和图9可以看出摇摆状态下，本发明提出的加速度计和陀螺仪的实时降噪方法也能够有效滤除加速度计量测量中的随机噪声。

本实例针对加速度计和陀螺仪的特性分别建立了单输入单输出的卡尔曼滤波状态方程和观测方程，滤波系统将加速度计和陀螺仪的量测噪声作为卡尔曼滤波系统的观测噪声处理。利用卡尔曼的工作特性，有效降低了加速度计和陀螺仪量测数据中的随机噪声。利用残差χ²检测法对载体机动变化情况进行实时监测，当检测函数值大于阈值时，说明系统处于快速机动变化的情况；当检测函数的值小于阈值时，说明系统的机动变化缓慢或者处于静止或匀速运动状态。实现了利用残差χ²检测法对载体机动变化情况进行实时监测的功能，根据机动情况在线调整卡尔曼滤波的滤波增益阵K的值和观测噪声方差阵R阵的值。为了保证加速计和陀螺量测值经过滤波后的结果能够实时跟随载体的机动，需要适当将R_g和R_a的初始值设置的小一些。当系统被判为处于快速机动变化的阶段时，为了使滤波结果能够更快的跟踪载体的机动，可以保持R_g和R_a不变，在当前卡尔曼滤波迭代计算的滤波增益阵K前乘以一个大于1的加权系数，然后在后续迭代计算中使用加权以后的K阵。当系统被判为处于缓慢机动变化或者静止或匀速机动的阶段时，为了增加系统的降噪性能，可以直接利用当前卡尔曼滤波迭代计算出的滤波增益阵K，在R_g和R_a前分别乘以一个远远大于1的加权系数。基于此，本发明能够适应动、静态环境下的加速度计和陀螺仪的随机噪声处理。

以上详细描述了本发明的优选实施方式，但是，本发明并不限于上述实施方式中的具体细节，在本发明的技术构思范围内，可以对本发明的技术方案进行多种等同变换，这些等同变换均属于本发明的保护范围。

Claims (6)

1.一种加速度计和陀螺仪的实时降噪系统，其特征在于，包括：

惯性测量单元，用于提取加速度计和陀螺仪的量测信息，所述量测信息为被测载体的线加速度和角速度信息；

信息处理单元，用于对所述量测信息进行卡尔曼滤波并在线调整卡尔曼滤波器参数得到降噪后的量测信息；

导航计算机，用于利用所述降噪后的量测信息进行导航解算，输出所述被测载体的导航信息；

所述信息处理单元包括：

建立方程模块，用于分别建立关于所述加速度计和陀螺仪的单输入单输出卡尔曼滤波状态方程和观测方程；

迭代计算模块，用于进行卡尔曼滤波迭代计算，并根据残差χ²检测法得到检测函数值γ_k，并将所述检测函数值γ_k与预先设定的阈值进行比较得出所述被测载体的机动变化情况；

在线调整模块，用于根据所述被测载体的机动变化情况调整卡尔曼滤波器的滤波增益矩阵K的值和观测噪声方差矩阵R的值，并将调整后的值反馈给迭代计算模块。

2.一种加速度计和陀螺仪的实时降噪方法，其特征在于，包括步骤：

提取加速度计和陀螺仪的量测信息，所述量测信息为被测载体的线加速度和角速度信息；

对所述量测信息进行卡尔曼滤波并在线调整卡尔曼滤波器参数得到降噪后的量测信息；

利用所述降噪后的量测信息进行导航解算，输出所述被测载体的导航信息；

所述在线调整卡尔曼滤波器参数的具体步骤为：

S1、建立关于加速度计和陀螺仪的单输入单输出卡尔曼滤波状态方程和观测方程；

S2、进行卡尔曼滤波迭代计算，并根据残差χ²检测法得到检测函数值γ_k，并将所述检测函数值γ_k与预先设定的阈值进行比较得出所述被测载体的机动变化情况；

S3、根据所述机动变化情况在线调整卡尔曼滤波器的滤波增益矩阵K的值和观测噪声方差矩阵R的值。

3.如权利要求2所述的加速度计和陀螺仪的实时降噪方法，其特征在于，所述步骤S1中建立关于加速度计和陀螺仪的单输入单输出卡尔曼滤波状态方程和观测方程的具体步骤为：

将扰动信息作为系统随机噪声，且在采样周期内设线加速度的变化率为0，以所述被测载体的线速度、线加速度为状态量建立所述加速度计的连续状态方程，以所述被测载体的线加速度和量测噪声建立所述加速度计的连续观测方程；

将扰动信息作为系统随机噪声，且在采样周期内设角加速度的变化率为0，以所述被测载体的角速度、角加速度为状态量建立所述陀螺仪的连续状态方程，以所述被测载体的角速度和量测噪声建立所述陀螺仪的连续观测方程；

分别对所述加速度计的连续状态方程和连续观测方程，所述陀螺仪的连续状态方程和连续观测方程进行离散化处理，得到所述加速度计的离散卡尔曼滤波状态方程和观测方程以及所述陀螺仪的离散卡尔曼滤波状态方程和观测方程。

4.如权利要求2所述的加速度计和陀螺仪的实时降噪方法，其特征在于，步骤S2中获取所述被测载体的机动变化情况的具体步骤为：

将所述检测函数值γ_k与所述阈值比较，若所述检测函数值γ_k大于所述阈值，则判决被所述被测载体处于快速机动变化状态；

若所述检测函数值γ_k小于阈值，则判决所述被测载体处于机动变化缓慢或匀速运动或静止的状态。

5.如权利要求2所述的加速度计和陀螺仪的实时降噪方法，其特征在于，所述步骤S3中在线调整卡尔曼滤波器的滤波增益矩阵K的值和观测噪声方差矩阵R的值的具体步骤为：

设置滤波初始值，包括：加速度计的观测噪声方差阵R_a、过程噪声方差阵Q_a、卡尔曼滤波的估计误差方差阵P_a和系统状态X_a，陀螺仪的观测噪声方差阵R_g、过程噪声方差阵Q_g、卡尔曼滤波的估计误差方差阵P_g和系统状态X_g；

当判决所述被测载体处于快速机动变化状态时，对当前迭代计算中的滤波增益矩阵K乘以一个大于1的加权系数并保持所述加速度计的观测噪声方差阵R_a和所述陀螺仪的观测噪声方差阵R_g的值不变，将得到的矩阵值用于后续迭代计算；

当判决所述被测载体处于机动变化缓慢或匀速运动或静止状态时，对当前迭代计算中的所述加速度计的观测噪声方差阵R_a和所述陀螺仪的观测噪声方差阵R_g的值分别乘以一个远大于1的加权系数且保持滤波增益阵K的值不变，将得到的矩阵值用于后续迭代计算。

6.如权利要求5所述的加速度计和陀螺仪的实时降噪方法，其特征在于，所述加速度计的滤波初始值和所述陀螺仪的滤波初始值分别设置为：

X_g＝[0 0 0]^T，P_g＝diag[10 10 10]，

R_g＝0.00001，Q_g＝diag[3×10^-8 3×10^-8]，

X_a＝[0 0 0]^T，P_a＝diag[10 10 10]，

R_a＝0.00001，Q_a＝diag[5×10^-4 5×10^-4]。