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

文章目录

传感器
观测向量
观测矩阵
状态向量
系统矩阵
测试结果

传感器

使用华为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−1Mx∗cos(θ)+My∗sin(θ)∗sin(ϕ)+Mz∗sin(θ)∗cos(ϕ)My∗cos(ϕ)−Mz∗sin(ϕ)

观测矩阵

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

ϕ = t a n − 1 2 ∗ ( q 0 ∗ q 1 + q 2 ∗ q 3 ) 1 − 2 ∗ ( q 1 ∗ q 1 + q 2 ∗ q 2 ) \phi= tan^{-1}\frac{2 * (q0 * q1 + q2 * q3)}{1 - 2* (q1 * q1 + q2 * q2)} ϕ=tan−11−2∗(q1∗q1+q2∗q2)2∗(q0∗q1+q2∗q3)
θ = s i n − 1 ( 2 ∗ ( q 0 ∗ q 2 − q 3 ∗ q 1 ) ) \theta = sin^{-1}(2\ast (q0 \ast q2 - q3 \ast q1)) θ=sin−1(2∗(q0∗q2−q3∗q1))
ψ = t a n − 1 ( 2 ∗ ( q 0 ∗ q 3 + q 1 ∗ q 2 ) , 1 − 2 ∗ ( q 2 ∗ q 2 + q 3 ∗ q 3 ) ) \psi = tan^{-1}(2* (q0 * q3 + q1 * q2), 1- 2*(q2 * q2 + q3 * q3)) ψ=tan−1(2∗(q0∗q3+q1∗q2),1−2∗(q2∗q2+q3∗q3))
可以看出，[ ϕ ; θ ; ψ \phi;\theta;\psi ϕ;θ;ψ]与[ q w , q x , q y , q z , δ b x , δ b y , δ b z q_w,q_x,q_y,q_z,\delta_b^x,\delta_b^y,\delta_b^z qw,qx,qy,qz,δbx,δby,δbz]为非线性关系，需要线性化。
方法如matlab求解雅克比矩阵或者使用python求解雅克比矩阵

状态向量

状态向量选取为

[ q w , q x , q y , q z , δ b x , δ b y , δ b z q_w,q_x,q_y,q_z,\delta_b^x,\delta_b^y,\delta_b^z qw,qx,qy,qz,δbx,δby,δbz]

q w , q x , q y , q z q_w,q_x,q_y,q_z qw,qx,qy,qz为四元数向量， δ b x , δ b y , δ b z \delta_b^x,\delta_b^y,\delta_b^z δbx,δby,δbz为机体坐标系下的陀螺零偏。

系统矩阵

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

测试结果

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

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

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