已知一四元数,对时间微分得(其中
,
):
令,则有:
由一阶龙格库塔法,其中
,可得微分方程
的解:
,进而可推导出:
可得出更新四元数的程序代码为:
q0=q0+(-wx*q1-wy*q2-wz*q3)*halt_T;
q1=q1+(wx*q0+wz*q2-wy*q3)*halt_T;
q2=q2+(wy*q0-wz*q1+wx*q3)*halt_T;
q3=q3+(wz*q0+wy*q1-wx*q2)*halt_T;