基于ARM与MEMS的AHRS设计
加入技术交流群
四元数姿态矩阵微分方程只要解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)所示。
评论