基于ARM与低成本MEMS器件的AHRS设计
四元数微分方程如式（4）所示，四元数姿态矩阵微分
方程只要解4个微分方程，比方向余弦姿态矩阵微分方程减少了大量的运算，便于微处理器的编程实现。
2．1 时间更新
系统的状态方程如式(5)所示。
其中状态变量为四元数X=[q0，q1，q2，q3]T，Wk-1为四维过程噪声。矩阵A可以根据陀螺仪测得的三轴角速率[ωx，ωy，ωz]T得到，如式(6)所示。其中△t为两次时间预测更新所流逝的时间。
2．2 观测更新
AHRS的观测更新是通过本体坐标系上的重力加速度和地磁场的参考矢量旋转至导航坐标系上，再与加速度和磁场传感器比较，得到观测变量的残余。由本体系转换至导航系的转移矩阵由四元数可以表示为式(9)。
当重力加速度观测更新时参考向量v等于重力加速度参考矢量(可设置为当平台静止水平放置时，加速度计测量得到的三维矢量为：
v=[AX AY AZ]T)
当磁场观测更新时v等于磁场参考矢量(可设置为当平台静止水平放置且航向指向正北时，磁阻计测量得到的三维矢量为：
v=[MX MY MZ]T)
H是h对X求偏导的雅可比矩阵，如式(11)所示。
卡尔曼增益矩阵Kk如式(12)所示，式中R阵为三维观测噪声协方差矩阵。
评论