基于扩展卡尔曼滤波(EKF)和低成本传感器的AHRS实现

传感器

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

观测向量

如四元数姿态表示所述，载体姿态主要有三种表示方法。此处使用三个欧拉角度作为观测向量

[$\varphi ;\theta ;\psi$]

通过加速度计和磁力计数据计算姿态角观测量：

$\varphi =ta{n}^{-1}(\frac{a_y}{a_z})$
$\theta =-ta{n}^{-1}(\frac{a_x}{a_z})$
$\psi =-ta{n}^{-1}\frac{M_y\ast cos(\varphi )-M_z\ast sin(\varphi )}{M_x\ast cos(\theta )+M_y\ast sin(\theta )\ast sin(\varphi )+M_z\ast sin(\theta )\ast cos(\varphi )}$

观测矩阵

欧拉角与四元数转化关系如下：

$\varphi =ta{n}^{-1}\frac{2\ast (q0\ast q1+q2\ast q3)}{1-2\ast (q1\ast q1+q2\ast q2)}$
$\theta =si{n}^{-1}(2\ast (q0\ast q2-q3\ast q1))$
$\psi =ta{n}^{-1}(2\ast (q0\ast q3+q1\ast q2),1-2\ast (q2\ast q2+q3\ast q3))$
可以看出，[$\varphi ;\theta ;\psi$]与[$q_w,q_x,q_y,q_z,{\delta }_b^x,{\delta }_b^y,{\delta }_b^z$]为非线性关系，需要线性化。
方法如matlab求解雅克比矩阵或者使用python求解雅克比矩阵

状态向量

状态向量选取为

[$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$为机体坐标系下的陀螺零偏。

系统矩阵

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

测试结果

https://github.com/akstuki/INSFusion.git

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

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