在姿态解算中,我们一般会利用角速度w和当前的四元数q(k)预测下一时刻的四元数q(k+1),但是有时候我们需要根据四元数反过来求角速度。
四元数的微分
令 q(t) 是一个单位四元数函数,ω(t) 是由 q(t) 确定的角速度。则 q(t) 的导数为:
在 t+Δt 时刻,旋转可以描述为 q(t+Δt)。在 Δt 过程中,物体坐标系在经过了 q(t) 旋转的前提下,又经过了额外的微小旋转。这个额外的微小旋转的瞬时旋转轴为 ω^=ω/∥ω∥,旋转角度为 Δθ=∥ω∥Δ,可以用一个单位四元数描述:
等式右边第一项是高阶项趋近于0,可省略。因此
离散形式
若dt为采样时间间隔
因此有:
如果为已经归一化,有, 上式两边同时乘以
上式中,就是需要求得的角速度。