传感器
使用华为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 ∗