[发明专利]阵列式MEMS磁传感器实时标定方法

权利要求书

1.一种阵列式MEMS磁传感器实时标定方法，其特征在于，包括：

获取阵列MEMS磁传感器实时数据并进行预处理；

将阵列式MEMS磁传感器数据进行均值运算；

建立统一的阵列式MEMS磁传感器标定模型；

通过参数模型，设计自适应Kalman滤波算法，实现参数估计，标定过程采样点数为M，若k＝M，则输出标定结果，完成阵列式MEMS磁传感器标定过程，若k＜M，表示标定过程未完成，则重复上述步骤，直至标定过程结束，其中，k和M都是正整数；

“获取阵列MEMS磁传感器实时数据并进行预处理；”具体包括：

由MEMS磁传感器测量模型可知：

式中，表示MEMS磁传感器测量结果；S_f表示比例因子，C_no表示非正交矩阵，C_si表示软铁干扰误差，表示导航系到载体系的方向余弦矩阵，mⁿ表示导航系下地磁场矢量，b_hi表示硬铁干扰误差，b_m表示偏置误差，η_m表示随机噪声；

当不考虑MEMS磁传感器存在位置变化时，MEMS磁传感器测量模型可以表示为：

式中，表示MEMS磁传感器测量结果；C＝S_fC_noC_si表示综合变换误差矩阵；b＝S_fC_nob_hi+b_m表示综合偏置误差；表示载体系下地磁场矢量；η＝η_m表示随机噪声；

考虑阵列式MEMS磁传感器测量时，其测量模型可以表示为：

式中，表示第i个磁传感器测量结果；C_i表示第i个磁传感器综合变换误差矩阵；表示地磁矢量在第i个磁传感器坐标系上的映射；b_i表示第i个磁传感器综合偏置误差；η_i表示第i个传感器随机噪声；

“将阵列式MEMS磁传感器数据进行均值运算；”具体包括：

阵列式MEMS磁传感器测量模型可知，对测量参数进行均值运算为：

式中，表示阵列式MEMS磁传感器测量均值；N表示构成阵列式MEMS磁传感器的个数；C_i表示第i个磁传感器综合变换误差矩阵；表示地磁矢量在第i个磁传感器坐标系上的映射；表示阵列式MEMS磁传感器综合偏置误差的均值；表示阵列式MEMS磁传感器随机噪声均值；参数可以用下式计算：

式中，表示阵列式MEMS磁传感器测量均值；表示第i个磁传感器测量结果；N表示构成阵列式MEMS磁传感器的个数；表示阵列式MEMS磁传感器综合偏置误差的均值；b_i表示第i个磁传感器综合偏置误差；表示阵列式MEMS磁传感器随机噪声均值；η_i表示第i个传感器随机噪声；

“建立统一的阵列式MEMS磁传感器标定模型；”具体包括：对阵列式MEMS磁传感器，在阵列中心定义一个载体坐标系b，此时每个MEMS磁传感器坐标系上的地磁场映射值与中心处的载体坐标系上的地磁场映射值之间的关系可以表示为：

式中，表示地磁矢量在第i个磁传感器坐标系上的映射；表示载体坐标系到第i个MEMS磁传感器坐标系的方向余弦矩阵；m^b表示载体系下地磁场矢量；

由于每个MEMS磁传感器坐标系与阵列中心载体坐标系之间不存在相对运动，因此可以认为是一个常值；所以，阵列式MEMS磁传感器测量模型可以重新表示为

式中，表示阵列式MEMS磁传感器测量均值；N表示构成阵列式MEMS磁传感器的个数；C_i表示第i个磁传感器综合变换误差矩阵；表示载体坐标系到第i个MEMS磁传感器坐标系的方向余弦矩阵；m^b表示载体系下地磁场矢量；表示阵列式MEMS磁传感器综合偏置误差的均值；表示阵列式MEMS磁传感器随机噪声均值；

定义

式中，表示综合变换误差均值矩阵；N表示构成阵列式MEMS磁传感器的个数；C_i表示第i个磁传感器综合变换误差矩阵；表示载体坐标系到第i个MEMS磁传感器坐标系的方向余弦矩阵；

由上面推导可知，统一的阵列式MEMS磁传感器测量模型可以表示为：

式中，表示阵列式MEMS磁传感器测量均值；表示综合变换误差均值矩阵；m^b表示载体系下地磁场矢量；表示阵列式MEMS磁传感器综合偏置误差的均值；表示阵列式MEMS磁传感器随机噪声均值；

“通过参数模型，设计自适应Kalman滤波算法，实现参数估计；”具体为：

对统一的阵列式MEMS磁传感器测量模型进行变换之后可得：

式中，m^b表示载体系下地磁场矢量；表示综合变换误差均值矩阵；表示阵列式MEMS磁传感器测量均值；表示阵列式MEMS磁传感器综合偏置误差的均值；表示阵列式MEMS磁传感器随机噪声均值；

对上式两边进行模值平方运算：

式中，m^b表示载体系下地磁场矢量；表示阵列式MEMS磁传感器测量均值；表示阵列式MEMS磁传感器综合偏置误差的均值；表示阵列式MEMS磁传感器随机噪声均值；R表示由综合变换误差均值矩阵构造的中间矩阵，可以由下式表示：

式中，表示综合变换误差均值矩阵；T表示矩阵转置运算；

定义阵列式MEMS磁传感器测量均值为阵列式MEMS磁传感器综合偏置误差的均值为则可构造参数模型：

式中，表示位置参数矢量的微分；y表示量测；H表示参数矩阵；x表示未知参数；ν表示量测噪声；其中各参数可以表示为：

x＝[x₁ x₂ x₃ x₄ x₅ x₆ x₇ x₈ x₉]^T

H＝[h₁ h₂ h₃ h₄ h₅ h₆ h₇ h₈ h₉]

式中，x_i表示参数矢量的第i个元素,其中，i＝1…9；h_i表示参数矩阵的第i个元素，其中，i＝1…9；y表示量测；表示阵列式MEMS磁传感器测量均值在x轴上的分量；表示阵列式MEMS磁传感器测量均值在y轴上的分量；表示阵列式MEMS磁传感器测量均值在z轴上的分量；ν表示量测噪声；表示阵列式MEMS磁传感器测量均值；表示阵列式MEMS磁传感器综合偏置误差的均值；分别表示阵列式MEMS磁传感器综合偏置误差均值在x、y、z轴上的分量；表示阵列式MEMS磁传感器随机噪声均值；R表示由综合变换误差均值矩阵构造的中间矩阵；m^b表示载体系下地磁场矢量；r_ij表示矩阵中的第i行j列元素,其中,i＝1…3,j＝1…3；

基于上述统一的阵列式MEMS磁传感器测量模型构造的参数方程，可以构造自适应Kalman滤波参数估计算法：

式中，e_k表示k时刻的残差；y_k表示k时刻的量测；H_k表示k时刻的参数矩阵；表示k-1时刻的估计参数矢量；Λ_k表示k时刻的自适应量测噪声；Λ_k-1表示k-1时刻的自适应量测噪声；表示测量速度噪声的标准差；K_k表示k时刻的增益矩阵；P_k-1表示k-1时刻的误差协方差阵；表示k时刻的估计参数矢量；P_k表示k时刻的误差协方差阵；根据上述参数估计，结合参数解算，即可实现阵列式MEMS磁传感器统一量测模型下的参数标定。

2.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现如下步骤：获取阵列MEMS磁传感器实时数据并进行预处理；

将阵列式MEMS磁传感器数据进行均值运算；

建立统一的阵列式MEMS磁传感器标定模型；

通过参数模型，设计自适应Kalman滤波算法，实现参数估计，标定过程采样点数为M，若k＝M，则输出标定结果，完成阵列式MEMS磁传感器标定过程，若k＜M，表示标定过程未完成，则重复上述步骤，直至标定过程结束，其中，k和M都是正整数；

“获取阵列MEMS磁传感器实时数据并进行预处理；”具体包括：

由MEMS磁传感器测量模型可知：

式中，表示MEMS磁传感器测量结果；S_f表示比例因子，C_no表示非正交矩阵，C_si表示软铁干扰误差，表示导航系到载体系的方向余弦矩阵，mⁿ表示导航系下地磁场矢量，b_hi表示硬铁干扰误差，b_m表示偏置误差，η_m表示随机噪声；

当不考虑MEMS磁传感器存在位置变化时，MEMS磁传感器测量模型可以表示为：

式中，表示MEMS磁传感器测量结果；C＝S_fC_noC_si表示综合变换误差矩阵；b＝S_fC_nob_hi+b_m表示综合偏置误差；表示载体系下地磁场矢量；η＝η_m表示随机噪声；

考虑阵列式MEMS磁传感器测量时，其测量模型可以表示为：

式中，表示第i个磁传感器测量结果；C_i表示第i个磁传感器综合变换误差矩阵；表示地磁矢量在第i个磁传感器坐标系上的映射；b_i表示第i个磁传感器综合偏置误差；η_i表示第i个传感器随机噪声；

“将阵列式MEMS磁传感器数据进行均值运算；”具体包括：

阵列式MEMS磁传感器测量模型可知，对测量参数进行均值运算为：

式中，表示阵列式MEMS磁传感器测量均值；N表示构成阵列式MEMS磁传感器的个数；C_i表示第i个磁传感器综合变换误差矩阵；表示地磁矢量在第i个磁传感器坐标系上的映射；表示阵列式MEMS磁传感器综合偏置误差的均值；表示阵列式MEMS磁传感器随机噪声均值；参数可以用下式计算：

式中，表示阵列式MEMS磁传感器测量均值；表示第i个磁传感器测量结果；N表示构成阵列式MEMS磁传感器的个数；表示阵列式MEMS磁传感器综合偏置误差的均值；b_i表示第i个磁传感器综合偏置误差；表示阵列式MEMS磁传感器随机噪声均值；η_i表示第i个传感器随机噪声；

“建立统一的阵列式MEMS磁传感器标定模型；”具体包括：对阵列式MEMS磁传感器，在阵列中心定义一个载体坐标系b，此时每个MEMS磁传感器坐标系上的地磁场映射值与中心处的载体坐标系上的地磁场映射值之间的关系可以表示为：

式中，表示地磁矢量在第i个磁传感器坐标系上的映射；表示载体坐标系到第i个MEMS磁传感器坐标系的方向余弦矩阵；m^b表示载体系下地磁场矢量；

由于每个MEMS磁传感器坐标系与阵列中心载体坐标系之间不存在相对运动，因此可以认为是一个常值；所以，阵列式MEMS磁传感器测量模型可以重新表示为

式中，表示阵列式MEMS磁传感器测量均值；N表示构成阵列式MEMS磁传感器的个数；C_i表示第i个磁传感器综合变换误差矩阵；表示载体坐标系到第i个MEMS磁传感器坐标系的方向余弦矩阵；m^b表示载体系下地磁场矢量；表示阵列式MEMS磁传感器综合偏置误差的均值；表示阵列式MEMS磁传感器随机噪声均值；

定义

式中，表示综合变换误差均值矩阵；N表示构成阵列式MEMS磁传感器的个数；C_i表示第i个磁传感器综合变换误差矩阵；表示载体坐标系到第i个MEMS磁传感器坐标系的方向余弦矩阵；

由上面推导可知，统一的阵列式MEMS磁传感器测量模型可以表示为：

式中，表示阵列式MEMS磁传感器测量均值；表示综合变换误差均值矩阵；m^b表示载体系下地磁场矢量；表示阵列式MEMS磁传感器综合偏置误差的均值；表示阵列式MEMS磁传感器随机噪声均值；

“通过参数模型，设计自适应Kalman滤波算法，实现参数估计；”具体为：

对统一的阵列式MEMS磁传感器测量模型进行变换之后可得：

式中，m^b表示载体系下地磁场矢量；表示综合变换误差均值矩阵；表示阵列式MEMS磁传感器测量均值；表示阵列式MEMS磁传感器综合偏置误差的均值；表示阵列式MEMS磁传感器随机噪声均值；

对上式两边进行模值平方运算：

式中，m^b表示载体系下地磁场矢量；表示阵列式MEMS磁传感器测量均值；表示阵列式MEMS磁传感器综合偏置误差的均值；表示阵列式MEMS磁传感器随机噪声均值；R表示由综合变换误差均值矩阵构造的中间矩阵，可以由下式表示：

式中，表示综合变换误差均值矩阵；T表示矩阵转置运算；

定义阵列式MEMS磁传感器测量均值为阵列式MEMS磁传感器综合偏置误差的均值为则可构造参数模型：

式中，表示位置参数矢量的微分；y表示量测；H表示参数矩阵；x表示未知参数；ν表示量测噪声；其中各参数可以表示为：

x＝[x₁ x₂ x₃ x₄ x₅ x₆ x₇ x₈ x₉]^T

H＝[h₁ h₂ h₃ h₄ h₅ h₆ h₇ h₈ h₉]

式中，x_i表示参数矢量的第i个元素,其中，i＝1…9；h_i表示参数矩阵的第i个元素，其中，i＝1…9；y表示量测；表示阵列式MEMS磁传感器测量均值在x轴上的分量；表示阵列式MEMS磁传感器测量均值在y轴上的分量；表示阵列式MEMS磁传感器测量均值在z轴上的分量；ν表示量测噪声；表示阵列式MEMS磁传感器测量均值；表示阵列式MEMS磁传感器综合偏置误差的均值；分别表示阵列式MEMS磁传感器综合偏置误差均值在x、y、z轴上的分量；表示阵列式MEMS磁传感器随机噪声均值；R表示由综合变换误差均值矩阵构造的中间矩阵；m^b表示载体系下地磁场矢量；r_ij表示矩阵中的第i行j列元素,其中,i＝1…3,j＝1…3；

基于上述统一的阵列式MEMS磁传感器测量模型构造的参数方程，可以构造自适应Kalman滤波参数估计算法：

式中，e_k表示k时刻的残差；y_k表示k时刻的量测；H_k表示k时刻的参数矩阵；表示k-1时刻的估计参数矢量；Λ_k表示k时刻的自适应量测噪声；Λ_k-1表示k-1时刻的自适应量测噪声；表示测量速度噪声的标准差；K_k表示k时刻的增益矩阵；P_k-1表示k-1时刻的误差协方差阵；表示k时刻的估计参数矢量；

P_k表示k时刻的误差协方差阵；根据上述参数估计，结合参数解算，即可实现阵列式MEMS磁传感器统一量测模型下的参数标定。