基于ARM与低成本MEMS器件的AHRS设计
当重力加速度观测更新时zk为加速度，传感器测量得到的三维矢量zk=[ax ay az]T，当磁场观测更新时zK为磁阻传感器，测量得到的三维矢量zK=[mx mymz]T。协方差更新：
3 程序结构
AHRS的软件设计主要分为：
①传感器初始化，包括设置传感器的更新速率、量程。
②初始化卡尔曼滤波的相关矩阵，根据传感器的特点设置过程激励噪声协方差矩阵Q，设为对角元素为0．1的四维对角方阵。
③若成功读取陀螺仪数据，进行卡尔曼滤波的时间更新。
④采集加速度传感器和磁阻传感器的数据，若读取成功则进行观测更新。加速度观测更新与磁场观测更新算法差别在于观测方差的R，可根据两种传感器的置信度没置相应的值，航向姿态参考系的程序流程如图2所示。本文引用地址：//m.amcfsurvey.com/article/148911.htm
4 传感器校准
4．1 陀螺仪温度补偿
低成本MEMS陀螺仪存在着较大的零点偏移，一般可以达到1～3°／s。可以通过增加扩展卡尔曼滤波的状态变量的维数，即增加三维陀螺零偏做数据融合得以解决。优点是可以动态地估计陀螺的零偏，有较强的适应能力，缺点是卡尔曼滤波算法的计算量以维数的三次方增加，因此实时性会有所降低，在要求姿态更新速率高而单片机计算性能有限的情况下，可以采用温度补偿的方法解决。陀螺仪的零点偏移与传感器温度和温度梯度密切相关。MPU-6000陀螺仪温度变化缓慢的情况下，可以认为其温度零点漂移对应此时传感器的温度。
可设温漂曲线为三次多项式BX=At3+Bt2+Ct1+D，通过最小二乘法拟合之后可得到各项系数。经过温度补偿后的陀螺仪曲线如图3所示。
评论