目前MEMS传感器在消费类电子产品中得到广泛应用，但是MEMS角速率陀螺仪存在严重的零点漂移和随机误差，在捷联惯性导航解算中会产生积分误差，难以达到应用的精度。加速度计和磁场计能分别测量出重力加速度和地磁场这两个不相关的三维矢量，可以作为平台姿态的观测矢量来校准陀螺仪。扩展卡尔曼滤波可以结合这几种传感器的特点，以陀螺仪测量得到的角速率作预测更新，以重力加速度和磁场观测更新，得到更高精度的姿态角信息。
1 硬件结构
MEMS器件的AHRS硬件基本组成为三轴角速率陀螺仪、三轴加速度计、三轴磁阻传感器和STM32系列微处理器STM32F103U8T6。航向姿态参考系统的硬件结构如图1所示。
IMU采用整合了16位的三轴陀螺仪和三轴加速度计的MPU6000，与多组件方案相比，有效避免了组合陀螺仪与加速器时之轴间安装误差的问题，节省了安装空间。同时，内部自带了16位A/D转换器，简化了电路设计。MPU6000的角速率量程为±250 °/s、±500 °/s、±1000 °/s与±2000 °/s。加速度测量范围为±2g、±4g、±8g与±16g。内部自带16位的数字温度传感器，方便对传感器进行温度补偿。数据可通过最高可达400 kHz的I2C总线或最高可达20 MHz的SPI接口传输，采样更新速率达到8 kHz，可保证系统测量的实时性。
图1 航向姿态参考系统的硬件结构
霍尼韦尔HMC5883为三轴12位I2C总线数字量输出磁阻传感器，测量范围为±1~±8 Gs，数据更新速率为80 Hz。内置OFFSET/SET/RESET电路，不会出现磁饱和与累加误差现象，支持自动校准程序，简化使用步骤，可以满足地磁场的测量要求。选用基于CortexM3内核的STM32系列ARM处理器STM32F103U8T6，主频达72 MHz，1.25 DMIPS/MHz;具有硬件单周期乘法器，保证姿态更新的实时性;具有丰富外设接口，可采用I2C总线接口从传感器中读取数据，通过串口与上位机进行通信。
2 四维扩展卡尔曼滤波算法
扩展卡尔曼滤波算法(Extended Kalman Filter, EKF)是一套由计算机实现的实时递推算法，所处理的对象是随机信号，利用系统噪声和观测噪声的统计特性，以系统的观测量作为滤波器的输入，以所要求的估计值(系统的状态变量)作为滤波器的输出，滤波器的输入和输出由时间更新和观测更新算法联系在一起，根据系统的状态方程和观测方程估算出所需要处理的信号。AHRS扩展卡尔曼滤波算法的状态变量采用四维四元数，与采用欧拉角相比，避免了采用欧拉角计算时涉及的大量三角函数运算，保证了更新速率和实时性，同时不存在采用欧拉角运算出现的奇异性。欧拉角与四元数的转换关系如式(1)~(3)所示。
四元数微分方程如式(4)所示，
四元数姿态矩阵微分方程只要解4个微分方程，比方向余弦姿态矩阵微分方程减少了大量的运算，便于微处理器的编程实现。
2.1 时间更新
系统的状态方程如式(5)所示。
其中状态变量为四元数X=[q0,q1,q2,q3]T，Wk-1为四维过程噪声。矩阵A可以根据陀螺仪测得的三轴角速率[ωX，ωY，ωZ]T得到，如式(6)所示。其中Δt为两次时间预测更新所流逝的时间。
状态变量的时间更新如式(7)所示。
协方差矩阵P预测如式(8)所示，式中Q为四维过程激励噪声协方差。
2.2 观测更新
AHRS的观测更新是通过本体坐标系上的重力加速度和地磁场的参考矢量旋转至导航坐标系上，再与加速度和磁场传感器比较，得到观测变量的残余。由本体系转换至导航系的转移矩阵由四元数可以表示为式(9)。
三维参考向量v转移至导航系中可由观测方程式(10)表示。
当重力加速度观测更新时参考向量v等于重力加速度参考矢量(可设置为当平台静止水平放置时，加速度计测量得到的三维矢量为：
当磁场观测更新时v等于磁场参考矢量(可设置为当平台静止水平放置且航向指向正北时，磁阻计测量得到的三维矢量为：
H是h对X求偏导的雅可比矩阵，如式(11)所示。
卡尔曼增益矩阵Kk如式(12)所示，式中R阵为三维观测噪声协方差矩阵。
观测更新:
当重力加速度观测更新时zk为加速度，传感器测量得到的三维矢量zk=[aXaYaZ]T，当磁场观测更新时zk为磁阻传感器，测量得到的三维矢量zk=[mXmYmZ]T。协方差更新：