传感器

使用华为honor7手机采集加速度计，陀螺仪和磁力计数据。
加速计数据在使用前经过低通滤波器。滤波结果如下图：

观测向量

如四元数姿态表示所述，载体姿态主要有三种表示方法。此处使用三个欧拉角度作为观测向量[ϕ;θ;ψ]
通过加速度计和磁力计数据计算姿态角观测量：
ϕ=tan−1(ayaz)
θ=−tan−1(axaz)
ψ=−tan−1My∗cos(ϕ)−Mz∗sin(ϕ)Mx∗cos(θ)+My∗sin(θ)∗sin(ϕ)+Mz∗sin(θ)∗cos(ϕ)

观测矩阵

欧拉角与四元数转化关系如下：
\phi= tan^{-1}\frac{2 * (q0 * q1 + q2 * q3)}{1 - 2* (q1 * q1 + q2 * q2)}
\theta = sin^{-1}(2\ast (q0 \ast q2 - q3 \ast q1))
\psi = tan^{-1}(2* (q0 * q3 + q1 * q2), 1- 2*(q2 * q2 + q3 * q3))
可以看出，[\phi;\theta;\psi]与[q_w,q_x,q_y,q_z,\delta_b^x,\delta_b^y,\delta_b^z]为非线性关系，需要线性化。
方法如matlab求解雅克比矩阵

状态

状态向量选取为[q_w,q_x,q_y,q_z,\delta_b^x,\delta_b^y,\delta_b^z]
q_w,q_x,q_y,q_z为四元数向量，\delta_b^x,\delta_b^y,\delta_b^z为机体坐标系下的陀螺零偏。

系统矩阵

系统矩阵参照四元数与载体姿态

结果

下图为解算结果，AHRS为本文模型滤出的结果，ekf为手机输出姿态信息。
从结果中可以看出，AHRS是要优于ekf的。

本文中AHRS实际上也是用的ekf方法，手机输出的姿态信息为了作区分起了个名字而已