本文介绍了一种基于扩展卡尔曼滤波（EKF）和低成本传感器（如华为honor7手机的加速度计、陀螺仪、磁力计）实现AHRS（航姿参考系统）的方法。首先，通过低通滤波处理加速计数据，然后利用观测向量（欧拉角）和观测矩阵计算姿态。状态向量包含四元数和陀螺零偏，系统矩阵根据四元数与载体姿态关系确定。经过EKF处理，测试结果显示AHRS性能优于手机内置的ekf姿态信息。
传感器
使用华为honor7手机采集加速度计，陀螺仪和磁力计数据。
加速计数据在使用前经过低通滤波器。滤波结果如下图：
观测向量
如四元数姿态表示所述，载体姿态主要有三种表示方法。此处使用三个欧拉角度作为观测向量
[ ϕ ; θ ; ψ \phi;\theta;\psi ϕ;θ;ψ]
通过加速度计和磁力计数据计算姿态角观测量：
ϕ = t a n − 1 ( a y a z ) \phi=tan^{-1}(\frac{a_y}{a_z} ) ϕ=tan−1(azay)
θ = − t a n − 1 ( a x a z ) \theta=-tan^{-1}(\frac{a_x}{a_z}) θ=−tan−1(azax)
ψ = − t a n − 1 M y ∗ c o s ( ϕ ) − M z ∗ s i n ( ϕ ) M x ∗ c o s ( θ ) + M y ∗ s i n ( θ ) ∗ s i n ( ϕ ) + M z ∗ s i n ( θ ) ∗ c o s ( ϕ ) \psi=-tan^{-1}\frac{M_y*cos(\phi)-M_z*sin(\phi)}{M_x*cos(\theta) + M_y*sin(\theta)*sin(\phi) + M_z*sin(\theta)*cos(\phi)} ψ=−tan−1
最低0.47元/天 解锁文章
扫一扫
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
