四元素基础理论及其应用

在阅读《Indirect Kalman Filter for 3D Attitude Estimation》一文时，发现文中对四元素的定义采用了非Hamilton方式，阅读有一些冲突，难以转换，至此，决定将该文章转换为Hamilton的表达形式。

1 四元素定义：
$$\mathbf{q}=q_w+q_x\mathbf{i}+q_y\mathbf{j}+q_z\mathbf{k}(1)$$
其中，虚数单位：$\mathbf{i}$，$\mathbf{j}$，$\mathbf{k}$满足如下关系：
$${\mathbf{i}}^2={\mathbf{j}}^2={\mathbf{k}}^2=\mathbf{i}\mathbf{j}\mathbf{k}=-1(2)$$
从而：
$$\mathbf{i}\mathbf{j}=-\mathbf{j}\mathbf{i}=\mathbf{k},\mathbf{j}\mathbf{k}=-\mathbf{k}\mathbf{j}=\mathbf{i},\mathbf{k}\mathbf{i}=-\mathbf{i}\mathbf{k}=\mathbf{j}(3)$$
此外，四元数还可以表达成如下式所示的形式：

常量+向量形式：
$$\mathbf{q}=q_w+{\mathbf{q}}_v(4)$$
其中：${\mathbf{q}}_v=q_x\mathbf{i}+q_y\mathbf{j}+q_z\mathbf{k}$

向量形式
$$\mathbf{q}=\left[\begin{array}{c}{q}_w\\ {\mathbf{q}}_v\end{array}\right]=\left[\begin{array}{c}{q}_w\\ q_x\\ q_y\\ q_z\end{array}\right](5)$$

2 四元素的主要性质
2.1 加法运算
$${\mathbf{q}}_1\pm {\mathbf{q}}_2=\left[\begin{array}{c}{q}_{w1}\\ {\mathbf{q}}_{v1}\end{array}\right]\pm \left[\begin{array}{c}{q}_{w2}\\ {\mathbf{q}}_{v2}\end{array}\right]=\left[\begin{array}{c}{q}_{w1}\pm q_{w2}\\ {\mathbf{q}}_{v1}\pm {\mathbf{q}}_{v2}\end{array}\right](6)$$
加法运算满足交换律和结合律：
$${\mathbf{q}}_1+{\mathbf{q}}_2={\mathbf{q}}_2+{\mathbf{q}}_1(7)$$
$${\mathbf{q}}_1+({\mathbf{q}}_2+{\mathbf{q}}_3)=({\mathbf{q}}_1+{\mathbf{q}}_2)+{\mathbf{q}}_3(8)$$

2.2 四元素数乘
$$k\mathbf{q}=\left[\begin{array}{c}k{q}_w\\ k{\mathbf{q}}_v\end{array}\right]=\left[\begin{array}{c}k{q}_w\\ k{q}_x\\ k{q}_y\\ k{q}_z\end{array}\right](9)$$

2.3 四元素点乘
点乘指的是两个四元素每个对应位置上的数值相乘再求和，即：
$$\mathbf{q}\cdot \mathbf{p}=q_w{p}_w+q_x{p}_x+q_y{p}_y+q_z{p}_z(10)$$

2.4 乘法运算$\otimes$
$$\mathbf{p}\otimes \mathbf{q}=\left[\begin{array}{c}{p}_w{q}_w-p_x{q}_x-p_y{q}_y-p_z{q}_z\\ p_w{q}_x+p_x{q}_w+p_y{q}_z-p_z{q}_y\\ p_w{q}_y-p_x{q}_z+p_y{q}_w+p_z{q}_x\\ p_w{q}_z+p_x{q}_y-p_y{q}_x+p_z{q}_w\end{array}\right]=\left[\begin{array}{c}{p}_w{q}_w-{\mathbf{p}}_v^{\top }{\mathbf{q}}_v\\ p_w{\mathbf{q}}_v+q_w{\mathbf{p}}_v+{\mathbf{p}}_v\times {\mathbf{q}}_v\end{array}\right](11)$$
因（11）式中含有叉乘项，故四元素乘法不满足交换律，即:
$$\mathbf{p}\otimes \mathbf{q}\ne \mathbf{q}\otimes \mathbf{p}(12)$$
例外：当${\mathbf{p}}_v\times {\mathbf{q}}_v=0$时，叉乘项为零时满足交换律，此时，${\mathbf{p}}_v=0$ 或${\mathbf{q}}_v=0$ 或 ${\mathbf{p}}_v||{\mathbf{q}}_v$(即两个向量平行)。

结合律：
$$({\mathbf{q}}_1\otimes {\mathbf{q}}_2)\otimes {\mathbf{q}}_3={\mathbf{q}}_1\otimes ({\mathbf{q}}_2\otimes {\mathbf{q}}_3)(13)$$

分配律:
$${\mathbf{q}}_1\otimes ({\mathbf{q}}_2+{\mathbf{q}}_3)={\mathbf{q}}_1\otimes {\mathbf{q}}_2+{\mathbf{q}}_1\otimes {\mathbf{q}}_3(14)$$
$$({\mathbf{q}}_1+{\mathbf{q}}_2)\otimes {\mathbf{q}}_3={\mathbf{q}}_1\otimes {\mathbf{q}}_3+{\mathbf{q}}_2\otimes {\mathbf{q}}_3(15)$$

矩阵乘法形式:
根据（11）式，提取出相应的四元素向量，便可以得到：
$${\mathbf{q}}_1\otimes {\mathbf{q}}_2=[{\mathbf{q}}_1{]}_L{\mathbf{q}}_2=\left[\begin{array}{cccc}{q}_{w1}& -q_{x1}& -q_{y1}& -q_{z1}\\ q_{x1}& q_{w1}& -q_{z1}& q_{y1}\\ q_{y1}& q_{z1}& q_{w1}& -q_{x1}\\ q_{z1}& -q_{y1}& q_{x1}& q_{w1}\end{array}\right]{\mathbf{q}}_2(16)$$
$${\mathbf{q}}_1\otimes {\mathbf{q}}_2=[q_2{]}_R{\mathbf{q}}_1=\left[\begin{array}{cccc}{q}_{w2}& -q_{x2}& -q_{y2}& -q_{z2}\\ q_{x2}& q_{w2}& q_{z2}& -q_{y2}\\ q_{y2}& -q_{z2}& q_{w2}& q_{x2}\\ q_{z2}& q_{y2}& -q_{x2}& q_{w2}\end{array}\right]{\mathbf{q}}_1(17)$$
根据（16）和（17）可以得到：
$$[\mathbf{q}{]}_L=\left[\begin{array}{cccc}{q}_w& -q_x& -q_y& -q_z\\ q_x& q_w& -q_z& q_y\\ q_y& q_z& q_w& -q_x\\ q_z& -q_y& q_x& q_w\end{array}\right],[\mathbf{q}{]}_R=\left[\begin{array}{cccc}{q}_w& -q_x& -q_y& -q_z\\ q_x& q_w& q_z& -q_y\\ q_y& -q_z& q_w& q_x\\ q_z& q_y& -q_x& q_w\end{array}\right]{\mathbf{q}}_1(18)$$
进一步可以表示为：
$$[\mathbf{q}{]}_L=q_w\mathbf{I}+\left[\begin{array}{cc}0& -{\mathbf{q}}_v^{\top }\\ {\mathbf{q}}_v& {\mathbf{q}}_v^{\wedge }\end{array}\right],[\mathbf{q}{]}_R=q_w\mathbf{I}+\left[\begin{array}{cc}0& -{\mathbf{q}}_v^{\top }\\ {\mathbf{q}}_v& -{\mathbf{q}}_v^{\wedge }\end{array}\right](19)$$
其中：
$${\mathbf{q}}^{\wedge }=\left[\begin{array}{ccc}0& -q_z& q_y\\ q_z& 0& -q_x\\ -q_y& q_x& 0\end{array}\right](20)$$
是反对称矩阵，即${\mathbf{q}}^{\wedge }=-[{\mathbf{q}}^{\wedge }{]}^{\top }$，可用于向量间的叉乘运算：
$$\mathbf{a}\times \mathbf{b}={\mathbf{a}}^{\wedge }\mathbf{b},\forall \mathbf{a},\mathbf{b}\in {\mathbb{R}}^3(21)$$

因为：
$$(\mathbf{q}\otimes \mathbf{x})\otimes \mathbf{p}=[\mathbf{p}{]}_R[\mathbf{q}{]}_L\mathbf{x}and\mathbf{q}\otimes (\mathbf{x}\otimes \mathbf{p})=[\mathbf{q}{]}_L[\mathbf{p}{]}_R\mathbf{x}(22)$$
从而，根据四元素乘法的结合律得：
$$[\mathbf{p}{]}_R[\mathbf{q}{]}_L=[\mathbf{q}{]}_L[\mathbf{p}{]}_R(23)$$

简化写法：
$$[\mathbf{q}{]}_L=[\mathbf{q}\mathbf{\Psi }(\mathbf{q})](24)$$
其中：
$$\mathbf{\Psi }(\mathbf{q})=\left[\begin{array}{c}-{\mathbf{q}}_v^{\top }\\ q_w{\mathbf{I}}_{3\times 3}+{\mathbf{q}}_v^{\wedge }\end{array}\right](25)$$
同理：
$$[\mathbf{q}{]}_R=[\mathbf{q}\mathbf{\Xi }(\mathbf{q})](26)$$
其中：
$$\mathbf{\Xi }(\mathbf{q})=\left[\begin{array}{c}-{\mathbf{q}}_v^{\top }\\ q_w{\mathbf{I}}_{3\times 3}-{\mathbf{q}}_v^{\wedge }\end{array}\right](27)$$

此外，对于unit quaternion $\mathbf{q}$有：
$$[{\mathbf{q}}^{-1}{]}_L=[\mathbf{q}{]}_L^{\top }(28)$$
$$[{\mathbf{q}}^{-1}{]}_R=[\mathbf{q}{]}_R^{\top }(29)$$

2.5 Identity quaternion
$${\mathbf{q}}_1=1=\left[\begin{array}{c}1\\ 0_v\end{array}\right](30)$$
满足：
$${\mathbf{q}}_1\otimes \mathbf{q}=\mathbf{q}\otimes {\mathbf{q}}_1=\mathbf{q}(31)$$

2.6 共轭
四元素的共轭为：
$${\mathbf{q}}^{\ast }=\left[\begin{array}{c}{q}_w\\ -{\mathbf{q}}_v\end{array}\right](32)$$
共轭四元素的性质:
$$\mathbf{q}\otimes {\mathbf{q}}^{\ast }={\mathbf{q}}^{\ast }\otimes \mathbf{q}=\left[\begin{array}{c}{q}_w^2+q_x^2+q_y^2+q_z^2\\ 0_v\end{array}\right](32)$$

以及：
$$(\mathbf{p}\otimes \mathbf{q}{)}^{\ast }={\mathbf{q}}^{\ast }\otimes {\mathbf{p}}^{\ast }(33)$$

2.7 四元素的模
定义:
$$||\mathbf{q}||=\sqrt{\mathbf{q}\otimes {\mathbf{q}}^{\ast }}=\sqrt{{\mathbf{q}}^{\ast }\otimes \mathbf{q}}=\sqrt{q_w^2+q_x^2+q_y^2+q_z^2}(34)$$

模的性质:
$$||\mathbf{p}\otimes \mathbf{q}||=||\mathbf{q}\otimes \mathbf{p}||=||\mathbf{p}||||\mathbf{q}||(35)$$
简单推导：
$$||\mathbf{p}\otimes \mathbf{q}|{|}^2=(\mathbf{p}\otimes \mathbf{q})\otimes (\mathbf{p}\otimes \mathbf{q}{)}^{\ast }=\mathbf{p}\otimes \mathbf{q}\otimes {\mathbf{q}}^{\ast }\otimes {\mathbf{p}}^{\ast }=\mathbf{p}\otimes (\mathbf{q}\otimes {\mathbf{q}}^{\ast })\otimes {\mathbf{p}}^{\ast }=\mathbf{p}\otimes \left[\begin{array}{c}||\mathbf{q}|{|}^2\\ 0_v\end{array}\right]\otimes {\mathbf{p}}^{\ast }=\mathbf{p}\otimes {\mathbf{p}}^{\ast }\otimes \left[\begin{array}{c}||\mathbf{q}|{|}^2\\ 0_v\end{array}\right]=(||\mathbf{p}||||\mathbf{q}||{)}^2$$

2.8 四元素的逆
四元素的逆记为${\mathbf{q}}^{-1}$,，其满足如下条件：
$$\mathbf{q}\otimes {\mathbf{q}}^{-1}={\mathbf{q}}^{-1}\otimes \mathbf{q}={\mathbf{q}}_1(36)$$
其中${\mathbf{q}}_1$为"Identity quaternion“， 逆通过下式计算：
$${\mathbf{q}}^{-1}={\mathbf{q}}^{\ast }/||\mathbf{q}|{|}^2(37)$$

2.9 Unit or normalized quaternion
对于unit quaternion有，$||\mathbf{q}||=1$，所以：
$${\mathbf{q}}^{-1}={\mathbf{q}}^{\ast }(38)$$

3 旋转四元素
形式：
$$\mathbf{q}=\left[\begin{array}{c}\cos (\theta /2)\\ \mathbf{u}\sin (\theta /2)\end{array}\right]=\left[\begin{array}{c}\cos (\theta /2)\\ u_x\sin (\theta /2)\\ u_y\sin (\theta /2)\\ u_z\sin (\theta /2)\end{array}\right](39)$$
其中，旋转轴为$\mathbf{u}$，为单位向量，旋转角度为$\theta$，将向量$\mathbf{x}$绕$\mathbf{u}$轴旋转$\theta$即可表示为:
$${\mathbf{x}}^{\prime }=\mathbf{q}\otimes \mathbf{x}\otimes {\mathbf{q}}^{\ast }(40)$$
可以验证，旋转四元素为unit quaternion，跟unit quaternion一样 具有如下性质：
$$(\mathbf{p}\otimes \mathbf{q}{)}^{-1}={\mathbf{q}}^{-1}\otimes {\mathbf{p}}^{-1}(41)$$

在介绍四元素基本的定义及相关性质之后，将对《Indirect Kalman Filter for 3D Attitude Estimation》一文当中的相关内容进行转义。

》1.3 Userful Identites
》1.3.1 叉乘反对称矩阵的相关属性
Anti-Commutativity
$${\mathbf{w}}^{\wedge }=-[{\mathbf{w}}^{\wedge }{]}^{\top }(42)$$
$${\mathbf{a}}^{\wedge }\mathbf{b}=-{\mathbf{b}}^{\wedge }\mathbf{a}\iff -{\mathbf{b}}^{\top }{\mathbf{a}}^{\wedge }={\mathbf{a}}^{\top }{\mathbf{b}}^{\wedge }(43)$$

Distributivity orver Addition
$${\mathbf{a}}^{\wedge }+{\mathbf{b}}^{\wedge }=(\mathbf{a}+\mathbf{b}{)}^{\wedge }(44)$$

数乘
$$c\cdot {\mathbf{w}}^{\wedge }=(c\mathbf{w}{)}^{\wedge }(45)$$

平行向量的叉乘
$$\mathbf{w}\times (c\mathbf{w})=c\cdot {\mathbf{w}}^{\wedge }\mathbf{w}=-c\cdot ({\mathbf{w}}^{\top }{\mathbf{w}}^{\wedge }{)}^{\top }=0_{3\times 1}(46)$$

拉格朗日公式
$${\mathbf{a}}^{\wedge }{\mathbf{b}}^{\wedge }=\mathbf{b}{\mathbf{a}}^{\top }-({\mathbf{a}}^{\top }\mathbf{b}){\mathbf{I}}_{3\times 3}(47)$$
$$\iff \mathbf{a}\times (\mathbf{b}\times \mathbf{c})=\mathbf{b}({\mathbf{a}}^{\top }\mathbf{c})-\mathbf{c}({\mathbf{a}}^{\top }\mathbf{b})(48)$$

$$(\mathbf{a}\times \mathbf{b}{)}^{\wedge }=\mathbf{b}{\mathbf{a}}^{\top }-\mathbf{a}{\mathbf{b}}^{\top }(49)$$

Jacobi Identity
$$\mathbf{a}\times (\mathbf{b}\times \mathbf{c})+\mathbf{b}\times (\mathbf{c}\times \mathbf{a})+\mathbf{c}\times (\mathbf{a}\times \mathbf{b})=0(49)$$
\qquad or
$${\mathbf{a}}^{\wedge }{\mathbf{b}}^{\wedge }\mathbf{c}+{\mathbf{b}}^{\wedge }{\mathbf{c}}^{\wedge }\mathbf{a}+{\mathbf{c}}^{\wedge }{\mathbf{a}}^{\wedge }\mathbf{b}=0(50)$$

Rotations
$$\begin{gathered} (\mathbf{R}\mathbf{a}{)}^{\wedge }=\mathbf{R}{\mathbf{a}}^{\wedge }{\mathbf{R}}^{\top }(51) \\ proof.(\mathbf{R}\mathbf{a})\times \mathbf{b}=(\mathbf{R}\mathbf{a})\times (\mathbf{R}{\mathbf{R}}^{\top }\mathbf{b})=\mathbf{R}[\mathbf{a}\times ({\mathbf{R}}^{\top }\mathbf{b})]=\mathbf{R}{\mathbf{a}}^{\wedge }{\mathbf{R}}^{\top }\mathbf{b} \\ \Rightarrow (\mathbf{R}\mathbf{a}{)}^{\wedge }={\mathbf{R}\mathbf{a}}^{\wedge }{\mathbf{R}}^{\top } \end{gathered}$$
$$\mathbf{R}(\mathbf{a}\times \mathbf{b})=(\mathbf{R}\mathbf{a})\times (\mathbf{R}\mathbf{b})(52)$$

叉乘反对称矩阵
$$({\mathbf{w}}^{\wedge }{)}^2={\mathbf{w}\mathbf{w}}^{\top }-|\mathbf{w}{|}^2\mathbf{I}(53)$$

$$\begin{array}{rl}({\mathbf{w}}^{\wedge }{)}^3& =({\mathbf{w}\mathbf{w}}^{\top }-|\mathbf{w}{|}^2\mathbf{I}){\mathbf{w}}^{\wedge }\\ & =\mathbf{w}{\mathbf{w}}^{\top }{\mathbf{w}}^{\wedge }-|\mathbf{w}{|}^2{\mathbf{w}}^{\wedge }\\ & =\mathbf{w}(-{\mathbf{w}}^{\wedge }\mathbf{w}{)}^{\top }-|\mathbf{w}{|}^2{\mathbf{w}}^{\wedge }\\ & =-|\mathbf{w}{|}^2{\mathbf{w}}^{\wedge }\end{array}(54)$$

$$\begin{array}{rl}({\mathbf{w}}^{\wedge }{)}^4& =({\mathbf{w}}^{\wedge }{)}^3{\mathbf{w}}^{\wedge }\\ & =-|\mathbf{w}{|}^2({\mathbf{w}}^{\wedge }{)}^2\end{array}(55)$$

$$\begin{array}{rl}({\mathbf{w}}^{\wedge }{)}^5& =({\mathbf{w}}^{\wedge }{)}^4{\mathbf{w}}^{\wedge }\\ & =-|\mathbf{w}{|}^2({\mathbf{w}}^{\wedge }{)}^3\\ & =-|\mathbf{w}{|}^2(-|\mathbf{w}{|}^2{\mathbf{w}}^{\wedge })\\ & =|\mathbf{w}{|}^4{\mathbf{w}}^{\wedge }(56)\end{array}$$

$$\begin{array}{rl}({\mathbf{w}}^{\wedge }{)}^6& =({\mathbf{w}}^{\wedge }{)}^5{\mathbf{w}}^{\wedge }\\ & =|\mathbf{w}{|}^4({\mathbf{w}}^{\wedge }{)}^2(57)\end{array}$$

$$\begin{array}{rl}({\mathbf{w}}^{\wedge }{)}^7& =({\mathbf{w}}^{\wedge }{)}^6{\mathbf{w}}^{\wedge }\\ & =|\mathbf{w}{|}^4({\mathbf{w}}^{\wedge }{)}^3\\ & =|\mathbf{w}{|}^4(-|\mathbf{w}{|}^2{\mathbf{w}}^{\wedge })\\ & =-|\mathbf{w}{|}^6{\mathbf{w}}^{\wedge }(58)\\ & ......\end{array}$$

》1.3.2 Properties of the matrix $\mathbf{\Omega }$
$\mathbf{\Omega }$矩阵出现在一个向量与四元素的乘积当中，可用于四元素的求导，它具有以下性质：
$$\begin{array}{rl}\mathbf{\Omega }(\mathbf{w})& =\left[\begin{array}{cccc}0& -w_x& -w_y& -w_z\\ w_x& 0& w_z& -w_y\\ w_y& -w_z& 0& w_x\\ w_z& w_y& -w_x& 0\end{array}\right]\\ & =\left[\begin{array}{cc}0& -{\mathbf{w}}^{\top }\\ \mathbf{w}& -{\mathbf{w}}^{\wedge }\end{array}\right]\end{array}(59)$$

$$\begin{array}{rl}\mathbf{\Omega }(\mathbf{w}{)}^2& =\left[\begin{array}{cc}-{\mathbf{w}}^{\top }\mathbf{w}& {\mathbf{w}}^{\top }{\mathbf{w}}^{\wedge }\\ -{\mathbf{w}}^{\wedge }\mathbf{w}& -{\mathbf{w}\mathbf{w}}^{\top }+{\mathbf{w}}^{\wedge }{\mathbf{w}}^{\wedge }\end{array}\right]\\ & =\left[\begin{array}{cc}-||\mathbf{w}|{|}^2& 0_{1\times 3}\\ 0_{3\times 1}& -||\mathbf{w}|{|}^2{\mathbf{I}}_{3\times 3}\end{array}\right]\\ & =-||\mathbf{w}|{|}^2{\mathbf{I}}_{4\times 4}(60)\end{array}$$

$$\mathbf{\Omega }(\mathbf{w}{)}^3=-||\mathbf{w}|{|}^2\mathbf{\Omega }(\mathbf{w})(61)$$

$$\mathbf{\Omega }(\mathbf{w}{)}^4=||\mathbf{w}|{|}^4{\mathbf{I}}_{4\times 4}(62)$$

$$\mathbf{\Omega }(\mathbf{w}{)}^5=||\mathbf{w}|{|}^4\mathbf{\Omega }(\mathbf{w})(63)$$

$$\mathbf{\Omega }(\mathbf{w}{)}^6=-||\mathbf{w}|{|}^6{\mathbf{I}}_{4\times 4}(64)$$

1.3.3 Properties of the matrix $\mathbf{\Xi }$

$$\mathbf{\Psi }(\mathbf{q})=\left[\begin{array}{c}-{\mathbf{q}}_v^{\top }\\ q_w{\mathbf{I}}_{3\times 3}+{\mathbf{q}}_v^{\wedge }\end{array}\right],{\mathbf{\Psi }}^{\top }(\mathbf{q})=[-{\mathbf{q}}_v{q}_w{\mathbf{I}}_{3\times 3}-{\mathbf{q}}_v^{\wedge }](65)$$

$$\begin{array}{rl}\Rightarrow {\mathbf{\Psi }}^{\top }(\mathbf{q})\mathbf{\Psi }(\mathbf{q})& =||\mathbf{q}|{|}^2{\mathbf{I}}_{3\times 3}(66)\\ \mathbf{\Psi }(\mathbf{q}){\mathbf{\Psi }}^{\top }(\mathbf{q})& =||\mathbf{q}||{\mathbf{I}}_{4\times 4}-\mathbf{q}{\mathbf{q}}^{\top }(67)\\ {\mathbf{\Psi }}^{\top }(\mathbf{q})\mathbf{q}& =0_{3\times 1}(68)\end{array}$$

$\mathbf{\Xi }$与$\mathbf{\Omega }$之间的关系为：
$$\mathbf{\Omega }(\mathbf{w})\mathbf{q}=\mathbf{\Psi }(\mathbf{q})\mathbf{w}(69)$$

》1.4 四元素与旋转矩阵之间的关系
给定旋转四元素$\mathbf{q}=\cos (\theta /2)+\mathbf{u}\sin (\theta /2)$，对应的旋转矩阵记为$\mathbf{R}(\mathbf{q})$，则
$$\mathbf{R}(\mathbf{q})=(2q_w^2-1)\mathbf{I}+2{\mathbf{q}}_v{\mathbf{q}}_v^{\top }+2q_w{\mathbf{q}}_v^{\wedge }(70)$$
其中：
$$q_w=\cos (\theta /2),{\mathbf{q}}_v=\mathbf{u}\sin (\theta /2)$$
或者表示为：
$$\mathbf{R}(\mathbf{q})={\mathbf{\Xi }}^{\top }(\mathbf{q})\mathbf{\Psi }(\mathbf{q})(71)$$

或者表示为：
$$\mathbf{R}(\mathbf{q})=\left[\begin{array}{ccc}1-2q_y^2-2q_z^2& 2q_x{q}_y-2q_w{q}_z& 2q_x{q}_z+2q_w{q}_y\\ 2q_x{q}_y+2q_w{q}_z& 1-2q_x^2-2q_z^2& 2q_y{q}_z-2q_w{q}_x\\ 2q_x{q}_z-2q_w{q}_y& 2q_y{q}_z+2q_w{q}_x& 1-2q_x^2-2q_y^2\end{array}\right](72)$$

旋转矩阵的乘法与旋转四元素的乘法之间的关系：
$$\mathbf{R}({}^0{\mathbf{q}}_1)\mathbf{R}{(}^1{\mathbf{q}}_2)=\mathbf{R}{(}^0{\mathbf{q}}_1\otimes {}^1{\mathbf{q}}_2)(73)$$
指数映射
$$\mathbf{R}(\mathbf{q})=exp(\theta {\mathbf{u}}^{\wedge })(74)$$
》1.5 四元素对时间的导数

待续。。。。。。

参考文献:
[1] Indirect Kalman Filter for 3D Attitude Estimation
[2] Quaternion kinematics for the error-state Kalman filter
[3] 视觉SLAM十四讲 从理论到实践