四元数微分求角速度【离散】

在姿态解算中，我们一般会利用角速度w和当前的四元数q(k)预测下一时刻的四元数q(k+1)，但是有时候我们需要根据四元数反过来求角速度。

 

四元数的微分

令 q(t) 是一个单位四元数函数，ω(t) 是由 q(t) 确定的角速度。则 q(t) 的导数为：

在 t+Δt 时刻，旋转可以描述为 q(t+Δt)。在 Δt 过程中，物体坐标系在经过了 q(t) 旋转的前提下，又经过了额外的微小旋转。这个额外的微小旋转的瞬时旋转轴为 ω^=ω/∥ω∥，旋转角度为 Δθ=∥ω∥Δ，可以用一个单位四元数描述： 

等式右边第一项是高阶项趋近于0，可省略。因此 

 

离散形式

若dt为采样时间间隔

因此有：

如果为已经归一化，有, 上式两边同时乘以

上式中，就是需要求得的角速度。