[发明专利]一种基于磁力计N阶距检测的MEMS系统误差估计方法

权利要求书

1.一种基于磁力计N阶距检测的MEMS系统误差估计方法，其特征在于，包含如下步骤：

步骤一、MEMS系统、磁力计开始工作，在计算机中实时存储MEMS系统输出的速度信息，磁力计测得的磁感应强度信息；

步骤二、利用N阶矩检测法判断是否存在外界磁干扰，所测试的N阶距差值若小于等于阈值，则触发MEMS系统误差估计卡尔曼滤波器工作，结合磁力计输出估计出MEMS系统的位置、速度、姿态误差，以及MEMS陀螺的刻度系数误差、陀螺漂移；若大于阈值，则MEMS系统误差估计卡尔曼滤波器关闭；

所涉及的N阶矩检测法判断条件为：

式中，b表示载体坐标系；表示t₁时刻b系下的磁场向量；k表示磁场恒定阶段的采样点数；||·||₂表示向量的二范数；N为阶矩值；v表示判断条件阈值；

所涉及的MEMS系统误差估计卡尔曼滤波器滤波器，其状态向量为：

xkdef=(xk1)T(xk2)T(xk3)T(xk4)T(xk5)TT]]>

，表示定义为；分别表示MEMS系统的位置、速度、姿态误差；表示刻度系数误差；表示MEMS陀螺漂移；

同时除去常规状态量外，关于刻度系数误差的微分方程为：

x·4=δK·g=0]]>

式中，δK_g为MEMS陀螺标度系数误差。

所涉及的MEMS系统误差估计卡尔曼滤波器的6维量测量Z_6×1为：

Z6×1=Z1TZ2TT=δVxδVyδVzδBxδByδBzT]]>

式中，Z₁表示MEMS系统速度差量测量，Z₂表示磁力计测得的当地磁场与已知地球磁场的磁场强度差量测量，δV_x,δV_y,δV_z表示速度差量测量在在当地水平坐标系三个轴上的分量,表示由惯导解算出速度估计值与真实值之间的误差，具体可由速度估计值与速度基准值做差即可，其中速度基准可由卫星或者多普勒等等设备直接读取；δB_x,δB_y,δB_z表示磁力计测得的当地磁场与已知地球磁场的磁场强度差在当地水平坐标系三个轴上的分量；

所涉及的MEMS系统误差估计卡尔曼滤波器的量测方程为：

Z1=[I3×3]δVxδVyδVz+wv]]>

式中，

w_v表示速度量测白噪声；w表示磁力计系统白噪声；分别为测力计测得的磁场强度；δK_gx,δK_gy,δK_gz分别为MEMS陀螺标度系数误差δK_g在当地水平坐标系三轴上的分量；ε_bx,ε_by,ε_bz分别为MEMS陀螺常值漂移ε_b在当地水平坐标系三轴上的分量；ε_rx,ε_ry,ε_rz分别为MEMS陀螺随机漂移ε_r在当地水平坐标系三轴上的分量；分别为载体坐标系对惯性坐标系转动角速率在当地水平坐标系三轴上的分量。