四元数计算要防止低级错误

在导航程序，特别是惯性导航程序中，常常会用到四元数的更新计算。

四元数更新的公式为：

    q[0]=q[0]+0.5*dt*(-wx*q[1]-wy*q[2]-wz*q[3]);
    q[1]=q[1]+0.5*dt*(+wx*q[0]-wy*q[3]+wz*q[2]);
    q[2]=q[2]+0.5*dt*(+wx*q[3]+wy*q[0]-wz*q[1]);
    q[3]=q[3]+0.5*dt*(-wx*q[2]+wy*q[1]+wz*q[0]);

很多教材里也是类似的写法。有的人就直接这样写到了程序里。

但是这个写法是错误的。

错误在于，四个数没有同步的计算。在计算q[1]的时候，q[0]已经被更改了。q[0]是下一时刻的，而q[2]和q[3]是上一时刻的。这样的四元数计算会出现严重的错误。

解决的办法大约有2类。一类方法是先用把计算的结果赋值到临时变量中，等待4个数都计算完了，再把临时变量赋值给四元数。第二种方法，就是利用一些矩阵计算的库，把这段代码写成一个整体的q=R*q矩阵计算，这样就能保证计算结束之后同步更新四元数。

这是一个很低级的错误，但是照着教材公式写代码的时候很容易出问题，我已经见过至少2个人这样写了。这种错误很难自己发现。

建议教材不要把四元数计算公式像这样写为4个分立公式，而是写成一个整体矩阵计算公式，以免误导。