基于ARM与MEMS的AHRS设计

　　卡尔曼增益矩阵Kk如式(12)所示，式中R阵为三维观测噪声协方差矩阵。

　　观测更新:

　　当重力加速度观测更新时zk为加速度，传感器测量得到的三维矢量zk=[aXaYaZ]T，当磁场观测更新时zk为磁阻传感器，测量得到的三维矢量zk=[mXmYmZ]T。协方差更新：

3　程序结构

　　AHRS的软件设计主要分为：

　　① 传感器初始化，包括设置传感器的更新速率、量程。

　　② 初始化卡尔曼滤波的相关矩阵，根据传感器的特点设置过程激励噪声协方差矩阵Q，设为对角元素为0.1的四维对角方阵。

　　③ 若成功读取陀螺仪数据，进行卡尔曼滤波的时间更新。

　　④ 采集加速度传感器和磁阻传感器的数据，若读取成功则进行观测更新。加速度观测更新与磁场观测更新算法差别在于观测方差的R，可根据两种传感器的置信度设置相应的值，航向姿态参考系的程序流程如图2所示。

　　图2　航向姿态参考系的程序流程

　　4　传感器校准

　　4.1　陀螺仪温度补偿

　　低成本MEMS陀螺仪存在着较大的零点偏移，一般可以达到1~3 °/s。可以通过增加扩展卡尔曼滤波的状态变量的维数，即增加三维陀螺零偏做数据融合得以解决。优点是可以动态地估计陀螺的零偏，有较强的适应能力，缺点是卡尔曼滤波算法的计算量以维数的三次方增加，因此实时性会有所降低，在要求姿态更新速率高而单片机计算性能有限的情况下，可以采用温度补偿的方法解决。陀螺仪的零点偏移与传感器温度和温度梯度密切相关。MPU6000陀螺仪温度变化缓慢的情况下，可以认为其温度零点漂移对应此时传感器的温度。