文档介绍：基于自适应卡尔曼滤波的九轴磁罗盘算法设计
 
   
 
 
 
 
 
 
 
     
 
 
 
 
 
摘要针对九轴磁罗盘姿态解算的问题，提出了一种基于三轴MEMS陀螺仪、三轴MEMS加速度计、三轴磁力计的自适应姿态解算算法：将陀螺仪数据作为预测数据，估计出卡尔曼的过程协方差；加速度计和磁力计数据作为观测数据，结合陀螺仪的误差估计出测量噪声协方差，通过自适应卡尔曼滤波实现多传感器数据融合，解算出九轴磁罗盘准确的姿态。实验结果表明：本文设计的自适应滤波算法有效的抑制了噪声干扰，降低随机误差干扰；并且能够通过自适应补偿位移加速度、外界磁干扰等因素对姿态估计的影响。
关键词九轴磁罗盘；姿态解算；卡尔曼滤波
0 引言
随着MEMS技术的不断发展，体积小、质量轻、功耗低的MEMS惯性传感器的应用范围由传统的航空航天、工业控制等拓展到了更为广泛的领域[1]，尤其对微小型运载体、便携设备等的姿态解算具有巨大的推动作用。基于MEMS惯性器件构建低成本微型航姿系统正成为一个研究热点[2~3]。
目前，常用测量姿态的传感器有陀螺仪、磁力计、加速度计等。陀螺仪测得的姿态角实时性好、短期精度高，但易受到温漂、时漂的影响；磁力计、加速度计不受初始位置限制，误差不随时间积累，但易受外部磁场以及载体自身加速度干扰[4]，因此单个传感器很难得到相对准确的姿态角信息。为了解决这类问题，需要对多传感器数据进行融合处理，才能够提高姿态解算的精度。三轴MEMS陀螺仪、三轴MEMS加速度计、三轴磁力计是测量载体姿态的九轴磁罗盘常用组合[5]。
1 基于Kalman滤波算法的算法设计
加速度计/磁力计姿态解算
九轴磁罗盘中三轴MEMS陀螺仪的输出即为载体的横滚角速率，俯仰角速率和方位角速率，通过相应的解算方法可以得到载体的姿态角信息，通过对三轴加速度计和三轴磁力计的信息进行计算可以得到静止或匀速运动状态下的方位角、俯仰角和横滚角。本文中方位角
α定义为：载体纵轴在水平面上的投影与地球子午线之间的夹角，数值以地理北向为起点顺时针方向为正，定义域为[0°~360°]；俯仰角β定义为：载体的纵向轴和当地水平面之间的夹角，向上为正，向下为负，定义域为[-90°~90°]；横滚角γ定义为：载体的横向轴与当地水平面之间的夹角，右倾为正，左倾为负，定义域为[-180°~180°]。
当载体在地理坐标系下处于水平静止时，加速度计各轴测量输出为A=[0,0,g]。当载体处于任意姿态静止时，假设加速度计测量值为B=[ax,ay,az]，可计算俯仰、横滚角：
(1)
载体坐标系与地理坐标系是不重合的。此时，地球磁场在各个轴的分量会产生叠加，三轴磁力计测得的地球磁场在载体坐标系三个轴上的分量，应先求出它们在水平面内的投影，才能求解方位角。
(2)
建立系统数学模型
系统状态方程
根据惯性导航姿态四元数理论，设四元数，则状态四元数微分方程：
(3)
取Kalman滤波器状态向量为，其中为三轴陀螺仪零偏，将式()用一阶泰勒展开式求解可以建立系统的状态方程：
(4)
其中，。
系统量测方程
利用空间矢量与转动四元数间的关系，可以推导出一种基于伪量测方程的新型四元