四元数变化率微分方程推导

三元欧拉角存在万向锁问题，不适用于所有情况下确定pose。因此利用四元数四个变量来代替$\delta \theta$的三个分量，可以在所有环境下确定姿态。
下面推导绕三个正交轴的角速度函数的四元数变化率微分方程。基于此能以t时刻的四元数计算t+$\delta$t时刻的四元数值。假定$\cos {\alpha }_r,\cos {\alpha }_p,\cos {\alpha }_y$为相对于roll轴，pitch轴，yaw轴的实际方向余弦，经过小角度旋转$\delta {\theta }_b$，对应四元数$q^{\prime }$如下：
$$\begin{array}{rl}{q}_1^{\prime }& =\cos {\alpha }_r\sin (\delta {\theta }_b/2)\\ q_2^{\prime }& =\cos {\alpha }_p\sin (\delta {\theta }_b/2)\\ q_3^{\prime }& =\cos {\alpha }_y\sin (\delta {\theta }_b/2)\\ q_4^{\prime }& =\cos (\delta {\theta }_b/2)\end{array}$$
其中旋转轴$n=(\cos {\alpha }_r,\cos {\alpha }_p,\cos {\alpha }_y)$，旋转角为$\delta {\theta }_b$

且该四元数为单位四元数。
$$q(t+\delta t)=q^{\prime }q(t)$$
将四元数$q^{\prime }$记为$q_1^{\prime }\hat{i}+q_2^{\prime }\hat{j}+q_3^{\prime }\hat{k}+q_4^{\prime }$，四元数q(t)记为$q_1\hat{i}+q_2\hat{j}+q_3\hat{k}+q_4$，二者相乘结果如下：
$$\begin{array}{rl}q(t+\delta t)=& (q_1^{\prime }\hat{i}+q_2^{\prime }\hat{j}+q_3^{\prime }\hat{k}+q_4^{\prime })(q_1\hat{i}+q_2\hat{j}+q_3\hat{k}+q_4)\\ =& (q_4^{\prime }{q}_1+q_3^{\prime }{q}_2-q_2^{\prime }{q}_3+q_1^{\prime }{q}_4)\hat{i}+\\ & (-q_3^{\prime }{q}_1+q_4^{\prime }{q}_2+q_1^{\prime }{q}_3+q_2^{\prime }{q}_4)\hat{j}+\\ & (q_2^{\prime }{q}_1-q_1^{\prime }{q}_2+q_4^{\prime }{q}_3+q_3^{\prime }{q}_4)\hat{k}+\\ & (-q_1^{\prime }{q}_1-q_2^{\prime }{q}_2-q_3^{\prime }{q}_3+q_4^{\prime }{q}_4)\end{array}$$
将上式由矩阵表示可以得到：
$$\begin{array}{r}q(t+\delta t)=\left[\begin{array}{cccc}{q}_4^{\prime }& q_3^{\prime }& -q_2^{\prime }& q_1^{\prime }\\ -q_3^{\prime }& q_4^{\prime }& q_1^{\prime }& q_2^{\prime }\\ q_2^{\prime }& -q_1^{\prime }& q_4^{\prime }& q_3^{\prime }\\ -q_1^{\prime }& -q_2^{\prime }& -q_3^{\prime }& q_4^{\prime }\end{array}\right]\left[\begin{array}{c}{q}_1\\ q_2\\ q_3\\ q_4\end{array}\right]\end{array}$$

如果${\bar{\omega }}_b$是很小的时间增量$\delta t$内平均实际角速度，则分量$\delta {\theta }_{bt}={\bar{\omega }}_b\delta t$且有：
$$\begin{array}{rl}\sin (\delta {\theta }_b/2)& \approx \frac{1}{2}{\bar{\omega }}_b\delta t\\ \cos (\delta {\theta }_b/2)& \approx 1\end{array}$$

由此，
$$\begin{array}{rl}{q}_1^{\prime }& \approx \cos {\alpha }_r\frac{1}{2}{\bar{\omega }}_b\delta t\\ q_2^{\prime }& \approx \cos {\alpha }_p\frac{1}{2}{\bar{\omega }}_b\delta t\\ q_3^{\prime }& \approx \cos {\alpha }_y\frac{1}{2}{\bar{\omega }}_b\delta t\\ q_4^{\prime }& \approx 1\end{array}$$

进而，
$$q(t+\delta t)\approx (\mathbf{I}+\frac{1}{2}{\bar{\Omega }}_q\delta t)q(t)$$

$$\begin{array}{r}{\bar{\Omega }}_q=\left[\begin{array}{cccc}0& {\bar{\omega }}_{yb}& -{\bar{\omega }}_{pb}& {\bar{\omega }}_{rb}\\ -{\bar{\omega }}_{yb}& 0& {\bar{\omega }}_{rb}& {\bar{\omega }}_{pb}\\ {\bar{\omega }}_{pb}& -{\bar{\omega }}_{rb}& 0& {\bar{\omega }}_{yb}\\ -{\bar{\omega }}_{rb}& -{\bar{\omega }}_{pb}& -{\bar{\omega }}_{yb}& 0\end{array}\right]\end{array}$$

其中 ${\bar{\omega }}_b=[{\bar{\omega }}_{rb},{\bar{\omega }}_{pb},{\bar{\omega }}_{yb}{]}^T$
$$\begin{array}{rl}{\bar{\omega }}_{rb}& =\cos {\alpha }_r{\bar{\omega }}_b\\ {\bar{\omega }}_{pb}& =\cos {\alpha }_p{\bar{\omega }}_b\\ {\bar{\omega }}_{yb}& =\cos {\alpha }_y{\bar{\omega }}_b\end{array}$$

进而，利用如下求导公式：
$$\frac{dq}{dt}=\underset{\delta t\to 0}{\lim }[\frac{q(t+\delta t)-q(t)}{\delta t}]$$

由此，
$$\frac{dq}{dt}=\frac{1}{2}{\bar{\Omega }}_{q}q(t)$$

将${\bar{\Omega }}_q$利用${\bar{\omega }}_{rb},{\bar{\omega }}_{pb},{\bar{\omega }}_{yb}$表示:

$${\bar{\Omega }}_q=\left[\begin{array}{cc}-{\lfloor {\bar{\omega }}_b\rfloor }_{\times }& {\bar{\omega }}_b\\ -{\bar{\omega }}_b^T& 0\end{array}\right]$$

$${\lfloor {\bar{\omega }}_b\rfloor }_{\times }=\left[\begin{array}{ccc}0& -{\bar{\omega }}_{yb}& {\bar{\omega }}_{pb}\\ {\bar{\omega }}_{yb}& 0& -{\bar{\omega }}_{rb}\\ -{\bar{\omega }}_{pb}& {\bar{\omega }}_{rb}& 0\end{array}\right]$$

$${\bar{\omega }}_b=\left[\begin{array}{c}{\bar{\omega }}_{rb}\\ {\bar{\omega }}_{pb}\\ {\bar{\omega }}_{yb}\end{array}\right]$$

将${\bar{\Omega }}_q$记为$\Omega (\omega )$, 将${\bar{\omega }}_{rb},{\bar{\omega }}_{pb},{\bar{\omega }}_{yb}$分别记为${\omega }_x,{\omega }_y,{\omega }_z$，则有:

$$\Omega (\omega )=\left[\begin{array}{cc}-{\lfloor \omega \rfloor }_{\times }& \omega \\ -{\omega }^T& 0\end{array}\right]$$

$${\lfloor \omega \rfloor }_{\times }=\left[\begin{array}{ccc}0& -{\omega }_z& {\omega }_y\\ {\omega }_z& 0& -{\omega }_x\\ -{\omega }_y& {\omega }_x& 0\end{array}\right]$$

$$\omega =\left[\begin{array}{c}{\omega }_x\\ {\omega }_y\\ {\omega }_z\end{array}\right]$$
带入此前四元数求导公式则有：
$$\dot{q}(t)=\frac{1}{2}\Omega (\omega (t))q(t)$$
上述结论与VINS-Mono : A Robust and Versatile Monocular Visual-Inertial State Estimator文章中的公式（3）和（4）一致。
也与MSCKF1.0中用到的公式（6）四元数的导数一致。

证毕。