[发明专利]MEMS陀螺仪动态数据的滤波方法

权利要求书

1.一种MEMS陀螺仪动态数据的滤波方法，其特征在于：其步骤是：

(1)确定MEMS陀螺仪常值漂移：当MEMS陀螺仪稳定工作时，分四次分别采集一小时的静态输出数据，计算四次MEMS陀螺仪静态数据的均值作为陀螺仪的常值漂移，以后采集的每个数据都需要减掉此项常值漂移；

(2)建立MEMS陀螺仪输出数据模型：陀螺仪动态输出数据序列为X_k，动态数据差分序列为D_k，其中D_k＝X_k+1-X_k；

MEMS陀螺仪输出数据模型为：

其中状态向量为MEMS陀螺仪输出角速度信号X_k和角速度差分信号D_k，其对应的状态输出分别为x_k和d_k，W_k，V_k分别为系统过程噪声和测量噪声，其作用在差分信号D_k上；

(3)确定Kalman滤波过程噪声方差Q和测量噪声方差R：过程噪声和测量噪声是与时间无关的高斯白噪声，它们的方差Q和R均为常数，Q和R的值需要通过实验来确定，其中

(4)对动态数据进行Kalman滤波：动态数据的Kalman滤波模型即为步骤2中建立的MEMS陀螺仪输出数据模型；

对动态数据进行Kalman滤波的步骤为：

①状态一步预测：通过上一时刻的最优状态估计值来预测本时刻的状态；系统状态向量状态转移矩阵

②计算一步预测误差方差矩阵：用上一时刻估计误差方差和过程噪声方差计算一步预测误差方差；系统过程噪声输入矩阵

③计算滤波增益矩阵：通过一步预测方差和测量噪声方差计算Kalman滤波增益；系统测量矩阵

④状态估计：应用陀螺仪的实际输出数据来修正一步预测结果，从而得到最优估计；Z_k为k时刻陀螺仪实际输出的角速度信号和角速度差分信号组成的向量；

⑤计算估计误差方差矩阵：P_k＝[I-K_kH_k]P_k，k-1；计算本次估计误差方差，为下一次的估计做准备；单位对角矩阵

⑥状态输出：

⑦确定状态初值和估计误差方差初值P₀，选取状态初值时不要偏离真实初始状态过多，估计误差方差初值P₀选取在(0，1)范围内，选取状态初值选取估计误差方差初值