姿态解算过程中四元数的更新

本文内容来自
【组合导航】四元数的运算及四元数微分理解

在之前的文章中，我们已经得到了四元数的微分表达形式；进一步地，我们需要将微分表达式离散化才能在嵌入式平台中实现基于四元数的姿态更新。
四元数的微分形式表示如下

求解在离散域中的四元数更新方程：

上式中$$\Delta {\theta }^2=\Delta {\theta }_x^2+\Delta {\theta }_y^2+\Delta {\theta }_z^2$$
类似的，我们还可以得到三阶和四阶算法如下：

或者直接使用欧拉法来进行求解：

嵌入式中的代码实现：

使用欧拉法得到的四元数更新形式

 float tmp0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
 float tmp1 = q1 + ( q0*gx + q2*gz - q3*gy)*halfT;
 float tmp2 = q2 + ( q0*gy - q1*gz + q3*gx)*halfT;
 float tmp3 = q3 + ( q0*gz + q1*gy - q2*gx)*halfT; 
 q0=tmp0;
 q1=tmp1;
 q2=tmp2;
 q3=tmp3;

使用二阶算法得到的四元数更新形式

 float delta = (dt * gx) * (dt * gx) + (dt * gy) * (dt * gy) + (dt * gz) * (dt * gz);
 float tmp0 = (1.0f - delta / 8.0f) * q0 + 0.5f * (-q1*gx - q2*gy - q3*gz) * dt;
 float tmp1 = (1.0f - delta / 8.0f) * q1 + 0.5f * ( q0*gx + q2*gz - q3*gy) * dt;
 float tmp2 = (1.0f - delta / 8.0f) * q2 + 0.5f * ( q0*gy - q1*gz + q3*gx) * dt;
 float tmp3 = (1.0f - delta / 8.0f) * q3 + 0.5f * ( q0*gz + q1*gy - q2*gx) * dt;
 q0=tmp0;
 q1=tmp1;
 q2=tmp2;
 q3=tmp3;