用于误差状态卡尔曼滤波的四元数运动学

琼·索拉

摘要

本文全面修订了与四元数及三维空间旋转相关的概念和公式，并详述了它们在误差状态卡尔曼滤波器等估计引擎中的正确使用方法。

该论文深入研究了旋转群及其李群结构，同时采用四元数和旋转矩阵两种表述形式。特别关注了旋转扰动、导数与积分的定义，并提供了大量直观解释和几何诠释，以帮助读者理解三维旋转的内在机制。

本材料旨在设计适用于实际应用的误差状态卡尔曼滤波器的精确公式，通过整合来自惯性测量单元（IMU）的信号实现。

1 四元数的定义与性质

1.1 四元数的定义

关于四元数的一个特别吸引我的介绍方式是通过凯莱-迪克森构造给出的：若有两个复数 $A=a+bi$ 和 $C=c+di$ ，通过构造 $Q=A+Cj$ 并定义 $k\triangleq ij$ ，便可得到一个四元数空间 ${\mathbb{H}}$ 中的数，

\displaystyle Q=a+bi+cj+dk\in{\mathbb{H}}\leavevmode\nobreak\ ,

(1)

其中 $\{a,b,c,d\}\in{\mathbb{R}}$ 和 $\{i,j,k\}$ 是三个虚数单位，其定义满足


	$\displaystyle i^{2}=j^{2}=k^{2}=ijk=-1\leavevmode\nobreak\ ,$	(2a)
from which we can derive

	$\displaystyle ij=-ji=k\leavevmode\nobreak\ ,\quad jk=-kj=i\leavevmode\nobreak\ ,\quad ki=-ik=j\leavevmode\nobreak\ .$	(2b)

由(1)式可见，我们可以在四元数定义中嵌入复数，进而嵌入实数和纯虚数，这意味着实数、虚数和复数本质上都是四元数，

\displaystyle Q=a\in{\mathbb{R}}\subset{\mathbb{H}}\leavevmode\nobreak\ ,\qquad\qquad Q=bi\in{\mathbb{I}}\subset{\mathbb{H}}\leavevmode\nobreak\ ,\qquad\qquad Q=a+bi\in{\mathbb{Z}}\subset{\mathbb{H}}\leavevmode\nobreak\ .

(3)

同样地，为了完备性起见，我们可以在 ${\mathbb{H}}$ 的三维虚子空间中定义数。我们将其称为纯四元数 ，并可用 ${\mathbb{H}}_{p}=\operatorname{Im}({\mathbb{H}})$ 表示纯四元数空间，

\displaystyle Q=bi+cj+dk\in{\mathbb{H}}_{p}\subset{\mathbb{H}}\leavevmode\nobreak\ .

(4)

值得注意的是，虽然单位长度的常规复数 ${\bf z}=e^{i\theta}$ 可通过单一复数乘法 ${\bf x}^{\prime}={\bf z}\!\cdot\!{\bf x}$ 编码二维平面中的旋转，但"扩展复数"或单位长度的四元数 ${\bf q}=e^{(u_{x}i+u_{y}j+u_{z}k)\theta/2}$ 则能通过双四元数乘法 ${\bf x}^{\prime}={\bf q}\otimes{\bf x}\otimes{\bf q}^{*}$ （如本文后续所述）编码三维空间中的旋转。

注意：并非所有四元数定义都相同。部分作者将乘积写作 $ib$ 而非 $bi$ ，因此他们得到性质 $k=ji=-ij$ ，这导致 $ijk=1$ 以及左手系四元数。此外，许多作者将实部置于末位，形成 $Q=ia+jb+kc+d$ 。这些选择并无本质区别，但会使整个公式体系在细节上呈现差异。更多解释与歧义消除请参阅第 3 节。

注意：还存在其他使公式细节产生差异的约定。这些约定涉及我们对旋转算子"含义"或"解释"的理解——无论是旋转矢量还是旋转参考系（本质上构成互逆操作）。更多解释与歧义消除请同样参阅第 3 节。

注：在上述不同约定中，本文聚焦于哈密顿约定，其最显著特性体现在定义式(2)中。要建立严谨的术语区分体系，需先完成大量基础理论构建；因此相关辨析内容将延后至前文第 3 节详述。

1.1.1 四元数的替代表示法

实数+虚数的表示法 $\{1,i,j,k\}$ 并不总是适合我们的需求。在运用代数(2)的前提下，四元数可表示为标量+向量的组合，

\displaystyle Q=q_{w}+q_{x}i+q_{y}j+q_{z}k\qquad\Leftrightarrow\qquad Q=q_{w}+{\bf q}_{v}\leavevmode\nobreak\ ,

(5)

其中 $q_{w}$ 被称为实部或标量部 ， ${\bf q}_{v}=q_{x}i+q_{y}j+q_{z}k=(q_{x},q_{y},q_{z})$ 则称为虚部或向量部 。 ¹¹1Our choice for the $(w,x,y,z)$ subscripts notation comes from the fact that we are interested in the geometric properties of the quaternion in the 3D Cartesian space. Other texts often use alternative subscripts such as $(0,1,2,3)$ or $(1,i,j,k)$ , perhaps better suited for mathematical interpretations. 亦可将其定义为标量-向量的有序对

\displaystyle Q=\langle q_{w},{\bf q}_{v}\rangle\leavevmode\nobreak\ .

(6)

我们通常将四元数 $Q$ 表示为四维向量 ${\bf q}$ ，

\displaystyle{\bf q}\triangleq\begin{bmatrix}q_{w}\\ {\bf q}_{v}\end{bmatrix}=\begin{bmatrix}q_{w}\\ q_{x}\\ q_{y}\\ q_{z}\end{bmatrix}\leavevmode\nobreak\ ,

(7)

这使得我们能够使用矩阵代数来处理涉及四元数的运算。在某些情况下，我们可以通过滥用“ $=$ ”符号来混合使用不同的表示法。典型的例子包括实四元数和纯四元数 ，

\displaystyle\textrm{general: }{\bf q}=q_{w}+{\bf q}_{v}=\begin{bmatrix}q_{w}\\ {\bf q}_{v}\end{bmatrix}\in{\mathbb{H}}\leavevmode\nobreak\ ,\quad\textrm{real: }q_{w}=\begin{bmatrix}q_{w}\\ {\bf 0}_{v}\end{bmatrix}\in{\mathbb{R}}\leavevmode\nobreak\ ,\quad\textrm{pure: }{\bf q}_{v}=\begin{bmatrix}0\\ {\bf q}_{v}\end{bmatrix}\in{\mathbb{H}}_{p}\leavevmode\nobreak\ .

(8)

1.2 四元数主要性质

1.2.1 加法

该求和过程直接明了，

\displaystyle{\bf p}\mathbin{\leavevmode\hbox to6.46pt{\vbox to6.46pt{\pgfpicture\makeatletter\hbox{\hskip 0.21527pt\lower-0.21527pt\hbox to0.0pt{\pgfsys@beginscope\pgfsys@invoke{ }\definecolor{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@rgb@stroke{0}{0}{0}\pgfsys@invoke{ }\pgfsys@color@rgb@fill{0}{0}{0}\pgfsys@invoke{ }\pgfsys@setlinewidth{0.4pt}\pgfsys@invoke{ }\nullfont\hbox to0.0pt{\pgfsys@beginscope\pgfsys@invoke{ }{}{}\pgfsys@setlinewidth{0.43056pt}\pgfsys@invoke{ }{}{{}}{} {}{}{}{{}}{} {}{}{}{{}}{} {}{}{}\pgfsys@moveto{0.0pt}{0.0pt}\pgfsys@lineto{6.02776pt}{0.0pt}\pgfsys@moveto{3.01387pt}{0.6028pt}\pgfsys@lineto{3.01387pt}{6.02776pt}\pgfsys@moveto{0.0pt}{3.31528pt}\pgfsys@lineto{6.02776pt}{3.31528pt}\pgfsys@stroke\pgfsys@invoke{ } \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope{}{}{}\hss}\pgfsys@discardpath\pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope\hss}}\lxSVG@closescope\endpgfpicture}}}{\bf q}=\begin{bmatrix}p_{w}\\ {\bf p}_{v}\end{bmatrix}\mathbin{\leavevmode\hbox to6.46pt{\vbox to6.46pt{\pgfpicture\makeatletter\hbox{\hskip 0.21527pt\lower-0.21527pt\hbox to0.0pt{\pgfsys@beginscope\pgfsys@invoke{ }\definecolor{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@rgb@stroke{0}{0}{0}\pgfsys@invoke{ }\pgfsys@color@rgb@fill{0}{0}{0}\pgfsys@invoke{ }\pgfsys@setlinewidth{0.4pt}\pgfsys@invoke{ }\nullfont\hbox to0.0pt{\pgfsys@beginscope\pgfsys@invoke{ }{}{}\pgfsys@setlinewidth{0.43056pt}\pgfsys@invoke{ }{}{{}}{} {}{}{}{{}}{} {}{}{}{{}}{} {}{}{}\pgfsys@moveto{0.0pt}{0.0pt}\pgfsys@lineto{6.02776pt}{0.0pt}\pgfsys@moveto{3.01387pt}{0.6028pt}\pgfsys@lineto{3.01387pt}{6.02776pt}\pgfsys@moveto{0.0pt}{3.31528pt}\pgfsys@lineto{6.02776pt}{3.31528pt}\pgfsys@stroke\pgfsys@invoke{ } \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope{}{}{}\hss}\pgfsys@discardpath\pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope\hss}}\lxSVG@closescope\endpgfpicture}}}\begin{bmatrix}q_{w}\\ {\bf q}_{v}\end{bmatrix}=\begin{bmatrix}p_{w}\mathbin{\leavevmode\hbox to6.46pt{\vbox to6.46pt{\pgfpicture\makeatletter\hbox{\hskip 0.21527pt\lower-0.21527pt\hbox to0.0pt{\pgfsys@beginscope\pgfsys@invoke{ }\definecolor{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@rgb@stroke{0}{0}{0}\pgfsys@invoke{ }\pgfsys@color@rgb@fill{0}{0}{0}\pgfsys@invoke{ }\pgfsys@setlinewidth{0.4pt}\pgfsys@invoke{ }\nullfont\hbox to0.0pt{\pgfsys@beginscope\pgfsys@invoke{ }{}{}\pgfsys@setlinewidth{0.43056pt}\pgfsys@invoke{ }{}{{}}{} {}{}{}{{}}{} {}{}{}{{}}{} {}{}{}\pgfsys@moveto{0.0pt}{0.0pt}\pgfsys@lineto{6.02776pt}{0.0pt}\pgfsys@moveto{3.01387pt}{0.6028pt}\pgfsys@lineto{3.01387pt}{6.02776pt}\pgfsys@moveto{0.0pt}{3.31528pt}\pgfsys@lineto{6.02776pt}{3.31528pt}\pgfsys@stroke\pgfsys@invoke{ } \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope{}{}{}\hss}\pgfsys@discardpath\pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope\hss}}\lxSVG@closescope\endpgfpicture}}}q_{w}\\ {\bf p}_{v}\mathbin{\leavevmode\hbox to6.46pt{\vbox to6.46pt{\pgfpicture\makeatletter\hbox{\hskip 0.21527pt\lower-0.21527pt\hbox to0.0pt{\pgfsys@beginscope\pgfsys@invoke{ }\definecolor{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@rgb@stroke{0}{0}{0}\pgfsys@invoke{ }\pgfsys@color@rgb@fill{0}{0}{0}\pgfsys@invoke{ }\pgfsys@setlinewidth{0.4pt}\pgfsys@invoke{ }\nullfont\hbox to0.0pt{\pgfsys@beginscope\pgfsys@invoke{ }{}{}\pgfsys@setlinewidth{0.43056pt}\pgfsys@invoke{ }{}{{}}{} {}{}{}{{}}{} {}{}{}{{}}{} {}{}{}\pgfsys@moveto{0.0pt}{0.0pt}\pgfsys@lineto{6.02776pt}{0.0pt}\pgfsys@moveto{3.01387pt}{0.6028pt}\pgfsys@lineto{3.01387pt}{6.02776pt}\pgfsys@moveto{0.0pt}{3.31528pt}\pgfsys@lineto{6.02776pt}{3.31528pt}\pgfsys@stroke\pgfsys@invoke{ } \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope{}{}{}\hss}\pgfsys@discardpath\pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope\hss}}\lxSVG@closescope\endpgfpicture}}}{\bf q}_{v}\end{bmatrix}\leavevmode\nobreak\ .

(9)

根据构造，该和具有交换律与结合律性质，

	$\displaystyle{\bf p}+{\bf q}$	$\displaystyle={\bf q}+{\bf p}$		(10)
	$\displaystyle{\bf p}+({\bf q}+{\bf r})$	$\displaystyle=({\bf p}+{\bf q})+{\bf r}\leavevmode\nobreak\ .$		(11)

1.2.2 四元数乘积

记作 $\otimes$ 的四元数乘积运算需使用原始形式(1)及四元数代数规则(2)，将其写成向量形式可得

\displaystyle{\bf p}\otimes{\bf q}=\begin{bmatrix}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{bmatrix}\leavevmode\nobreak\ .

(12)

该运算亦可表述为标量部分与向量部分的组合：

\displaystyle{\bf p}\otimes{\bf q}=\begin{bmatrix}p_{w}q_{w}-{\bf p}_{v}^{\top}{\bf q}_{v}\\ p_{w}{\bf q}_{v}+q_{w}{\bf p}_{v}+{\bf p}_{v}\!\times\!{\bf q}_{v}\end{bmatrix}\leavevmode\nobreak\ ,

(13)

其中叉积的存在表明，四元数乘法在一般情况下是不可交换的，

\displaystyle{\bf p}\otimes{\bf q}\neq{\bf q}\otimes{\bf p}\leavevmode\nobreak\ .

(14)

这种普遍非交换性的例外情况仅限于以下情形：当其中一个四元数为实数 ${\bf p}_{v}\!\times\!{\bf q}_{v}=0$ 时，或 ${\bf p}=p_{w}$ ，或 ${\bf q}=q_{w}$ 时；或者当两个向量部分平行时 ${\bf p}_{v}\|{\bf q}_{v}$ 。只有在这些情况下，四元数乘法才是可交换的。

然而，四元数乘法具有结合律特性，

\displaystyle({\bf p}\otimes{\bf q})\otimes{\bf r}={\bf p}\otimes({\bf q}\otimes{\bf r})\leavevmode\nobreak\ ,

(15)

并且对和具有分配性，

\displaystyle{\bf p}\otimes({\bf q}+{\bf r})={\bf p}\otimes{\bf q}+{\bf p}\otimes{\bf r}\qquad\textrm{and}\qquad({\bf p}+{\bf q})\otimes{\bf r}={\bf p}\otimes{\bf r}+{\bf q}\otimes{\bf r}\leavevmode\nobreak\ .

(16)

两个四元数的乘积是双线性的，可以表示为两种等价的矩阵乘积形式，即

\displaystyle{\bf q}_{1}\otimes{\bf q}_{2}=[{\bf q}_{1}]_{L}\,{\bf q}_{2}\qquad\textrm{and}\qquad{\bf q}_{1}\otimes{\bf q}_{2}=[{\bf q}_{2}]_{R}\,{\bf q}_{1}\leavevmode\nobreak\ ,

(17)

其中 $[{\bf q}]_{L}$ 和 $[{\bf q}]_{R}$ 分别为左、右四元数乘积矩阵，通过简单观察即可由(12)和(17)式导出，

\displaystyle[{\bf q}]_{L}=\begin{bmatrix}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{bmatrix},\qquad[{\bf q}]_{R}=\begin{bmatrix}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{bmatrix},

(18)

或更简洁地，由(13)和(17)式表示，

\displaystyle[{\bf q}]_{L}=q_{w}\,{\bf I}+\begin{bmatrix}0&-{\bf q}_{v}^{\top}\\ {\bf q}_{v}&\left[{\bf q}_{v}\right]_{\times}\end{bmatrix},\qquad[{\bf q}]_{R}=q_{w}\,{\bf I}+\begin{bmatrix}0&-{\bf q}_{v}^{\top}\\ {\bf q}_{v}&-\left[{\bf q}_{v}\right]_{\times}\end{bmatrix}\leavevmode\nobreak\ .\leavevmode\nobreak\

(19)

此处， 斜算子 ²²2The skew-operator can be found in the literature in a number of different names and notations, either related to the cross operator $\times$ , or to the ‘hat’ operator ^∧, so that all the forms below are equivalent, $\left[{\bf a}\right]_{\times}\equiv[{\bf a}_{\times}]\equiv{\bf a}\!\times\equiv{\bf a}_{\times}\equiv[{\bf a}]\equiv\widehat{{\bf a}}\equiv{\bf a}^{\wedge}\leavevmode\nobreak\ .$ $\left[\bullet\right]_{\times}$ 生成叉积矩阵，

\displaystyle\left[{\bf a}\right]_{\times}\triangleq\begin{bmatrix}0&-a_{z}&a_{y}\\ a_{z}&0&-a_{x}\\ -a_{y}&a_{x}&0\end{bmatrix}\leavevmode\nobreak\ ,

(20)

该矩阵为斜对称矩阵 $\left[{\bf a}\right]_{\times}^{\top}=-\left[{\bf a}\right]_{\times}$ ，等价于叉积运算，即，

\displaystyle\left[{\bf a}\right]_{\times}{\bf b}={\bf a}\!\times\!{\bf b}\leavevmode\nobreak\ ,\quad\forall\,{\bf a},{\bf b}\in{\mathbb{R}}^{3}\leavevmode\nobreak\ .

(21)

最后，由于

	$\displaystyle{\bf q}\otimes{\bf x}\otimes{\bf p}$	$\displaystyle=({\bf q}\otimes{\bf x})\otimes{\bf p}=[{\bf p}]_{R}\,[{\bf q}]_{L}\,{\bf x}$
		$\displaystyle={\bf q}\otimes({\bf x}\otimes{\bf p})=[{\bf q}]_{L}\,[{\bf p}]_{R}\,{\bf x}\leavevmode\nobreak\ ,$

我们得到关系式

\displaystyle[{\bf p}]_{R}\,[{\bf q}]_{L}=[{\bf q}]_{L}\,[{\bf p}]_{R}\leavevmode\nobreak\ ,

(22)

即左乘和右乘四元数矩阵是可交换的。这些矩阵的更多性质将在 2.8 节中提供。

配备乘积运算 $\otimes$ 的四元数构成一个非交换群。该群的单位元 ${\bf q}_{1}=1$ 和逆元 ${\bf q}^{-1}$ 将在下文探讨。

1.2.3 单位元

关于该乘积运算的单位四元数 ${\bf q}_{{\,}_{\bf 1}}$ 满足 ${\bf q}_{{\,}_{\bf 1}}\otimes{\bf q}={\bf q}\otimes{\bf q}_{{\,}_{\bf 1}}={\bf q}$ 。它对应于实数乘积中的单位元"1"，以四元数形式表示，

\displaystyle{\bf q}_{{\,}_{\bf 1}}=1=\begin{bmatrix}1\\ {\bf 0}_{v}\end{bmatrix}\leavevmode\nobreak\ .

1.2.4 共轭

四元数的共轭定义为

\displaystyle{\bf q}^{*}\triangleq q_{w}-{\bf q}_{v}=\begin{bmatrix}q_{w}\\ -{\bf q}_{v}\end{bmatrix}\leavevmode\nobreak\ .

(23)

其具有以下性质

\displaystyle{\bf q}\otimes{\bf q}^{*}={\bf q}^{*}\otimes{\bf q}=q_{w}^{2}+q_{x}^{2}+q_{y}^{2}+q_{z}^{2}=\begin{bmatrix}q_{w}^{2}+q_{x}^{2}+q_{y}^{2}+q_{z}^{2}\\ {\bf 0}_{v}\end{bmatrix}\leavevmode\nobreak\ ,

(24)

且

\displaystyle({\bf p}\otimes{\bf q})^{*}={\bf q}^{*}\otimes{\bf p}^{*}\leavevmode\nobreak\ .

(25)

1.2.5 范数

四元数的范数定义为

\displaystyle{\left\|{\bf q}\right\|}\triangleq\sqrt{{\bf q}\otimes{\bf q}^{*}}=\sqrt{{\bf q}^{*}\otimes{\bf q}}=\sqrt{q_{w}^{2}+q_{x}^{2}+q_{y}^{2}+q_{z}^{2}}\leavevmode\nobreak\ \in{\mathbb{R}}\leavevmode\nobreak\ .

(26)

它具有以下性质

\displaystyle{\left\|{\bf p}\otimes{\bf q}\right\|}={\left\|{\bf q}\otimes{\bf p}\right\|}={\left\|{\bf p}\right\|}{\left\|{\bf q}\right\|}\leavevmode\nobreak\ .

(27)

•

1.2.6 逆

四元数的逆 ${\bf q}^{-1}$ 满足该四元数与其逆相乘等于单位元，

\displaystyle{\bf q}\otimes{\bf q}^{-1}={\bf q}^{-1}\otimes{\bf q}={\bf q}_{{\,}_{\bf 1}}\leavevmode\nobreak\ .

(28)

其计算方式为

\displaystyle{\bf q}^{-1}={\bf q}^{*}/{\left\|{\bf q}\right\|}^{2}\leavevmode\nobreak\ .

(29)

1.2.7 单位或归一化四元数

对于单位四元数， ${\left\|{\bf q}\right\|}=1$ ，因此

\displaystyle{\bf q}^{-1}={\bf q}^{*}\leavevmode\nobreak\ .

(30)

当将单位四元数解释为方向规范或旋转算子时，该性质意味着逆旋转可通过共轭四元数实现。单位四元数总可表示为以下形式：

\displaystyle{\bf q}=\begin{bmatrix}\cos\theta\\ {\bf u}\sin\theta\end{bmatrix}\leavevmode\nobreak\ ,

(31)

其中 ${\bf u}=u_{x}i+u_{y}j+u_{z}k$ 是单位向量， $\theta$ 是标量。

根据(27)，赋予乘积运算 $\otimes$ 的单位四元数构成一个非交换群，其中逆元与共轭元重合。

1.3 四元数的附加性质

1.3.1 四元数交换子

四元数的交换子定义为 $[{\bf p},{\bf q}]\triangleq{\bf p}\otimes{\bf q}-{\bf q}\otimes{\bf p}$ 。由(13)式可得，

\displaystyle{\bf p}\otimes{\bf q}-{\bf q}\otimes{\bf p}=2\,{\bf p}_{v}\!\times\!{\bf q}_{v}\leavevmode\nobreak\ .

(32)

这直接导致了一个简单的结果，

\displaystyle{\bf p}_{v}\otimes{\bf q}_{v}-{\bf q}_{v}\otimes{\bf p}_{v}=2\,{\bf p}_{v}\!\times\!{\bf q}_{v}\leavevmode\nobreak\ .

(33)

我们将在后续利用这一性质。

1.3.2 纯四元数的乘积

纯四元数是指实部或标量部分为零的四元数，即 $Q={\bf q}_{v}$ 或 ${\bf q}=[0,{\bf q}_{v}]$ 。由(13)可知，

\displaystyle{\bf p}_{v}\otimes{\bf q}_{v}=-{\bf p}_{v}^{\top}{\bf q}_{v}+{\bf p}_{v}\!\times\!{\bf q}_{v}=\begin{bmatrix}-{\bf p}_{v}^{\top}{\bf q}_{v}\\ {\bf p}_{v}\!\times\!{\bf q}_{v}\end{bmatrix}\leavevmode\nobreak\ .

(34)

这意味着

\displaystyle{\bf q}_{v}\otimes{\bf q}_{v}=-{\bf q}_{v}^{\top}{\bf q}_{v}=-{\left\|{\bf q}_{v}\right\|}^{2}\leavevmode\nobreak\ ,

(35)

对于纯单位四元数 ${\bf u}\in{\mathbb{H}}_{p},\leavevmode\nobreak\ {\left\|{\bf u}\right\|}=1$ ，

\displaystyle{\bf u}\otimes{\bf u}=-1\leavevmode\nobreak\ ,

(36)

这与标准的虚数情况类似， $i\cdot i=-1$ 。

1.3.3 纯四元数的自然幂

让我们定义 ${\bf q}^{n},\leavevmode\nobreak\ n\in{\mathbb{N}}$ 为 ${\bf q}$ 的 $n$ 次幂，使用四元数乘积 $\otimes$ 。那么，如果 ${\bf v}$ 是一个纯四元数，且我们令 ${\bf v}={\bf u}\,\theta$ ，其中 $\theta={\left\|{\bf v}\right\|}\in{\mathbb{R}}$ 和 ${\bf u}$ 为单位四元数，则从(35)式可得循环模式

\displaystyle{\bf v}^{2}=-\theta^{2}\quad,\quad{\bf v}^{3}=-{\bf u}\,\theta^{3}\quad,\quad{\bf v}^{4}=\theta^{4}\quad,\quad{\bf v}^{5}={\bf u}\,\theta^{5}\quad,\quad{\bf v}^{6}=-\theta^{6}\quad,\quad\cdots

(37)

而对于纯单位四元数 ${\bf u}$ ，这简化为模式

\displaystyle{\bf u}^{2}=-1\quad,\quad{\bf u}^{3}=-{\bf u}\quad,\quad{\bf u}^{4}=1\quad,\quad{\bf u}^{5}={\bf u}\quad,\quad{\bf u}^{6}=-1\quad,\quad\cdots

(38)

1.3.4 纯四元数的指数

四元数指数函数是与普通指数函数类似的四元数函数。与实数指数情形完全一致，其定义为绝对收敛的幂级数：

\displaystyle e^{\bf q}\triangleq\sum_{k=0}^{\infty}\frac{1}{k!}{\bf q}^{k}\quad\in{\mathbb{H}}\leavevmode\nobreak\ .

(39)

显然，实四元数的指数函数与普通指数函数完全一致。

更有趣的是，纯四元数 ${\bf v}=v_{x}i+v_{y}j+v_{z}k$ 的指数定义为一个新的四元数，

\displaystyle e^{\bf v}=\sum_{k=0}^{\infty}\frac{1}{k!}{\bf v}^{k}\quad\in{\mathbb{H}}\leavevmode\nobreak\ .

(40)

令 ${\bf v}={\bf u}\,\theta$ ，其中 $\theta={\left\|{\bf v}\right\|}\in{\mathbb{R}}$ 和 ${\bf u}$ 为单位量，并考虑(37)，我们将级数中的标量项与向量项分组，

\displaystyle e^{{\bf u}\theta}

\displaystyle=\left(1-\frac{\theta^{2}}{2!}+\frac{\theta^{4}}{4!}+\cdots\right)+\left({\bf u}\theta-\frac{{\bf u}\theta^{3}}{3!}+\frac{{\bf u}\theta^{5}}{5!}+\cdots\right)

(41)

并分别识别出它们对应于 $\cos\theta$ 和 $\sin\theta$ 的级数展开。 ³³3We remind that $\cos\theta=1-\theta^{2}/2!+\theta^{4}/4!-\cdots$ , and $\sin\theta=\theta-\theta^{3}/3!+\theta^{5}/5!-\cdots$ . 由此可得

\displaystyle e^{\bf v}=e^{{\bf u}\,\theta}=\cos\theta+{\bf u}\sin\theta=\begin{bmatrix}\cos\theta\\ {\bf u}\sin\theta\end{bmatrix}\leavevmode\nobreak\ ,

(42)

这构成了欧拉公式的一个优美扩展， $e^{i\theta}=\cos\theta+i\sin\theta$ ，该公式最初是为虚数定义的。注意到由于 ${\left\|e^{{\bf v}}\right\|}^{2}=\cos^{2}\theta+\sin^{2}\theta=1$ ，纯四元数的指数是一个单位四元数。同时还需注意这一性质，

\displaystyle e^{-{\bf v}}=\left(e^{{\bf v}}\right)^{*}\leavevmode\nobreak\ .

(43)

对于小角度四元数，我们通过展开 $\sin\theta$ 和 $\cos\theta$ 的泰勒级数并截断来避免 ${\bf u}={\bf v}/{\left\|{\bf v}\right\|}$ 中的除以零问题，从而获得不同精度的近似解。

\displaystyle e^{\bf v}\approx\begin{bmatrix}1-\theta^{2}/2\\ {\bf v}\big{(}1-\theta^{2}/6\big{)}\end{bmatrix}\approx\begin{bmatrix}1\\ {\bf v}\end{bmatrix}\xrightarrow[\theta\to 0]{}\begin{bmatrix}1\\ {\bf 0}\end{bmatrix}\leavevmode\nobreak\ .

(44)

1.3.5 一般四元数的指数

由于四元数乘法不满足交换律，对于一般四元数 ${\bf p}$ 和 ${\bf q}$ ，我们不能写成 $e^{{\bf p}+{\bf q}}=e^{\bf p}e^{\bf q}$ 。然而，当乘积中的任一成员为标量时交换律成立，因此，

\displaystyle e^{\bf q}=e^{q_{w}+{\bf q}_{v}}=e^{q_{w}}\,e^{{\bf q}_{v}}\leavevmode\nobreak\ .

(45)

于是，将(42)与 ${\bf u}\theta={\bf q}_{v}$ 联立可得

\displaystyle e^{\bf q}=e^{q_{w}}\begin{bmatrix}\cos{\left\|{\bf q}_{v}\right\|}\\ \frac{{\bf q}_{v}}{{\left\|{\bf q}_{v}\right\|}}\sin{\left\|{\bf q}_{v}\right\|}\end{bmatrix}\leavevmode\nobreak\ .

(46)

1.3.6 单位四元数的对数

显然可见，若 ${\left\|{\bf q}\right\|}=1$ ，

\displaystyle\log{\bf q}=\log(\cos\theta+{\bf u}\sin\theta)=\log(e^{{\bf u}\,\theta})={\bf u}\,\theta=\begin{bmatrix}0\\ {\bf u}\,\theta\end{bmatrix}\leavevmode\nobreak\ ,

(47)

即，单位四元数的对数是一个纯四元数。通过反转（42）可以轻松获得角度-轴值，

	$\displaystyle{\bf u}$	$\displaystyle={\bf q}_{v}/{\left\\|{\bf q}_{v}\right\\|}$		(48)
	$\displaystyle\theta$	$\displaystyle=\arctan({\left\\|{\bf q}_{v}\right\\|},q_{w})\leavevmode\nobreak\ .$		(49)

对于小角度四元数，我们通过表达 $\arctan(x)$ 的泰勒级数并截断， ⁴⁴4We remind that $\arctan x=x-x^{3}/3+x^{5}/5-\cdots$ , and $\arctan(y,x)\equiv\arctan(y/x)$ . 获得不同阶数的近似值，以避免除以零的情况，

\displaystyle\log({\bf q})={\bf u}\theta

\displaystyle={\bf q}_{v}\frac{\arctan({{\left\|{\bf q}_{v}\right\|},q_{w}})}{{\left\|{\bf q}_{v}\right\|}}\approx\frac{{\bf q}_{v}}{q_{w}}\left(1-\frac{{\left\|{\bf q}_{v}\right\|}^{2}}{3q_{w}^{2}}\right)\approx{\bf q}_{v}\xrightarrow[\theta\to 0]{}{\bf 0}\leavevmode\nobreak\ .

(50)

1.3.7 一般四元数的对数

进一步地，若 ${\bf q}$ 为一普通四元数，

\displaystyle\log{\bf q}=\log({\left\|{\bf q}\right\|}\frac{{\bf q}}{{\left\|{\bf q}\right\|}})=\log{\left\|{\bf q}\right\|}+\log\frac{{\bf q}}{{\left\|{\bf q}\right\|}}=\log{\left\|{\bf q}\right\|}+{\bf u}\,\theta=\begin{bmatrix}\log{\left\|{\bf q}\right\|}\\ {\bf u}\,\theta\end{bmatrix}\leavevmode\nobreak\ .

(51)

1.3.8 类型 ${\bf q}^{t}$ 的指数形式

对于 ${\bf q}\in{\mathbb{H}}$ 与 $t\in{\mathbb{R}}$ ，我们有

\displaystyle{\bf q}^{t}=\exp(\log({\bf q}^{t}))=\exp(t\log({\bf q}))\leavevmode\nobreak\ .

(52)

当 ${\left\|{\bf q}\right\|}=1$ 时，可表示为 ${\bf q}=[\cos\theta,\leavevmode\nobreak\ {\bf u}\sin\theta]$ ，故得 $\log({\bf q})={\bf u}\theta$ ，由此可得

\displaystyle{\bf q}^{t}=\exp(t\,{\bf u}\theta)=\begin{bmatrix}\cos t\theta\\ {\bf u}\sin t\theta\end{bmatrix}\leavevmode\nobreak\ .

(53)

由于指数项 $t$ 最终成为角度 $\theta$ 的线性乘数，可将其视为线性角度插值器。我们将在 2.7 节深入阐述这一概念。

2 旋转与交叉关系

2.1 三维向量旋转公式

Refer to caption — 图 1：向量 ${\bf x}$ 绕轴 ${\bf u}$ 旋转角度 $\phi$ 的示意图。详见正文说明。

我们在图 1 中展示了遵循右手定则的三维向量 ${\bf x}$ 绕单位向量 ${\bf u}$ 所定义轴线旋转角度 $\phi$ 的过程。这一旋转通过将向量 ${\bf x}$ 分解为平行于 ${\bf u}$ 的分量 ${\bf x}_{||}$ 和正交于 ${\bf u}$ 的分量 ${\bf x}_{\bot}$ 来实现，即

\displaystyle{\bf x}={\bf x}_{||}+{\bf x}_{\bot}\leavevmode\nobreak\ .

这些分量可简便计算得出（其中 $\alpha$ 表示向量 ${\bf x}$ 与轴线 ${\bf u}$ 之间的夹角），

	$\displaystyle{\bf x}_{\|\|}$	$\displaystyle={\bf u}\,({\left\\|{\bf x}\right\\|}\cos\alpha)={\bf u}\,{\bf u}^{\top}\,{\bf x}$
	$\displaystyle{\bf x}_{\bot}$	$\displaystyle={\bf x}-{\bf x}_{\|\|}={\bf x}-{\bf u}\,{\bf u}^{\top}\,{\bf x}\leavevmode\nobreak\ .$

旋转时，平行分量不发生转动，

\displaystyle{\bf x}_{||}^{\prime}={\bf x}_{||}\leavevmode\nobreak\ ,

而正交分量将在与 ${\bf u}$ 垂直的平面内发生旋转。具体而言，若我们构建该平面的正交基 $\{{\bf e}_{1},{\bf e}_{2}\}$ ，满足

	$\displaystyle{\bf e}_{1}$	$\displaystyle={\bf x}_{\bot}$
	$\displaystyle{\bf e}_{2}$	$\displaystyle={\bf u}\!\times\!{\bf x}_{\bot}={\bf u}\!\times\!{\bf x}\leavevmode\nobreak\ ,$

条件 ${\left\|{\bf e}_{1}\right\|}={\left\|{\bf e}_{2}\right\|}$ ，则有 ${\bf x}_{\bot}={\bf e}_{1}\!\cdot\!1+{\bf e}_{2}\!\cdot\!0$ 。在该平面上进行 $\phi$ 弧度的旋转将产生，

\displaystyle{\bf x}_{\bot}^{\prime}={\bf e}_{1}\cos\phi+{\bf e}_{2}\sin\phi\leavevmode\nobreak\ ,

其发展过程为，

\displaystyle{\bf x}_{\bot}^{\prime}={\bf x}_{\bot}\cos\phi+({\bf u}\!\times\!{\bf x})\sin\phi\leavevmode\nobreak\ .

加上平行部分后，便得到了旋转向量的表达式 ${\bf x}^{\prime}={\bf x}^{\prime}_{||}+{\bf x}^{\prime}_{\bot}$ ，这被称为向量旋转公式

\displaystyle\boxed{{\bf x}^{\prime}={\bf x}_{||}+{\bf x}_{\bot}\cos\phi+({\bf u}\times{\bf x})\sin\phi}\leavevmode\nobreak\ .

(54)

2.2 旋转群 $SO(3)$

在文献[0]中，旋转群[1]定义为原点旋转在复合运算下构成的群。旋转是保持向量长度和相对向量方向（即手性）的线性变换。其在机器人学中的重要性在于它表示三维空间中刚体的旋转：刚体运动要求运动过程中刚体内部的距离、角度和相对方向必须保持不变——否则，若范数、角度或相对方向未能保持，该物体就不能被视为刚体。

让我们通过满足这些性质的算子来定义旋转。作用于向量[1]的旋转算子[0]可以从欧几里得空间的度量（由点积和叉积构成）定义如下。

•

旋转变换保持向量范数不变，


	$\displaystyle{\left\\|r({\bf v})\right\\|}=\sqrt{\langle r({\bf v}),r({\bf v})\rangle}=\sqrt{\langle{\bf v},{\bf v}\rangle}\triangleq{\left\\|{\bf v}\right\\|}\leavevmode\nobreak\ ,\quad\forall{\bf v}\in{\mathbb{R}}^{3}\leavevmode\nobreak\ .$		(55a)

•

旋转保持向量间的角度不变

\displaystyle\langle r({\bf v}),r({\bf w})\rangle=\langle{\bf v},{\bf w}\rangle={\left\|{\bf v}\right\|}{\left\|{\bf w}\right\|}\cos\alpha\leavevmode\nobreak\ ,\quad\forall{\bf v},{\bf w}\in{\mathbb{R}}^{3}\leavevmode\nobreak\ .

(55b)

•

旋转保持了向量间的相对方向关系，

$\displaystyle{\bf u}\times{\bf v}={\bf w}\iff r({\bf u})\times r({\bf v})=r({\bf w})\leavevmode\nobreak\ .$ (56)

容易证明前两个条件是等价的。因此我们可以将旋转群[0]定义为，

\displaystyle SO(3):\{r:{\mathbb{R}}^{3}\to{\mathbb{R}}^{3}\,/\,\forall\,{\bf v},{\bf w}\in{\mathbb{R}}^{3}\leavevmode\nobreak\ ,\leavevmode\nobreak\ {\left\|r({\bf v})\right\|}={\left\|{\bf v}\right\|}\leavevmode\nobreak\ ,\leavevmode\nobreak\ r({\bf v})\!\times\!r({\bf w})=r({\bf v}\!\times\!{\bf w})\}\leavevmode\nobreak\ .

(57)

旋转群通常由旋转矩阵的集合来表示。然而，四元数同样能很好地表示它。本章旨在证明这两种表示方式具有同等有效性。正如读者将在表 1 中所见，它们在概念和代数层面都展现出诸多相似性。

表 1：用于表示

SO(3)

的旋转矩阵与四元数

	旋转矩阵， ${\bf R}$	四元数， ${\bf q}$
参数	$3\times 3=9$	$1+3=4$
自由度	3	3
约束条件	$9-3=6$	$4-3=1$
约束条件	${\bf R}{\bf R}^{\top}={\bf I}\leavevmode\nobreak\ \leavevmode\nobreak\ ;\leavevmode\nobreak\ \leavevmode\nobreak\ \det({\bf R})=+1$	${\bf q}\otimes{\bf q}^{*}=1$
ODE	$\dot{\bf R}={\bf R}\left[{\bm{\omega}}\right]_{\times}$	$\dot{\bf q}=\frac{1}{2}{\bf q}\otimes{\bm{\omega}}$
指数映射	${\bf R}=\exp(\left[{\bf u}\phi\right]_{\times})$	${\bf q}=\exp({\bf u}\phi/2)$
对数映射	$\log({\bf R})=\left[{\bf u}\phi\right]_{\times}$	$\log({\bf q})={\bf u}\phi/2$
与 $SO(3)$ 的关系	单一覆盖	双层覆盖
恒等元	${\bf I}$	$1$
逆元	${\bf R}^{\top}$	${\bf q}^{*}$
复合运算	${\bf R}_{1}\,{\bf R}_{2}$	${\bf q}_{1}\otimes{\bf q}_{2}$
旋转算子	${\bf R}={\bf I}+\sin\phi\left[{\bf u}\right]_{\times}+(1-\cos\phi)\left[{\bf u}\right]_{\times}^{2}$	${\bf q}=\cos\phi/2+{\bf u}\sin\phi/2$
旋转作用	${\bf R}\,{\bf x}$	${\bf q}\otimes{\bf x}\otimes{\bf q}^{*}$
插值	${\bf R}^{t}={\bf I}+\sin t\phi\left[{\bf u}\right]_{\times}\!+(1\!-\!\cos t\phi)\left[{\bf u}\right]_{\times}^{2}$	${\bf q}^{t}=\cos t\phi/2+{\bf u}\sin t\phi/2$
	${\bf R}_{1}({\bf R}_{1}^{\top}{\bf R}_{2})^{t}$	${\bf q}_{1}\otimes({\bf q}_{1}^{*}\otimes{\bf q}_{2})^{t}$
		${\bf q}_{1}\frac{\sin((1-t)\Delta\theta)}{\sin(\Delta\theta)}+{\bf q}_{2}\frac{\sin(t\Delta\theta)}{\sin(\Delta\theta)}$
交叉关系	$\begin{aligned} \rule{0.0pt}{11.62494pt}{\bf R}\{{\bf q}\}&=(q_{w}^{2}-{\bf q}_{v}^{\top}{\bf q}_{v})\,{\bf I}+2\,{\bf q}_{v}{\bf q}_{v}^{\top}+2\,q_{w}\left[{\bf q}_{v}\right]_{\times}\\ {\bf R}\{-{\bf q}\}&={\bf R}\{{\bf q}\}\leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \,\text{double cover}\\ {\bf R}\{1\}&={\bf I}\leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \text{identity}\\ {\bf R}\{{\bf q}^{*}\}&={\bf R}\{{\bf q}\}^{\top}\leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \text{inverse}\\ {\bf R}\{{\bf q}_{1}\otimes{\bf q}_{2}\}&={\bf R}\{{\bf q}_{1}\}\,{\bf R}\{{\bf q}_{2}\}\leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \text{composition}\\ {\bf R}\{{\bf q}^{t}\}&={\bf R}\{{\bf q}\}^{t}\leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \,\text{interpolation}\end{aligned}$

或许最重要的区别在于，单位四元数群构成了 $SO(3)$ 的双重覆盖（因此严格来说并非 $SO(3)$ 本身），这一特性在我们的大多数应用中并不关键。 ⁵⁵5The effect of the double cover needs to be considered when performing interpolation in the space of rotations. This is however easy, as we will see in Section 2.7. 为便于快速比较和评估，表格已前置插入。 $SO(3)$ 的旋转矩阵与四元数表示形式将在后续章节中探讨。

2.3 旋转群与旋转矩阵

由于 $r()$ 运算符是通过标量积和向量积（二者均为线性运算）定义的，因此它是线性的。该运算可通过矩阵 ${\bf R}\in{\mathbb{R}}^{3\times 3}$ 表示，并通过矩阵乘积对向量 ${\bf v}\in{\mathbb{R}}^{3}$ 实施旋转。

\displaystyle r({\bf v})={\bf R}\,{\bf v}\leavevmode\nobreak\ .

(58)

将其代入(55a)，利用点积 $\langle{\bf a},{\bf b}\rangle={\bf a}^{\top}{\bf b}$ 并展开可得对于所有 ${\bf v}$ ，

\displaystyle({\bf R}{\bf v})^{\top}({\bf R}{\bf v})={\bf v}^{\top}{\bf R}^{\top}{\bf R}{\bf v}={\bf v}^{\top}{\bf v}\leavevmode\nobreak\ ,

(59)

由此得到关于 ${\bf R}$ 的正交性条件，

\displaystyle\boxed{{\bf R}^{\top}{\bf R}={\bf I}={\bf R}\,{\bf R}^{\top}}\leavevmode\nobreak\ .

(60)

上述条件实为正交性条件，因为通过令 ${\bf R}=[{\bf r}_{1},{\bf r}_{2},{\bf r}_{3}]$ 并代入上式可观察到，矩阵 ${\bf R}$ 的列向量 ${\bf r}_{i}$ （其中 $i\in\{1,2,3\}$ ）具有单位长度且彼此正交

	$\displaystyle\langle{\bf r}_{i},{\bf r}_{i}\rangle$	$\displaystyle={\bf r}_{i}^{\top}{\bf r}_{i}=1$
	$\displaystyle\langle{\bf r}_{i},{\bf r}_{j}\rangle$	$\displaystyle={\bf r}_{i}^{\top}{\bf r}_{j}=0\leavevmode\nobreak\ ,\quad\textrm{if }i\neq j\leavevmode\nobreak\ .$

保持向量范数与角度不变的变换集合因此被称为正交群 ，记作 $O(3)$ 。正交群包含旋转（刚体运动）与反射（非刚体运动）。此处群的概念本质上（非正式地）意味着两个正交矩阵的乘积仍是正交矩阵 ⁶⁶6Let ${\bf Q}_{1}$ and ${\bf Q}_{2}$ be orthogonal, and build ${\bf Q}={\bf Q}_{1}\,{\bf Q}_{2}$ . Then ${\bf Q}^{\top}{\bf Q}={\bf Q}_{2}^{\top}{\bf Q}_{1}^{\top}{\bf Q}_{1}{\bf Q}_{2}={\bf Q}_{2}^{\top}{\bf I}{\bf Q}_{2}={\bf I}$ . 且每个正交矩阵都存在逆矩阵。实际上，正交性条件（60）意味着逆旋转可通过转置矩阵实现，

\displaystyle{\bf R}^{-1}={\bf R}^{\top}\leavevmode\nobreak\ .

(61)

添加相对方向条件（56）可保证刚体运动（从而排除反射），并对 ${\bf R}$ 、 ⁷⁷7Notice that reflections satisfy $|{\bf R}|=\det({\bf R})=-1$ , and do not form a group since $|{\bf R}_{1}{\bf R}_{2}|=1\neq-1$ . 产生一个额外约束

\displaystyle\boxed{\det({\bf R})=1}\leavevmode\nobreak\ .

(62)

具有正单位行列式的正交矩阵通常被称为真或特殊正交矩阵。这类特殊正交矩阵的集合构成 $O(3)$ 的一个子群，称为特殊正交群 $SO(3)$ 。作为群结构，两个旋转矩阵的乘积始终是旋转矩阵。 ⁸⁸8See footnote 6 for $O(3)$ and add this for $SO(3)$ : let $|{\bf R}_{1}|=|{\bf R}_{2}|=1$ , then $|{\bf R}_{1}{\bf R}_{2}|=|{\bf R}_{1}|\,|{\bf R}_{2}|=1$ .

2.3.1 指数映射

指数映射（以及下节将介绍的对数映射）是在三维旋转空间中实现便捷严谨运算的强大数学工具。它开启了适用于旋转空间的微积分体系之门，使我们能正确定义导数、扰动与速度并进行运算。因此该映射在旋转/姿态空间的估计问题中具有基础性地位。

旋转构成刚体运动。这种刚性意味着可以在 $SO(3)$ 中定义一条连续轨迹或路径 $r(t)$ ，将刚体从其初始方位 $r(0)$ 连续旋转至当前方位 $r(t)$ 。由于连续性，研究此类变换的时间导数是合理的。我们通过推导刚见过的性质(60)和(62)来实现这一点。

首先注意到，在满足 (60)条件的同时，不可能连续脱离单位行列式条件 (62)，因为这将导致行列式从 $+1$ 跳跃至 $-1$ 。 ⁹⁹9Put otherwise: a rotation cannot become a reflection through a continuous transformation. 因此，我们只需研究正交性条件（60）的时间导数。其表达式为

\displaystyle\frac{d}{dt}{({\bf R}^{\top}{\bf R})}=\dot{\bf R}^{\top}{\bf R}+{\bf R}^{\top}\dot{\bf R}=0\leavevmode\nobreak\ ,

(63)

由此可得

\displaystyle{\bf R}^{\top}\dot{\bf R}=-({\bf R}^{\top}\dot{\bf R})^{\top}\leavevmode\nobreak\ ,

(64)

这意味着矩阵 ${\bf R}^{\top}\dot{\bf R}$ 是斜对称的（即，等于其转置的负矩阵）。斜对称 $3\times 3$ 矩阵的集合记为 ${\mathfrak{so}}(3)$ ，称为 $SO(3)$ 的李代数 。斜对称 $3\times 3$ 矩阵具有如下形式：

\displaystyle\left[{\bm{\omega}}\right]_{\times}\triangleq\begin{bmatrix}0&-\omega_{z}&\omega_{y}\\ \omega_{z}&0&-\omega_{x}\\ -\omega_{y}&\omega_{x}&0\end{bmatrix}\leavevmode\nobreak\ ;

(65)

它们具有 3 个自由度，对应于我们已在（20）中介绍的叉积矩阵。这建立了一一映射 ${\bm{\omega}}\in{\mathbb{R}}^{3}\leftrightarrow\left[{\bm{\omega}}\right]_{\times}\in{\mathfrak{so}}(3)$ 。现取向量 ${\bm{\omega}}=(\omega_{x},\omega_{y},\omega_{z})\in{\bf R}^{3}$ 并写作

\displaystyle{\bf R}^{\top}\dot{\bf R}=\left[{\bm{\omega}}\right]_{\times}\leavevmode\nobreak\ .

(66)

由此导出常微分方程(ODE)

\displaystyle\dot{\bf R}={\bf R}\left[{\bm{\omega}}\right]_{\times}\leavevmode\nobreak\ .

(67)

在原点附近，我们有 ${\bf R}={\bf I}$ ，上述方程简化为 $\dot{\bf R}=\left[{\bm{\omega}}\right]_{\times}$ 。因此，可将李代数 ${\mathfrak{so}}(3)$ 解释为 $r(t)$ 在原点处的导数空间；它构成了 $SO(3)$ 的切空间 ，或称速度空间 。基于这些事实，我们完全可以将 ${\bm{\omega}}$ 称为瞬时角速度矢量。

若 ${\bm{\omega}}$ 为常数，上述微分方程可进行时间积分如下

\displaystyle{\bf R}(t)={\bf R}(0)\,e^{\left[{\bm{\omega}}\right]_{\times}t}={\bf R}(0)\,e^{\left[{\bm{\omega}}t\right]_{\times}}

(68)

其中指数 $e^{\left[x\right]_{\times}}$ 由其泰勒级数定义，如下节所示。由于 ${\bf R}(0)$ 和 ${\bf R}(t)$ 是旋转矩阵，显然 $e^{\left[{\bm{\omega}}t\right]_{\times}}={\bf R}(0)^{\top}{\bf R}(t)$ 也是一个旋转矩阵。定义向量 ${\bm{\phi}}\triangleq{\bm{\omega}}\Delta t$ 为编码整个周期 $\Delta t$ 内完整旋转的旋转向量，我们得到

\displaystyle\boxed{{\bf R}=e^{\left[{\bm{\phi}}\right]_{\times}}}\leavevmode\nobreak\ .

(69)

这被称为指数映射，是从 ${\mathfrak{so}}(3)$ 到 $SO(3)$ 的一个映射，

\displaystyle\exp:{\mathfrak{so}}(3)\to SO(3)\leavevmode\nobreak\ ;\leavevmode\nobreak\ \left[{\bm{\phi}}\right]_{\times}\mapsto\exp(\left[{\bm{\phi}}\right]_{\times})=e^{\left[{\bm{\phi}}\right]_{\times}}\leavevmode\nobreak\ .

(70)

2.3.2 大写指数映射

上述指数映射有时会以符号滥用的方式表示，即将 ${\bm{\phi}}\in{\mathbb{R}}^{3}$ 与 $\left[{\bm{\phi}}\right]_{\times}\in{\mathfrak{so}}(3)$ 混淆。为避免可能的歧义，我们选择用大写 $\operatorname*{Exp}$ 的显式符号表示这个新映射 ${\mathbb{R}}^{3}\to SO(3)$ ，如（见图 2）所示

\displaystyle\operatorname*{Exp}:{\mathbb{R}}^{3}\to SO(3)\leavevmode\nobreak\ ;\leavevmode\nobreak\ {\bm{\phi}}\mapsto\operatorname*{Exp}({\bm{\phi}})=e^{\left[{\bm{\phi}}\right]_{\times}}\leavevmode\nobreak\ .

(71)

它与指数映射的关系是显而易见的，

\displaystyle\operatorname*{Exp}({\bm{\phi}})\triangleq\exp(\left[{\bm{\phi}}\right]_{\times})\leavevmode\nobreak\ .

(72)

在接下来的章节中，我们将看到被称为旋转向量或角度-轴向量的向量 ${\bm{\phi}}$ ，通过 ${\bm{\phi}}={\bm{\omega}}\Delta t=\phi{\bf u}$ 编码了旋转角度 $\phi$ 和旋转轴 ${\bf u}$ 。

2.3.3 旋转矩阵与旋转向量：罗德里格斯旋转公式

旋转矩阵通过指数映射(69)由旋转向量 ${\bm{\phi}}=\phi{\bf u}$ 定义，其中叉积矩阵 $\left[{\bm{\phi}}\right]_{\times}=\phi\left[{\bf u}\right]_{\times}$ 如(20)所定义。当 ${\bm{\phi}}=\phi{\bf u}$ 时，(69)的泰勒展开式为：

\displaystyle{\bf R}=e^{\phi\left[{\bf u}\right]_{\times}}={\bf I}+\phi\left[{\bf u}\right]_{\times}+\frac{1}{2}\phi^{2}\left[{\bf u}\right]_{\times}^{2}+\frac{1}{3!}\phi^{3}\left[{\bf u}\right]_{\times}^{3}+\frac{1}{4!}\phi^{4}\left[{\bf u}\right]_{\times}^{4}+\dots

(73)

当作用于单位向量 ${\bf u}$ 时，矩阵 $\left[{\bf u}\right]_{\times}$ 满足：

	$\displaystyle\left[{\bf u}\right]_{\times}^{2}$	$\displaystyle={\bf u}{\bf u}^{\top}-{\bf I}$		(74)
	$\displaystyle\left[{\bf u}\right]_{\times}^{3}$	$\displaystyle=-\left[{\bf u}\right]_{\times}\leavevmode\nobreak\ ,$		(75)

因此， $\left[{\bf u}\right]_{\times}$ 的所有幂次都可以按照循环模式用 $\left[{\bf u}\right]_{\times}$ 和 $\left[{\bf u}\right]_{\times}^{2}$ 表示。

\displaystyle\left[{\bf u}\right]_{\times}^{4}

\displaystyle=-\left[{\bf u}\right]_{\times}^{2}

\displaystyle\left[{\bf u}\right]_{\times}^{5}

\displaystyle=\left[{\bf u}\right]_{\times}

\displaystyle\left[{\bf u}\right]_{\times}^{6}

\displaystyle=\left[{\bf u}\right]_{\times}^{2}

\displaystyle\left[{\bf u}\right]_{\times}^{7}

\displaystyle=-\left[{\bf u}\right]_{\times}\leavevmode\nobreak\ \leavevmode\nobreak\ \cdots\leavevmode\nobreak\ .

(76)

将泰勒级数按 $\left[{\bf u}\right]_{\times}$ 和 $\left[{\bf u}\right]_{\times}^{2}$ 分组，并分别识别出其中的 $\sin\phi$ 和 $\cos\phi$ 级数，可推导出由旋转矢量求取旋转矩阵的闭合形式，即所谓的罗德里格斯旋转公式 ，

\displaystyle\boxed{{\bf R}={\bf I}+\sin\phi\left[{\bf u}\right]_{\times}+(1-\cos\phi)\left[{\bf u}\right]_{\times}^{2}}\leavevmode\nobreak\ ,

(77)

我们将其记为 ${\bf R}\{{\bm{\phi}}\}\triangleq\operatorname*{Exp}({\bm{\phi}})$ 。该公式存在若干变体，例如，利用(74)式可得，

\displaystyle{\bf R}

\displaystyle={\bf I}\cos\phi+\left[{\bf u}\right]_{\times}\sin\phi+{\bf u}{\bf u}^{\top}(1-\cos\phi)\leavevmode\nobreak\ .

(78)

2.3.4 对数映射

我们定义对数映射为指数映射的逆运算，

\displaystyle\log:SO(3)\to{\mathfrak{so}}(3)\leavevmode\nobreak\ ;\leavevmode\nobreak\ {\bf R}\mapsto\log({\bf R})=\left[{\bf u}\,\phi\right]_{\times}\leavevmode\nobreak\ ,

(79)

随着

	$\displaystyle\phi$	$\displaystyle=\arccos\left(\frac{\operatorname*{trace}({\bf R})-1}{2}\right)$		(80)
	$\displaystyle{\bf u}$	$\displaystyle=\frac{({\bf R}-{\bf R}^{\top})^{\vee}}{2\sin\phi}\leavevmode\nobreak\ ,$		(81)

其中 $\bullet^{\vee}$ 是 $\left[\bullet\right]_{\times}$ 的逆运算，即满足 $(\left[{\bf v}\right]_{\times})^{\vee}={\bf v}$ 与 $\left[{\bf V}^{\vee}\right]_{\times}={\bf V}$ 关系式。

我们还定义了其大写形式 $\operatorname*{Log}$ ，通过该形式可直接从旋转矩阵反解出旋转矢量 ${\bm{\phi}}={\bf u}\phi\in{\mathbb{R}}^{3}$ ，


	$\displaystyle\operatorname{Log}:SO(3)\to{\mathbb{R}}^{3}\leavevmode\nobreak\ ;\leavevmode\nobreak\ {\bf R}\mapsto\operatorname{Log}({\bf R})={\bf u}\,\phi\leavevmode\nobreak\ .$		(82a)

它与对数映射的关系是简单的，

\displaystyle\operatorname*{Log}({\bf R})\triangleq(\log({\bf R}))^{\vee}\leavevmode\nobreak\ .

(83)

2.3.5 旋转作用

将向量 ${\bf x}$ 绕单位轴 ${\bf u}$ 旋转角度 $\phi$ 可通过线性乘积实现

\displaystyle{\bf x}^{\prime}={\bf R}\,{\bf x}\leavevmode\nobreak\ ,

(84)

其中 ${\bf R}=\operatorname*{Exp}({\bf u}\phi)$ 。这可通过展开(84)、并运用(77)、(74)及(75)来证明

\displaystyle\begin{split}{\bf x}^{\prime}&={\bf R}\,{\bf x}\\ &=({\bf I}+\sin\phi\left[{\bf u}\right]_{\times}+(1-\cos\phi)\left[{\bf u}\right]_{\times}^{2})\,{\bf x}\\ &={\bf x}+\sin\phi\left[{\bf u}\right]_{\times}{\bf x}+(1-\cos\phi)\left[{\bf u}\right]_{\times}^{2}{\bf x}\\ &={\bf x}+\sin\phi({\bf u}\!\times\!{\bf x})+(1-\cos\phi)({\bf u}{\bf u}^{\top}-{\bf I})\,{\bf x}\\ &={\bf x}_{\|}+{\bf x}_{\bot}+\sin\phi({\bf u}\!\times\!{\bf x})-(1-\cos\phi)\,{\bf x}_{\bot}\\ &={\bf x}_{\|}+({\bf u}\!\times\!{\bf x})\sin\phi+{\bf x}_{\bot}\cos\phi\leavevmode\nobreak\ ,\end{split}

(85)

这正是向量旋转公式(54)。

2.4 旋转群与四元数

出于教学目的，我们着重强调四元数与旋转矩阵作为旋转群 $SO(3)$ 表示形式之间的关联性为此，我们最初将著名的四元数旋转作用公式作为假设，该公式表述为：

\displaystyle r({\bf v})={\bf q}\otimes{\bf v}\otimes{\bf q}^{*}\leavevmode\nobreak\ ,

(86)

这使得我们能够通过重述旋转矩阵所用论述的方式，完整地展开四元数章节。这一假设的准确性将在稍后的 2.4.5 节中得到证明，从而验证该方法的有效性。

让我们将上述旋转代入正交性条件（55a），并利用（27）展开得到

\displaystyle{\left\|{\bf q}\otimes{\bf v}\otimes{\bf q}^{*}\right\|}={\left\|{\bf q}\right\|}^{2}{\left\|{\bf v}\right\|}={\left\|{\bf v}\right\|}\leavevmode\nobreak\ .

(87)

由此可得 ${\left\|{\bf q}\right\|}^{2}=1$ ，即四元数的单位范数条件，其表达式为

\displaystyle\boxed{{\bf q}^{*}\otimes{\bf q}=1={\bf q}\otimes{\bf q}^{*}}\leavevmode\nobreak\ .

(88)

该条件类似于我们在旋转矩阵中遇到的（60）条件，其表达式为 ${\bf R}^{\top}{\bf R}={\bf I}={\bf R}{\bf R}^{\top}$ 。建议读者稍作停留，思考二者的相似之处。

同样地，我们证明相对方位条件（56）通过构造自动满足（如下所示，我们两次运用了公式（33）），

\displaystyle\begin{split}r({\bf v})\times r({\bf w})&=({\bf q}\otimes{\bf v}\otimes{\bf q}^{*})\times({\bf q}\otimes{\bf w}\otimes{\bf q}^{*})\\ (\ref{equ:quatCommutatorPure})\leavevmode\nobreak\ \leavevmode\nobreak\ &=\frac{1}{2}\big{(}({\bf q}\otimes{\bf v}\otimes{\bf q}^{*})\otimes({\bf q}\otimes{\bf w}\otimes{\bf q}^{*})-({\bf q}\otimes{\bf w}\otimes{\bf q}^{*})\otimes({\bf q}\otimes{\bf v}\otimes{\bf q}^{*})\big{)}\\ &=\frac{1}{2}({\bf q}\otimes{\bf v}\otimes{\bf w}\otimes{\bf q}^{*}-{\bf q}\otimes{\bf w}\otimes{\bf v}\otimes{\bf q}^{*})\\ &=\frac{1}{2}({\bf q}\otimes({\bf v}\otimes{\bf w}-{\bf w}\otimes{\bf v})\otimes{\bf q}^{*})\\ (\ref{equ:quatCommutatorPure})\leavevmode\nobreak\ \leavevmode\nobreak\ &={\bf q}\otimes({\bf v}\times{\bf w})\otimes{\bf q}^{*}\\ &=r({\bf v}\times{\bf w})\leavevmode\nobreak\ .\end{split}

(89)

单位四元数集合在乘法运算下构成一个群。该群在拓扑结构上是三维球面，即四维单位球体的三维表面 ${\mathbb{R}}^{4}$ ，通常记作 $S^{3}$ 。

2.4.1 指数映射

考虑一个单位四元数 ${\bf q}\in S^{3}$ ，即满足 ${\bf q}^{*}\otimes{\bf q}=1$ ，我们按照处理旋转矩阵正交条件 ${\bf R}^{\top}{\bf R}={\bf I}$ 的方式进行推导。对其求时间导数，

\displaystyle\frac{d({\bf q}^{*}\otimes{\bf q})}{dt}=\dot{\bf q}^{*}\otimes{\bf q}+{\bf q}^{*}\otimes\dot{\bf q}=0\leavevmode\nobreak\ ,

(90)

由此可得

\displaystyle{\bf q}^{*}\otimes\dot{\bf q}=-(\dot{\bf q}^{*}\otimes{\bf q})=-({\bf q}^{*}\otimes\dot{\bf q})^{*}\leavevmode\nobreak\ ,

(91)

这意味着 ${\bf q}^{*}\otimes\dot{\bf q}$ 是一个纯四元数（即，它等于其共轭的相反数，因此实部为零）。因此我们取一个纯四元数 ${\bm{\Omega}}\in{\mathbb{H}}_{p}$ 并表示为，

\displaystyle{\bf q}^{*}\otimes\dot{\bf q}={\bm{\Omega}}=\begin{bmatrix}0\\ {\bm{\Omega}}\end{bmatrix}\in{\mathbb{H}}_{p}\leavevmode\nobreak\ .

(92)

左乘 ${\bf q}$ 得到微分方程，

\displaystyle\dot{\bf q}={\bf q}\otimes{\bm{\Omega}}\leavevmode\nobreak\ .

(93)

在原点附近，我们有 ${\bf q}=1$ ，上述方程简化为 $\dot{\bf q}={\bm{\Omega}}\in{\mathbb{H}}_{p}$ 。因此，纯四元数的空间 ${\mathbb{H}}_{p}$ 构成了四元数单位球面 $S^{3}$ 的切空间 ，即其李代数。然而在四元数情形下，这个空间并非直接对应于速度空间，而是半速度空间，这一点我们稍后将详细说明。

若 ${\bm{\Omega}}$ 为常数，该微分方程可积分得到

\displaystyle{\bf q}(t)={\bf q}(0)\otimes e^{{\bm{\Omega}}\,t}\leavevmode\nobreak\ ,

(94)

由于 ${\bf q}(0)$ 和 ${\bf q}(t)$ 均为单位四元数，其指数映射 $e^{{\bm{\Omega}}t}$ 仍为单位四元数——这从四元数指数性质（42）中已可推知。定义 ${\bf V}\triangleq{\bm{\Omega}}\Delta t$ 后可得

\displaystyle\boxed{{\bf q}=e^{{\bf V}}}\leavevmode\nobreak\ .

(95)

这仍是一个指数映射：从纯四元数空间到由单位四元数表征的旋转空间的映射

\displaystyle\exp:{\mathbb{H}}_{p}\to S^{3}\leavevmode\nobreak\ ;\leavevmode\nobreak\ {\bf V}\mapsto\exp({\bf V})=e^{{\bf V}}

(96)

2.4.2 大写指数映射

如我们所见，指数映射(96)中的纯四元数 ${\bf V}$ 通过 ${\bf V}=\theta{\bf u}=\phi{\bf u}/2$ 编码了旋转轴 ${\bf u}$ 与旋转角度的一半 $\theta=\phi/2$ 。关于这个半角特性，我们将在 2.4.5、2.4.6 和 2.8 节详细解释。目前只需说明：由于旋转作用是通过双乘积 ${\bf x}^{\prime}={\bf q}\otimes{\bf x}\otimes{\bf q}^{*}$ 实现的，向量 ${\bf x}$ 经历的旋转是 ${\bf q}$ 编码角度的"两倍"，或者说四元数 ${\bf q}$ 编码了对 ${\bf x}$ 预期旋转的"一半"。

为了直接表达角轴旋转参数 ${\bm{\phi}}=\phi{\bf u}\in{\mathbb{R}}^{3}$ 与四元数之间的关系，我们定义了一个大写的指数映射版本，该映射捕捉了半角效应（见图 3），

\displaystyle\operatorname*{Exp}:{\bf R}^{3}\to S^{3}\leavevmode\nobreak\ ;\leavevmode\nobreak\ {\bm{\phi}}\mapsto\operatorname*{Exp}({\bm{\phi}})=e^{{\bm{\phi}}/2}

(97)

其与指数映射的关系是显而易见的，

\displaystyle\operatorname*{Exp}({\bm{\phi}})\triangleq\exp({\bm{\phi}}/2)\leavevmode\nobreak\ .

(98)

引入角速度向量 ${\bm{\omega}}=2{\bm{\Omega}}\in{\mathbb{R}}^{3}$ 也很方便，这样(93)和(94)就变为，

	$\displaystyle\dot{\bf q}$	$\displaystyle=\frac{1}{2}{\bf q}\otimes{\bm{\omega}}$		(99)
	$\displaystyle{\bf q}$	$\displaystyle=e^{{\bm{\omega}}t/2}\leavevmode\nobreak\ .$		(100)

2.4.3 四元数与旋转向量

设 ${\bm{\phi}}=\phi{\bf u}$ 为表示绕轴 ${\bf u}$ 旋转 $\phi$ 弧度的旋转向量。那么，可以使用欧拉公式的扩展来展开指数映射（完整推导参见(37–42)），

\displaystyle\boxed{{\bf q}\triangleq\operatorname*{Exp}(\phi{\bf u})=e^{\phi{\bf u}/2}=\cos\frac{\phi}{2}+{\bf u}\sin\frac{\phi}{2}=\begin{bmatrix}\cos(\phi/2)\\ {\bf u}\sin(\phi/2)\end{bmatrix}}\leavevmode\nobreak\ .

(101)

我们称此为旋转向量到四元数的转换公式，本文档中将表示为 ${\bf q}={\bf q}\{{\bm{\phi}}\}\triangleq\operatorname*{Exp}({\bm{\phi}})$ .

2.4.4 对数映射

我们定义对数映射为指数映射的逆运算，

\displaystyle\log:S^{3}\to{\mathbb{H}}_{p}\leavevmode\nobreak\ ;\leavevmode\nobreak\ {\bf q}\mapsto\log({\bf q})={\bf u}\theta\leavevmode\nobreak\ ,

(102)

这显然就是我们在第 1.3.6 节中给出的四元数对数定义。

我们还定义了大写对数映射，它能直接在笛卡尔三维空间中提供旋转角度 $\phi$ 和旋转轴 ${\bf u}$ ，

\displaystyle\operatorname*{Log}:S^{3}\to{\mathbb{R}}^{3}\leavevmode\nobreak\ ;\leavevmode\nobreak\ {\bf q}\mapsto\operatorname*{Log}({\bf q})={\bf u}\phi\leavevmode\nobreak\ .

(103)

它与对数映射的关系是简单的，

\displaystyle\operatorname*{Log}({\bf q})\triangleq 2\log({\bf q})\leavevmode\nobreak\ .

(104)

在实现过程中，我们采用了 $\arctan(y,x)$ 的四象限版本。根据(101)，


$\displaystyle\phi$	$\displaystyle=2\arctan({\left\\|{\bf q}_{v}\right\\|},q_{w})$	(105a)
$\displaystyle{\bf u}$	$\displaystyle={\bf q}_{v}/{\left\\|{\bf q}_{v}\right\\|}\leavevmode\nobreak\ .$	(105b)

对于小角度四元数，(105b)会出现发散现象。此时我们采用 $\arctan()$ 函数的截断泰勒级数展开，得到：

\operatorname*{Log}({\bf q})=\theta{\bf u}\approx 2\,\frac{{\bf q}_{v}}{q_{w}}\left(1-\frac{{\left\|{\bf q}_{v}\right\|}^{2}}{3q_{w}^{2}}\right)\leavevmode\nobreak\ .

(106)

2.4.5 旋转作用

我们最终得以证明使用四元数进行向量旋转的假设 (86)，从而验证了迄今为止提出的所有理论内容。将向量 ${\bf x}$ 绕轴 ${\bf u}$ 旋转角度 $\phi$ 的操作通过双四元数乘积（又称三明治积）实现：

\displaystyle{\bf x}^{\prime}={\bf q}\otimes{\bf x}\otimes{\bf q}^{*}\leavevmode\nobreak\ ,

(107)

其中 ${\bf q}=\operatorname*{Exp}({\bf u}\phi)$ ，且向量 ${\bf x}$ 已表示为四元数形式，即

\displaystyle{\bf x}=xi+yj+zk=\begin{bmatrix}0\\ {\bf x}\end{bmatrix}\in{\mathbb{H}}_{p}\leavevmode\nobreak\ .

(108)

为证明该双乘积确实实现所需的向量旋转，我们运用(13)、(101)及基本向量与三角恒等式，将(107)展开如下：

\displaystyle\begin{split}{\bf x}^{\prime}&={\bf q}\otimes{\bf x}\otimes{\bf q}^{*}\\ &=\Big{(}\cos\frac{\phi}{2}+{\bf u}\sin\frac{\phi}{2}\Big{)}\otimes(0+{\bf x})\otimes\Big{(}\cos\frac{\phi}{2}-{\bf u}\sin\frac{\phi}{2}\Big{)}\\ &={\bf x}\cos^{2}\frac{\phi}{2}+({\bf u}\otimes{\bf x}-{\bf x}\otimes{\bf u})\sin\frac{\phi}{2}\cos\frac{\phi}{2}-{\bf u}\otimes{\bf x}\otimes{\bf u}\sin^{2}\frac{\phi}{2}\\ &={\bf x}\cos^{2}\frac{\phi}{2}+2({\bf u}\!\times\!{\bf x})\sin\frac{\phi}{2}\cos\frac{\phi}{2}-({\bf x}({\bf u}^{\top}{\bf u})-2{\bf u}({\bf u}^{\top}{\bf x}))\sin^{2}\frac{\phi}{2}\\ &={\bf x}(\cos^{2}\frac{\phi}{2}-\sin^{2}\frac{\phi}{2})+({\bf u}\!\times\!{\bf x})(2\sin\frac{\phi}{2}\cos\frac{\phi}{2})+{\bf u}({\bf u}^{\top}{\bf x})(2\sin^{2}\frac{\phi}{2})\\ &={\bf x}\cos\phi+({\bf u}\!\times\!{\bf x})\sin\phi+{\bf u}({\bf u}^{\top}{\bf x})(1-\cos\phi)\\ &=({\bf x}-{\bf u}\,{\bf u}^{\top}{\bf x})\cos\phi+({\bf u}\!\times\!{\bf x})\sin\phi+{\bf u}\,{\bf u}^{\top}{\bf x}\\ &={\bf x}_{\bot}\cos\phi+({\bf u}\!\times\!{\bf x})\sin\phi+{\bf x}_{||}\leavevmode\nobreak\ ,\end{split}

(109)

这正是向量旋转公式(54)。

2.4.6 $SO(3)$ 流形的双重覆盖

考虑单位四元数 ${\bf q}$ 。当视为常规4维向量时， ${\bf q}$ 与表示方向原点的单位四元数 ${\bf q}_{1}=[1,0,0,0]$ 之间的夹角 $\theta$ 为，

\displaystyle\cos\theta={\bf q}_{1}^{\top}{\bf q}={\bf q}(1)=q_{w}\leavevmode\nobreak\ .

(110)

同时，四元数 ${\bf q}$ 在三维空间中对物体旋转的角度 $\phi$ 满足

\displaystyle{\bf q}=\begin{bmatrix}q_{w}\\ {\bf q}_{v}\end{bmatrix}=\begin{bmatrix}\cos\phi/2\\ {\bf u}\sin\phi/2\end{bmatrix}\leavevmode\nobreak\ .

(111)

这意味着我们有 $q_{w}=\cos\theta=\cos\phi/2$ ，因此四元数向量与四维空间中恒等元之间的夹角，等于该四元数在三维空间中所旋转角度的一半，

\displaystyle\theta=\phi/2\leavevmode\nobreak\ .

(112)

我们在图 4 中展示了这种双重覆盖现象。当两个四元数向量之间的夹角达到 $\theta=\pi/2$ 时，三维旋转已完成 $\phi=\pi$ （即半周旋转）；而当四元数向量完成半周旋转 $\theta=\pi$ 时，三维旋转已实现完整一周旋转。四元数向量的后半周旋转 $\pi<\theta<2\pi$ 对应着三维旋转的第二个完整周期 $2\pi<\phi<4\pi$ ，即旋转流形的第二次覆盖。

2.5 旋转矩阵与四元数

正如我们所见，给定旋转向量 ${\bm{\phi}}={\bf u}\,\phi$ ，其指数映射单位四元数和旋转矩阵产生旋转算子 ${\bf q}=\operatorname*{Exp}({\bf u}\,\phi)$ 并且 ${\bf R}=\operatorname*{Exp}({\bf u}\,\phi)$ 它们以相同的角度 $\phi$ 绕同一轴 ${\bf u}$ 旋转向量 ${\bf x}$ 。 ¹⁰¹⁰10The obvious notation ambiguity between the exponential maps ${\bf R}=\operatorname*{Exp}({\bm{\phi}})$ and ${\bf q}=\operatorname*{Exp}({\bm{\phi}})$ is easily resolved by the context: at occasions it is just the type of the returned value, ${\bf R}$ or ${\bf q}$ ; other times it is the presence or absence of the quaternion product $\otimes$ . 也就是说，如果

\displaystyle\forall{\bm{\phi}},{\bf x}\in{\mathbb{R}}^{3},\leavevmode\nobreak\ {\bf q}=\operatorname*{Exp}({\bm{\phi}}),\leavevmode\nobreak\ {\bf R}=\operatorname*{Exp}({\bm{\phi}})

(113)

那么，

\displaystyle{\bf q}\otimes{\bf x}\otimes{\bf q}^{*}={\bf R}\,{\bf x}\leavevmode\nobreak\ .

(114)

由于该恒等式两边对 ${\bf x}$ 呈线性关系，通过展开左侧并匹配右侧项，可得到与四元数等价的旋转矩阵表达式，即四元数转旋转矩阵公式：

\displaystyle\boxed{{\bf R}=\begin{bmatrix}q_{w}^{2}+q_{x}^{2}-q_{y}^{2}-q_{z}^{2}&2(q_{x}q_{y}-q_{w}q_{z})&2(q_{x}q_{z}+q_{w}q_{y})\\ 2(q_{x}q_{y}+q_{w}q_{z})&q_{w}^{2}-q_{x}^{2}+q_{y}^{2}-q_{z}^{2}&2(q_{y}q_{z}-q_{w}q_{x})\\ 2(q_{x}q_{z}-q_{w}q_{y})&2(q_{y}q_{z}+q_{w}q_{x})&q_{w}^{2}-q_{x}^{2}-q_{y}^{2}+q_{z}^{2}\end{bmatrix}}\leavevmode\nobreak\ ,

(115)

本文中以 ${\bf R}={\bf R}\{{\bf q}\}$ 表示。四元数乘积的矩阵形式（17–19）提供了另一种计算公式，因为

\displaystyle{\bf q}\otimes{\bf x}\otimes{\bf q}^{*}

\displaystyle=[{\bf q}^{*}]_{R}\,[{\bf q}]_{L}\begin{bmatrix}0\\ {\bf x}\end{bmatrix}=\begin{bmatrix}0\\ {\bf R}\,{\bf x}\end{bmatrix}\leavevmode\nobreak\ ,

(116)

经过一些简单推导后可得

\displaystyle\boxed{{\bf R}=(q_{w}^{2}-{\bf q}_{v}^{\top}{\bf q}_{v})\,{\bf I}+2\,{\bf q}_{v}{\bf q}_{v}^{\top}+2\,q_{w}\left[{\bf q}_{v}\right]_{\times}}\leavevmode\nobreak\ .

(117)

旋转矩阵 ${\bf R}$ 相对于四元数具有以下性质：

$\displaystyle{\bf R}\{[1,0,0,0]^{\top}\}$	$\displaystyle={\bf I}$	(118)
$\displaystyle{\bf R}\{-{\bf q}\}$	$\displaystyle={\bf R}\{{\bf q}\}$	(119)
$\displaystyle{\bf R}\{{\bf q}^{*}\}$	$\displaystyle={\bf R}\{{\bf q}\}^{\top}$	(120)
$\displaystyle{\bf R}\{{\bf q}_{1}\otimes{\bf q}_{2}\}$	$\displaystyle={\bf R}\{{\bf q}_{1}\}{\bf R}\{{\bf q}_{2}\}\leavevmode\nobreak\ ,$	(121)

其中我们观察到： (118) 单位四元数表示零旋转；(119) 四元数与其负数表示相同旋转，构成 $SO(3)$ 的双重覆盖；(120) 共轭四元数表示逆旋转；且(121) 四元数乘积按与旋转矩阵相同的顺序组合连续旋转。

此外，我们有以下性质

\displaystyle{\bf R}\{{\bf q}^{t}\}={\bf R}\{{\bf q}\}^{t}\leavevmode\nobreak\ ,

(122)

该关系描述了四元数与旋转矩阵在运行标量 $t$ 上的球面插值。

2.6 旋转合成

四元数组合的操作方式与旋转矩阵类似，即通过适当的四元数和矩阵乘积，并保持相同顺序（图 5），

\displaystyle{\bf q}_{\mathcal{A}\mathcal{C}}

\displaystyle={\bf q}_{\mathcal{A}\mathcal{B}}\otimes{\bf q}_{\mathcal{B}\mathcal{C}}\leavevmode\nobreak\ ,

\displaystyle{\bf R}_{\mathcal{A}\mathcal{C}}

\displaystyle={\bf R}_{\mathcal{A}\mathcal{B}}\,{\bf R}_{\mathcal{B}\mathcal{C}}\leavevmode\nobreak\ .

(123)

这直接源于所涉及乘积的结合律性质，

$\displaystyle{\bf x}_{\mathcal{A}}$	$\displaystyle={\bf q}_{\mathcal{A}\mathcal{B}}\otimes{\bf x}_{\mathcal{B}}\otimes{\bf q}_{\mathcal{A}\mathcal{B}}^{*}$	$\displaystyle{\bf x}_{\mathcal{A}}$	$\displaystyle={\bf R}_{\mathcal{A}\mathcal{B}}\,{\bf x}_{\mathcal{B}}$
	$\displaystyle={\bf q}_{\mathcal{A}\mathcal{B}}\otimes({\bf q}_{\mathcal{B}\mathcal{C}}\otimes{\bf x}_{\mathcal{C}}\otimes{\bf q}_{\mathcal{B}\mathcal{C}}^{})\otimes{\bf q}_{\mathcal{A}\mathcal{B}}^{}$		$\displaystyle={\bf R}_{\mathcal{A}\mathcal{B}}\,({\bf R}_{\mathcal{B}\mathcal{C}}\,{\bf x}_{\mathcal{C}})$
	$\displaystyle=({\bf q}_{\mathcal{A}\mathcal{B}}\otimes{\bf q}_{\mathcal{B}\mathcal{C}})\otimes{\bf x}_{\mathcal{C}}\otimes({\bf q}_{\mathcal{B}\mathcal{C}}^{}\otimes{\bf q}_{\mathcal{A}\mathcal{B}}^{})$		$\displaystyle=({\bf R}_{\mathcal{A}\mathcal{B}}\,{\bf R}_{\mathcal{B}\mathcal{C}})\,{\bf x}_{\mathcal{C}}$
	$\displaystyle=({\bf q}_{\mathcal{A}\mathcal{B}}\otimes{\bf q}_{\mathcal{B}\mathcal{C}})\otimes{\bf x}_{\mathcal{C}}\otimes({\bf q}_{\mathcal{A}\mathcal{B}}\otimes{\bf q}_{\mathcal{B}\mathcal{C}})^{*}$		$\displaystyle={\bf R}_{\mathcal{A}\mathcal{C}}\,{\bf x}_{\mathcal{C}}\leavevmode\nobreak\ .$
	$\displaystyle={\bf q}_{\mathcal{A}\mathcal{C}}\otimes{\bf x}_{\mathcal{C}}\otimes{\bf q}_{\mathcal{A}\mathcal{C}}^{*}\leavevmode\nobreak\ ,$

关于符号表示的说明

恰当的符号表示有助于确定复合运算中因子的正确顺序，特别是对于多次旋转的复合运算（参见图 5）。例如，令 ${\bf q}_{ji}$ （相应地 ${\bf R}_{ji}$ ）表示从状态 $i$ 到状态 $j$ 的旋转，即 ${\bf x}_{j}={\bf q}_{ji}\otimes{\bf x}_{i}\otimes{\bf q}_{ji}^{*}$ （相应地 ${\bf x}_{j}={\bf R}_{ji}{\bf x}_{i}$ ）。那么给定由四元数 ${\bf q}_{OA},{\bf q}_{AB},{\bf q}_{BC},{\bf q}_{OX},{\bf q}_{XZ}$ 表示的一系列旋转时，我们只需将索引串联起来即可得到：

\displaystyle{\bf q}_{OC}

\displaystyle={\bf q}_{OA}\otimes{\bf q}_{AB}\otimes{\bf q}_{BC}

\displaystyle{\bf R}_{OC}

\displaystyle={\bf R}_{OA}\,{\bf R}_{AB}\,{\bf R}_{BC}\leavevmode\nobreak\ ,

且已知逆向旋转对应于共轭运算 ${\bf q}_{ji}={\bf q}_{ij}^{*}$ 或转置运算 ${\bf R}_{ji}={\bf R}_{ij}^{\top}$ ，因此还可得出：

	$\displaystyle{\bf q}_{ZA}$	$\displaystyle={\bf q}_{XZ}^{}\otimes{\bf q}_{OX}^{}\otimes{\bf q}_{OA}$	$\displaystyle{\bf R}_{ZA}$	$\displaystyle={\bf R}_{XZ}^{\top}\,{\bf R}_{OX}^{\top}\,{\bf R}_{OA}$
		$\displaystyle={\bf q}_{ZX}\otimes{\bf q}_{XO}\otimes{\bf q}_{OA}$		$\displaystyle={\bf R}_{ZX}\,{\bf R}_{XO}\,{\bf R}_{OA}\leavevmode\nobreak\ .$

2.7 球面线性插值(SLERP)

四元数非常适合用于计算精确的姿态插值。给定由四元数 ${\bf q}_{0}$ 和 ${\bf q}_{1}$ 表示的两种姿态，我们希望找到一个四元数函数 ${\bf q}(t),\leavevmode\nobreak\ t\in[0,1]$ ，能够实现从 ${\bf q}(0)={\bf q}_{0}$ 到 ${\bf q}(1)={\bf q}_{1}$ 的线性插值。这种插值的特点是：当 $t$ 从 $0$ 变化到 $1$ 时，物体会以恒定角速度绕固定轴，从姿态 ${\bf q}_{0}$ 连续旋转至姿态 ${\bf q}_{1}$ 。

方法一

第一种方法采用四元数代数，并遵循文献 ${\mathbb{R}}^{3}$ 中的几何推理，这应与目前所述内容易于关联。首先，计算从 ${\bf q}_{0}$ 到 ${\bf q}_{1}$ 的姿态增量 $\Delta{\bf q}$ ，使得 ${\bf q}_{1}={\bf q}_{0}\otimes\Delta{\bf q}$ ，

\displaystyle\Delta{\bf q}={\bf q}_{0}^{*}\otimes{\bf q}_{1}\leavevmode\nobreak\ .

(124)

随后利用对数映射 ¹¹¹¹11We can use here either the maps $\log()$ and $\exp()$ , or their capitalized forms $\operatorname*{Log}()$ and $\operatorname*{Exp}()$ . The involved factor 2 in the resulting angles is finally irrelevant as it cancels out in the final formula. 求得对应的旋转向量 $\Delta{\bm{\phi}}={\bf u}\Delta\phi$ ，

\displaystyle{\bf u}\,\Delta\phi=\operatorname*{Log}(\Delta{\bf q})\leavevmode\nobreak\ .

(125)

最后保持旋转轴 ${\bf u}$ 不变，取旋转角度 $\delta\phi=t\Delta\phi$ 的线性分数部分。通过指数映射 $\delta{\bf q}=\operatorname*{Exp}({\bf u}\,\delta\phi)$ 将其转化为四元数形式，并与原始四元数组合得到插值结果，

\displaystyle{\bf q}(t)={\bf q}_{0}\otimes\operatorname*{Exp}(t\,{\bf u}\,\Delta\phi)\leavevmode\nobreak\ .

(126)

整个过程可表示为 ${\bf q}(t)={\bf q}_{0}\otimes\operatorname*{Exp}(t\operatorname*{Log}({\bf q}_{0}^{*}\otimes{\bf q}_{1}))$ ，简化为

\displaystyle\boxed{{\bf q}(t)={\bf q}_{0}\otimes({\bf q}_{0}^{*}\otimes{\bf q}_{1})^{t}}\leavevmode\nobreak\ ,

(127)

并且通常（参见 53）被实现为：

\displaystyle{\bf q}(t)={\bf q}_{0}\otimes\begin{bmatrix}\cos(t\,\Delta\phi/2)\\ {\bf u}\sin(t\,\Delta\phi/2)\end{bmatrix}\leavevmode\nobreak\ .

(128)

注：

类似方法可用于定义旋转矩阵的 Slerp 插值，得到

\displaystyle{\bf R}(t)={\bf R}_{0}\operatorname*{Exp}(t\operatorname*{Log}({\bf R}_{0}^{\top}{\bf R}_{1}))={\bf R}_{0}({\bf R}_{0}^{\top}{\bf R}_{1})^{t}\leavevmode\nobreak\ ,

(129)

其中矩阵指数 ${\bf R}^{t}$ 可采用罗德里格斯公式（77）实现，从而得到：

\displaystyle{\bf R}(t)={\bf R}_{0}\left({\bf I}+\sin(t\Delta\phi)\left[{\bf u}\right]_{\times}+(1-\cos(t\Delta\phi))\left[{\bf u}\right]_{\times}^{2}\right)\leavevmode\nobreak\ .

(130)

方法二

还可开发其他独立于四元数代数内部结构、甚至独立于弧线嵌入空间维度的 Slerp 方法。特别参见图 6，我们可以将四元数 ${\bf q}_{0}$ 和 ${\bf q}_{1}$ 视为单位球面上的两个单位向量，并在同一空间中进行插值。插值结果 ${\bf q}(t)$ 是以恒定角速度沿连接 ${\bf q}_{0}$ 到 ${\bf q}_{1}$ 的最短球面路径的单位向量。该路径是通过单位球面与由 ${\bf q}_{0}$ 、 ${\bf q}_{1}$ 及原点定义的平面（图中虚线圆周）相交形成的平面弧线。关于这些方法与前述等价性的证明，参见 Dam 等人（1998）。

第一种方法采用向量代数，直接遵循上述思路。将 ${\bf q}_{0}$ 和 ${\bf q}_{1}$ 视为两个单位向量；它们之间的夹角 ¹²¹²12The angle $\Delta\theta=\arccos({\bf q}_{0}^{\top}{\bf q}_{1})$ is the angle between the two quaternion vectors in Euclidean 4-space, not the real rotated angle in 3D space, which from (125) is $\Delta\phi={\left\|\operatorname*{Log}({\bf q}_{0}^{*}\otimes{\bf q}_{1})\right\|}$ . See Section 2.4.6 for further details. 可通过标量积导出，

\displaystyle\cos(\Delta\theta)

\displaystyle={\bf q}_{0}^{\top}{\bf q}_{1}

\displaystyle\Delta\theta

\displaystyle=\arccos({\bf q}_{0}^{\top}{\bf q}_{1})\leavevmode\nobreak\ .

(131)

我们按如下步骤进行。我们确定旋转平面（此处记为 $\pi$ ），并构建其标准正交基 $\{{\bf q}_{0},{\bf q}_{\bot}\}$ ，其中 ${\bf q}_{\bot}$ 是通过将 ${\bf q}_{1}$ 相对于 ${\bf q}_{0}$ 进行正交归一化得到的，

\displaystyle{\bf q}_{\bot}

\displaystyle=\frac{{\bf q}_{1}-({\bf q}_{0}^{\top}{\bf q}_{1}){\bf q}_{0}}{{\left\|{\bf q}_{1}-({\bf q}_{0}^{\top}{\bf q}_{1}){\bf q}_{0}\right\|}}\leavevmode\nobreak\ ,

(132)

因此（参见图 6 - 右）

\displaystyle{\bf q}_{1}={\bf q}_{0}\cos\Delta\theta+{\bf q}_{\bot}\sin\Delta\theta\leavevmode\nobreak\ .

(133)

那么，我们只需在平面 $\pi$ 上将 ${\bf q}_{0}$ 旋转角度 $t\Delta\theta$ 的一部分，即可得到球面插值，

\displaystyle\boxed{{\bf q}(t)={\bf q}_{0}\cos(t\Delta\theta)+{\bf q}_{\bot}\sin(t\Delta\theta)}\leavevmode\nobreak\ .

(134)

方法三

一种类似的方法归功于 Glenn Davis 在 Shoemake（1985）中的工作，其原理在于：连接 ${\bf q}_{0}$ 与 ${\bf q}_{1}$ 的大圆弧上任意点都必须是这两个端点的线性组合（因为这三个向量共面）。通过公式（131）计算出角度 $\Delta\theta$ 后，我们可以从（133）中分离出 ${\bf q}_{\bot}$ 并代入（134）。应用恒等式 $\sin(\Delta\theta-t\Delta\theta)=\sin\Delta\theta\cos t\Delta\theta-\cos\Delta\theta\sin t\Delta\theta$ 后，即可得到 Davis 公式（另类推导可参阅 Eberly（2010））。

\displaystyle\boxed{{\bf q}(t)={\bf q}_{0}\frac{\sin((1-t)\Delta\theta)}{\sin(\Delta\theta)}+{\bf q}_{1}\frac{\sin(t\Delta\theta)}{\sin(\Delta\theta)}}\leavevmode\nobreak\ .

(135)

该公式具有对称性的优势：定义反向插值器 $s=1-t$ 即可得到

\displaystyle{\bf q}(s)={\bf q}_{1}\frac{\sin((1-s)\Delta\theta)}{\sin(\Delta\theta)}+{\bf q}_{0}\frac{\sin(s\Delta\theta)}{\sin(\Delta\theta)}\leavevmode\nobreak\ .

这与交换 ${\bf q}_{0}$ 和 ${\bf q}_{1}$ 角色后的公式完全相同。

所有这些基于四元数的 SLERP 方法都需要注意确保沿最短路径进行适当插值，即旋转角度 $\phi\leq\pi$ 。由于四元数对 $SO(3)$ 的双重覆盖特性（参见第 2.4.6 节），只有当四元数间呈锐角 $\Delta\theta\leq\pi/2$ 时插值才会沿最短路径进行（图 7）。检测和解决这种情况很简单：若 $\cos(\Delta\theta)={\bf q}_{0}^{\top}{\bf q}_{1}<0$ ，则将例如 ${\bf q}_{1}$ 替换为 $-{\bf q}_{1}$ 并重新计算。

2.8 四元数与等斜旋转：揭秘其神奇之处

本节将为关于四元数的两个引人入胜的问题提供几何解释，我们称之为"魔法"：

•

为什么乘积 ${\bf q}\otimes{\bf x}\otimes{\bf q}^{*}$ 会使向量 ${\bf x}$ 发生旋转？
•

为何在通过 ${\bf q}=e^{{\bm{\phi}}/2}=[\cos\phi/2,{\bf u}\sin\phi/2]$ 构造四元数时需要考虑半角？

我们需要一个几何解释，即超越代数证明（109）和第 2.4.6 节中双覆盖事实的合理解释。

首先，让我们在此重述方程（116），该方程通过四元数乘积矩阵 $[{\bf q}]_{L}$ 和 $[{\bf q}^{*}]_{R}$ （定义见 19）表达了四元数的旋转作用。

\displaystyle{\bf q}\otimes{\bf x}\otimes{\bf q}^{*}

\displaystyle=[{\bf q}^{*}]_{R}\,[{\bf q}]_{L}\begin{bmatrix}0\\ {\bf x}\end{bmatrix}=\begin{bmatrix}0\\ {\bf R}\,{\bf x}\end{bmatrix}\leavevmode\nobreak\ .

对于单位四元数 ${\bf q}$ ，其四元数乘积矩阵 $[{\bf q}]_{L}$ 和 $[{\bf q}^{*}]_{R}$ 满足两个显著特性，

	$\displaystyle[{\bf q}]\,[{\bf q}]^{\top}$	$\displaystyle={\bf I}_{4}$		(136)
	$\displaystyle\det([{\bf q}])$	$\displaystyle=+1\leavevmode\nobreak\ ,$		(137)

因此它们属于 $SO(4)$ 的元素，即 ${\mathbb{R}}^{4}$ 空间中的正规旋转矩阵。更具体地说，它们代表了一种特殊类型的旋转，称为等斜旋转 ，我们将在下文解释。因此，根据(116)，一个四元数旋转对应于 ${\mathbb{R}}^{4}$ 中两个串联的等斜旋转。

为了阐释四元数旋转的深层原理，我们需要理解 ${\mathbb{R}}^{4}$ 中的等斜旋转。为此，我们首先需要理解 ${\mathbb{R}}^{4}$ 中的一般旋转。而要理解 ${\mathbb{R}}^{4}$ 中的旋转，我们需要回溯到 ${\mathbb{R}}^{3}$ ——其旋转本质上是平面旋转。让我们逐步解析这些概念。

${\mathbb{R}}^{3}$ 中的旋转：

在 ${\mathbb{R}}^{3}$ 中，我们考虑向量 ${\bf x}$ 绕任意轴 ${\bf u}$ 的旋转——参见图 8，并回顾图 1。旋转时，平行于旋转轴 ${\bf u}$ 的向量保持静止，而垂直于旋转轴的向量则在垂直于该轴的平面 $\pi$ 内旋转。对于一般向量 ${\bf x}$ ，其在该平面内的两个分量会进行旋转，而轴向分量则保持静止。

${\mathbb{R}}^{4}$ 中的旋转：

在 ${\mathbb{R}}^{4}$ 中（参见图 9），由于额外的维度， ${\mathbb{R}}^{3}$ 中的一维旋转轴变成了一个新的二维平面。

这第二个平面为第二次旋转提供了空间。事实上，四元数空间中的旋转包含了四维空间中两个正交平面上的独立旋转。这意味着每个平面上的四维向量仅在其所属平面内旋转，且针对某一平面的旋转不会影响向量在另一正交平面上的分量。因此，这些平面被称为 “不变平面”。

${\mathbb{R}}^{4}$ 中的等斜旋转：

等斜旋转（源自希腊语，iso:“相等”，klinein:“倾斜”）是指 ${\mathbb{R}}^{4}$ 中两个不变平面旋转角度大小相等的旋转。当两个角度符号相同时， ¹³¹³13Given the two invariant planes, we arbitrarily select their orientations so that we can associate positive and negative rotation angles in them. 我们称之为左等斜旋转 ；当符号相反时，则称为右等斜旋转 。等斜旋转的一个显著特性（我们已在式 22 中见过）是：左、右等斜旋转具有可交换性。

\displaystyle[{\bf p}]_{R}\,[{\bf q}]_{L}=[{\bf q}]_{L}\,[{\bf p}]_{R}\leavevmode\nobreak\ .

(138)

${\mathbb{R}}^{4}$ 与 ${\mathbb{R}}^{3}$ 中的四元数旋转：

给定单位四元数 ${\bf q}=e^{{\bf u}\,\theta/2}$ （表示绕轴 ${\bf u}$ 旋转角度 $\theta$ 的 ${\mathbb{R}}^{3}$ 空间旋转），矩阵 $[{\bf q}]_{L}$ 是 ${\mathbb{R}}^{4}$ 空间中对应于左乘四元数 ${\bf q}$ 的左等斜旋转，而 $[{\bf q}^{*}]_{R}$ 则是对应于右乘四元数 ${\bf q}^{*}$ 的右等斜旋转。这些等斜旋转的角度大小精确为 $\theta/2$ 和 ¹⁴¹⁴14This can be checked by extracting the eigenvalues of the isoclinic rotation matrices: they are formed by pairs of conjugate complex numbers with a phase equal to $\mathbin{\leavevmode\hbox to6.46pt{\vbox to6.46pt{\pgfpicture\makeatletter\hbox{\hskip 0.21527pt\lower-0.21527pt\hbox to0.0pt{\pgfsys@beginscope\pgfsys@invoke{ }\definecolor{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@rgb@stroke{0}{0}{0}\pgfsys@invoke{ }\pgfsys@color@rgb@fill{0}{0}{0}\pgfsys@invoke{ }\pgfsys@setlinewidth{0.4pt}\pgfsys@invoke{ }\nullfont\hbox to0.0pt{\pgfsys@beginscope\pgfsys@invoke{ }{}{}\pgfsys@setlinewidth{0.43056pt}\pgfsys@invoke{ }{}{{}}{} {}{}{}{{}}{} {}{}{}{{}}{} {}{}{}\pgfsys@moveto{0.0pt}{0.0pt}\pgfsys@lineto{6.02776pt}{0.0pt}\pgfsys@moveto{3.01387pt}{0.6028pt}\pgfsys@lineto{3.01387pt}{6.02776pt}\pgfsys@moveto{0.0pt}{3.31528pt}\pgfsys@lineto{6.02776pt}{3.31528pt}\pgfsys@stroke\pgfsys@invoke{ } \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope{}{}{}\hss}\pgfsys@discardpath\pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope\hss}}\lxSVG@closescope\endpgfpicture}}}\theta/2$ . 。且不变平面相同。那么，旋转表达式（116），在此再次重现，

\displaystyle\begin{bmatrix}0\\ {\bf x}^{\prime}\end{bmatrix}={\bf q}\otimes{\bf x}\otimes{\bf q}^{*}

\displaystyle=[{\bf q}^{*}]_{R}\,[{\bf q}]_{L}\begin{bmatrix}0\\ {\bf x}\end{bmatrix}\leavevmode\nobreak\ ,

表示对 4-向量 $(0,{\bf x})^{\top}$ 进行两次链式等斜旋转，一次左旋一次右旋，每次旋转 ${\mathbb{R}}^{3}$ 中所需旋转角度的一半。在 ${\mathbb{R}}^{4}$ 的某个不变平面内（见图 10），这两个半角因符号相反而相互抵消。

在另一个平面中，它们叠加产生总旋转角度 $\theta$ 。若根据（116）定义所得旋转矩阵 ${\bf R}_{4}$ ，可轻易发现（另见（138）），

\displaystyle{\bf R}_{4}\triangleq[{\bf q}^{*}]_{R}[{\bf q}]_{L}=[{\bf q}]_{L}[{\bf q}^{*}]_{R}=\begin{bmatrix}1&\bf 0\\ \bf 0&{\bf R}\end{bmatrix}\leavevmode\nobreak\ ,

(139)

其中 ${\bf R}$ 是 ${\mathbb{R}}^{3}$ 中的旋转矩阵，它显然旋转了 ${\mathbb{R}}^{4}$ 的 ${\mathbb{R}}^{3}$ 子空间中的向量，同时保持第四维度不变。

本文讨论的内容有些超出了当前文档的范围。此外，这一论述并不完整，因为它除了在(139)中给出的结果外，并未提供为何需要进行 ${\bf q}\otimes{\bf x}\otimes{\bf q}^{*}$ 而非例如 ${\bf q}\otimes{\bf x}\otimes{\bf q}$ 的直观解释或几何说明。 ¹⁵¹⁵15Let it suffice to say that ${\bf q}\otimes{\bf x}\otimes{\bf q}^{*}$ works for rotations if ${\bf q}$ is a unit quaternion. In fact, the product ${\bf q}_{v}\otimes{\bf x}\otimes{\bf q}_{v}$ produces reflections (not rotations!) in ${\mathbb{R}}^{3}$ if ${\bf q}_{v}$ is a unit pure quaternion. Finally, the product ${\bf q}\otimes{\bf x}\otimes{\bf q}$ , with ${\bf q}$ a unit non-pure quaternion, exhibits no remarkable properties. 我们在此处包含这一内容，仅作为提供另一种解释四元数旋转方式的手段，希望读者能对其机制有更直观的理解。感兴趣的读者建议参阅 ${\mathbb{R}}^{4}$ 中关于等斜旋转的相关文献。

3 四元数约定：我的选择

3.1 四元数的不同变体

确定四元数的方法有多种，主要涉及以下四个二元选择：

•

其元素的排列顺序——实部在前或实部在后：

\displaystyle{\bf q}=\begin{bmatrix}q_{w}\\ {\bf q}_{v}\end{bmatrix}\qquad\textit{vs.}\qquad{\bf q}=\begin{bmatrix}{\bf q}_{v}\\ q_{w}\end{bmatrix}\leavevmode\nobreak\ .

(140)

•

四元数代数的乘法公式定义：
这意味着，给定一个旋转轴𝐮，一个四元数𝐪_right{𝐮θ}使用右手法则将向量绕𝐮旋转θ角度，而另一个四元数𝐪_left{𝐮θ}则使用左手法则。
•

旋转算子的功能——旋转坐标系或旋转向量：

Passive vs. Active. (142)

•

在被动情况下，操作的方向——局部到全局或全局到局部：

\displaystyle{\bf x}_{global}={\bf q}\otimes{\bf x}_{local}\otimes{\bf q}^{*}\qquad vs.\qquad{\bf x}_{local}={\bf q}\otimes{\bf x}_{global}\otimes{\bf q}^{*}

(143)

这种选择的多样性导致了 12 种不同的组合。历史发展使得某些约定优于其他约定 (Chou, 1992; Yazell, 2009)。如今，在现有文献中，我们可以找到多种四元数变体，如 Hamilton、STS ¹⁶¹⁶16Space Transportation System, commonly known as NASA’s Space Shuttle. 、JPL ¹⁷¹⁷17Jet Propulsion Laboratory. 、ISS ¹⁸¹⁸18International Space Station. 、ESA ¹⁹¹⁹19European Space Agency. 、工程学、机器人学等，可能还有更多命名方式。其中许多形式可能是相同的，有些则不同，但这一事实很少被明确说明，许多著作对上述四种选择中其四元数的描述也往往不够充分。

这些差异以非显而易见的方式影响着旋转、组合等操作的相应公式。因此，这些公式并不兼容，我们需要从一开始就做出明确选择。

两种最常用且文献记载最完备的约定是哈密顿约定（(140–143)左侧选项）和 JPL 约定（右侧选项，(142 除外）。表 2 总结了它们的特性。JPL 约定主要应用于航空航天领域，而哈密顿约定更常见于机器人学等其他工程领域——但这并非绝对规则。

表 2：关于 4 个二元选择的哈密顿与 JPL 四元数约定对比

	四元数类型	哈密顿	JPL
1	分量顺序	$(q_{w}\,,\,{\bf q}_{v})$	$({\bf q}_{v}\,,\,q_{w})$
2	代数	$ij=k$	$ij=-k$
2	手性	右手系	左手系
3	函数	被动式	被动式
4	右乘运算表示	局部到全局	全局到局部
	默认表示法， ${\bf q}$	${\bf q}\triangleq{\bf q}_{\mathcal{G}\mathcal{L}}$	${\bf q}\triangleq{\bf q}_{\mathcal{L}\mathcal{G}}$
	默认操作	${\bf x}_{\mathcal{G}}={\bf q}\otimes{\bf x}_{\mathcal{L}}\otimes{\bf q}^{*}$	${\bf x}_{\mathcal{L}}={\bf q}\otimes{\bf x}_{\mathcal{G}}\otimes{\bf q}^{*}$

如公式(2)所示，我选择了哈密顿约定。该约定采用右手系，与机器人学中广泛使用的诸多软件库（如 Eigen、ROS、Google Ceres）兼容，也与大量基于 IMU 的姿态估计卡尔曼滤波文献一致 (Chou,, 1992; Kuipers,, 1999; Piniés 等,, 2007; Roussillon 等,, 2011; Martinelli,, 2012，以及其他众多文献)。

JPL 约定可能较少被采用，至少在机器人领域如此。该约定在 (Trawny 和 Roumeliotis，2005) 中有详尽阐述，这部参考著作的目标与范围与本文极为相近，但完全专注于 JPL 约定。JPL 四元数被用于 JPL 文献（显然如此）以及 Li、Mourikis、Roumeliotis 及其同事的关键论文中（参见例如 (Li 和 Mourikis，2012；Li 等人，2014)），这些论文都借鉴了 Trawny 和 Roumeliotis 的文档。这些著作是我们处理视觉-惯性里程计与 SLAM 问题时的主要灵感来源——这也正是我们的研究方向。

在本节剩余部分，我们将更深入地分析这两种四元数约定。

3.1.1 四元数分量的顺序

虽然并非最根本的区别，但汉密尔顿四元数与 JPL 四元数之间最显著的差异在于其分量顺序——标量部分分别位于首位（汉密尔顿约定）或末位（JPL 约定）。这种改变的影响相当明显，不应构成理解上的重大挑战。事实上，只要保持其他三个特征不变，即使某些将四元数实部置于末尾的文献（例如 C++库 Eigen）仍被视为采用汉密尔顿约定。

为提升表述清晰度，我们采用下标 $(w,x,y,z)$ 表示四元数分量，而非常用的 $(0,1,2,3)$ 标记法。当调整分量顺序时， $q_{w}$ 始终代表实部，而 $q_{0}$ 是否同样适用则并不明确——某些情况下可能遇到诸如 ${\bf q}=(q_{1},q_{2},q_{3},q_{0})$ 的表示（其中 $q_{0}$ 为实部且位于末尾），但在通用的 ${\bf q}=(q_{0},q_{1},q_{2},q_{3})$ 表达式中，位于末尾的实部应为 $q_{3}$ 。 ²⁰²⁰20See also footnote 1. 在不同约定间转换时，必须谨慎处理涉及完整 $4\times 4$ 或 $3\times 4$ 四元数相关矩阵的公式，因为它们的行和/或列需要交换位置。这操作本身并不困难，但可能难以察觉，因此容易出错。

关于分量顺序的两个有趣现象是：

•

以实部优先表示时，四元数自然可视为扩展复数，呈现为人熟知的实数+虚数形式。这种表达方式之所以令我们感到亲切，或许正源于此。
•

以实部置后表示时，采用向量形式表达的四元数 ${\bf q}=\begin{bmatrix}x,y,z,w\end{bmatrix}\in{\mathbb{H}}$ , 其格式与投影三维空间中的齐次向量完全等价， ${\bf p}=\begin{bmatrix}x,y,z,w\end{bmatrix}\in{\mathbb{P}}^{3}$ ，在这两种情况下， $x,y,z$ 均明确对应于三个笛卡尔坐标轴。在处理三维几何问题时，这种对应关系使得四元数与齐次向量的代数运算更加统一，特别是（但不仅限于）当齐次向量被约束在单位球面 ${\left\|{\bf p}\right\|}=1$ 上时。

3.1.2 四元数代数的规范定义

哈密尔顿约定定义了 $ij=k$ ，因此，

\displaystyle i^{2}=j^{2}=k^{2}=ijk=-1\leavevmode\nobreak\ ,\quad ij=-ji=k\leavevmode\nobreak\ ,\quad jk=-kj=i\leavevmode\nobreak\ ,\quad ki=-ik=j\leavevmode\nobreak\ ,

(144)

而 JPL 约定则定义了 $ji=k$ ，因此其四元数代数变为，

\displaystyle i^{2}=j^{2}=k^{2}=-ijk=-1\leavevmode\nobreak\ ,\quad-ij=ji=k\leavevmode\nobreak\ ,\quad-jk=kj=i\leavevmode\nobreak\ ,\quad-ki=ik=j\leavevmode\nobreak\ .

(145)

有趣的是，这些细微的符号变化保留了四元数作为旋转算子的基本性质。从数学上看，关键影响在于(13)式中叉积符号的改变，这导致了四元数手性的变化 (Shuster,, 1993)：哈密尔顿采用 $ij=k$ ，因此是右手系的，即遵循右手定则旋转向量；JPL 采用 $ji=k$ ，属于左手系 (Trawny and Roumeliotis,, 2005)。由于左右手系旋转符号相反，可以说它们的四元数 ${\bf q}_{left}$ 与 ${\bf q}_{right}$ 满足关系：

\displaystyle{\bf q}_{\textit{left}}={\bf q}_{\textit{right}}^{*}\leavevmode\nobreak\ .

(146)

3.1.3 旋转算子的函数

我们已经了解了如何在三维空间中旋转向量。这在 (Shuster,, 1993) 中被称为主动解释，因为操作符（这会影响所有旋转操作符）主动地旋转向量，

\displaystyle{\bf x}^{\prime}

\displaystyle={\bf q}_{\textit{active}}\otimes{\bf x}\otimes{\bf q}_{\textit{active}}^{*}\leavevmode\nobreak\ ,

\displaystyle{\bf x}^{\prime}

\displaystyle={\bf R}_{\textit{active}}\,{\bf x}\leavevmode\nobreak\ .

(147)

另一种理解 ${\bf q}$ 和 ${\bf R}$ 对向量 ${\bf x}$ 作用的方式是：假设向量保持静止，而是我们观察者的视角发生了由 ${\bf q}$ 或 ${\bf R}$ 指定的旋转。这在此处被称为坐标系变换 ，并在 (Shuster,, 1993) 中称为被动解释，因为向量本身并未移动，

\displaystyle{\bf x}_{\mathcal{B}}

\displaystyle={\bf q}_{\textit{passive}}\otimes{\bf x}_{\mathcal{A}}\otimes{\bf q}_{\textit{passive}}^{*}\leavevmode\nobreak\ ,

\displaystyle{\bf x}_{\mathcal{B}}

\displaystyle={\bf R}_{\textit{passive}}\,{\bf x}_{\mathcal{A}}\leavevmode\nobreak\ ,

(148)

其中 $\mathcal{A}$ 和 $\mathcal{B}$ 是两个笛卡尔参考坐标系， ${\bf x}_{\mathcal{A}}$ 和 ${\bf x}_{\mathcal{B}}$ 是同一向量 ${\bf x}$ 在这些坐标系中的表达。具体解释和规范表示法请参阅下文。

主动解释与被动解释由互为逆运算的算子所支配，即

\displaystyle{\bf q}_{\textit{active}}

\displaystyle={\bf q}_{\textit{passive}}^{*}\leavevmode\nobreak\ ,

\displaystyle{\bf R}_{active}

\displaystyle={\bf R}_{passive}^{\top}\leavevmode\nobreak\ .

汉密尔顿和 JPL 均采用被动约定。

方向余弦矩阵

少数作者将被动算子理解为并非旋转算子，而是一种称为方向余弦矩阵的姿态描述方式，

\displaystyle{\bf C}=\begin{bmatrix}c_{xx}&c_{xy}&c_{zx}\\ c_{xy}&c_{yy}&c_{zy}\\ c_{xz}&c_{yz}&c_{zz}\end{bmatrix}\leavevmode\nobreak\ ,

(149)

其中每个分量 $c_{ij}$ 表示源坐标系中 $i$ 轴与目标坐标系中 $j$ 轴夹角的余弦值。我们有以下恒等式，

\displaystyle{\bf C}\equiv{\bf R}_{\textit{passive}}\leavevmode\nobreak\ .

(150)

3.1.4 旋转算子的方向

在被动情况下，第二个解释来源与旋转矩阵和四元数的操作方向有关，即从局部坐标系转换到全局坐标系，还是从全局转换到局部。

给定两个笛卡尔坐标系 $\mathcal{G}$ 和 $\mathcal{L}$ ，我们将 $\mathcal{G}$ 和 $\mathcal{L}$ 分别定义为全局坐标系和局部坐标系。"全局"与"局部"是相对概念，即 $\mathcal{G}$ 相对于 $\mathcal{L}$ 为全局坐标系，而 $\mathcal{L}$ 相对于 $\mathcal{G}$ 为局部坐标系——换言之， $\mathcal{L}$ 是在参考坐标系 $\mathcal{G}$ 中定义的坐标系。 ²¹²¹21Other common denominations for the {global, local} frames are {parent, child} and {world, body}. The first one is convenient when more than two frames are involved in a system (e.g. the frames of each moving link in a humanoid robot); the second one is convenient for a solid vehicle body (e.g. a plane, a car) moving in a unique reference frame identified as the world. 我们定义 ${\bf q}_{\mathcal{G}\mathcal{L}}$ 和 ${\bf R}_{\mathcal{G}\mathcal{L}}$ 分别为将向量从坐标系 $\mathcal{L}$ 转换到坐标系 $\mathcal{G}$ 的四元数和旋转矩阵，其转换关系表现为：坐标系 $\mathcal{L}$ 中的向量 ${\bf x}_{\mathcal{L}}$ 在坐标系 $\mathcal{G}$ 中通过四元数乘积和矩阵乘积表示为

\displaystyle{\bf x}_{\mathcal{G}}

\displaystyle={\bf q}_{\mathcal{G}\mathcal{L}}\otimes{\bf x}_{\mathcal{L}}\otimes{\bf q}_{\mathcal{G}\mathcal{L}}^{*}\leavevmode\nobreak\ ,

\displaystyle{\bf x}_{\mathcal{G}}

\displaystyle={\bf R}_{\mathcal{G}\mathcal{L}}\,{\bf x}_{\mathcal{L}}\leavevmode\nobreak\ .

(151)

相反的转换（从 $\mathcal{G}$ 到 $\mathcal{L}$ ）则通过

\displaystyle{\bf x}_{\mathcal{L}}

\displaystyle={\bf q}_{\mathcal{L}\mathcal{G}}\otimes{\bf x}_{\mathcal{G}}\otimes{\bf q}_{\mathcal{L}\mathcal{G}}^{*}\leavevmode\nobreak\ ,

\displaystyle{\bf x}_{\mathcal{L}}

\displaystyle={\bf R}_{\mathcal{L}\mathcal{G}}\,{\bf x}_{\mathcal{G}}\leavevmode\nobreak\ ,

(152)

其中

\displaystyle{\bf q}_{\mathcal{L}\mathcal{G}}

\displaystyle={\bf q}_{\mathcal{G}\mathcal{L}}^{*}\leavevmode\nobreak\ ,

\displaystyle{\bf R}_{\mathcal{L}\mathcal{G}}

\displaystyle={\bf R}_{\mathcal{G}\mathcal{L}}^{\top}\leavevmode\nobreak\ .

(153)

汉密尔顿采用局部到全局作为默认规范，表示在帧 $\mathcal{G}$ 中表达的帧 $\mathcal{L}$ ，

\displaystyle{\bf q}_{\textit{Hamilton}}\triangleq{\bf q}_{[\textit{with\leavevmode\nobreak\ respect\leavevmode\nobreak\ to}][\textit{of\,}]}={\bf q}_{[\textit{to}][\textit{from}]}={\bf q}_{\mathcal{G}\mathcal{L}}\leavevmode\nobreak\ ,

(154)

而 JPL 则采用相反的全局到局部转换，

\displaystyle{\bf q}_{\textit{JPL}}\triangleq{\bf q}_{[\textit{of}\,][\textit{with\leavevmode\nobreak\ respect\leavevmode\nobreak\ to}]}={\bf q}_{[\textit{to}][\textit{from}]}={\bf q}_{\mathcal{L}\mathcal{G}}\leavevmode\nobreak\ .

(155)

请注意

\displaystyle{\bf q}_{\textit{JPL}}\triangleq{\bf q}_{\mathcal{L}\mathcal{G},left}={\bf q}_{\mathcal{L}\mathcal{G},\textit{right}}^{*}={\bf q}_{\mathcal{G}\mathcal{L},\textit{right}}\triangleq{\bf q}_{\textit{Hamilton}}\leavevmode\nobreak\ ,

(156)

这并不特别有用，但说明了在混合约定时很容易混淆。还需注意，我们可以得出 ${\bf q}_{JPL}={\bf q}_{Hamilton}$ 的结论，但这远非一个美妙的结果，反而是极大混淆的根源，因为等式仅存在于四元数值中，而这两个四元数在公式中使用时，其含义和代表的事物是不同的。

4 扰动、导数与积分

4.1 $SO(3)$ 中的加性与减性算子

在向量空间 ${\mathbb{R}}^{n}$ 中，加减运算通过常规的' $+$ '加法和' $-$ '减法实现。而在 $SO(3)$ 中无法直接进行此类运算，但可通过定义等效运算符来建立完整的微积分体系。

因此，我们定义在切空间元素 $\mathsf{R}\in SO(3)$ 与 $\mathsf{R}$ 处的元素 ${\bm{\theta}}\in{\mathbb{R}}^{3}$ 之间的加减运算符 $\oplus,\ominus$ 如下。

加运算符

“加”运算符 $\oplus:SO(3)\times{\mathbb{R}}^{3}\to SO(3)$ 生成 $SO(3)$ 中的一个元素 $\mathsf{S}$ ，该元素是将 $SO(3)$ 中的参考元素 $\mathsf{R}$ 与一个（通常较小的）旋转组合的结果。此旋转由向量空间 ${\bm{\theta}}\in{\mathbb{R}}^{3}$ 中的一个向量指定，该向量空间在参考元素 $\mathsf{R}$ 处与 $SO(3)$ 流形相切，即

\displaystyle\mathsf{S}=\mathsf{R}\oplus{\bm{\theta}}

\displaystyle\triangleq\mathsf{R}\circ\operatorname*{Exp}({\bm{\theta}})

\displaystyle\mathsf{R},\mathsf{S}\in SO(3),\leavevmode\nobreak\ {\bm{\theta}}\in{\mathbb{R}}^{3}\leavevmode\nobreak\ .

(157)

请注意，该运算符可针对 $SO(3)$ 的任何表示形式进行定义。特别地，对于四元数与旋转矩阵，我们有

	$\displaystyle{\bf q}_{\mathsf{S}}$	$\displaystyle=\,{\bf q}_{\mathsf{R}}\oplus{\bm{\theta}}={\bf q}_{\mathsf{R}}\otimes\operatorname*{Exp}({\bm{\theta}})$		(158)
	$\displaystyle{\bf R}_{\mathsf{S}}$	$\displaystyle={\bf R}_{\mathsf{R}}\oplus{\bm{\theta}}={\bf R}_{\mathsf{R}}\!\cdot\!\operatorname*{Exp}({\bm{\theta}})\leavevmode\nobreak\ .$		(159)

减运算符

“减”运算符 $\ominus:SO(3)\times SO(3)\to{\mathbb{R}}^{3}$ 是上述运算的逆运算。它返回 $SO(3)$ 中两个元素之间的向量角度差 ${\bm{\theta}}\in{\mathbb{R}}^{3}$ 。该差值在参考元素 $\mathsf{R}$ 的切向量空间中表示，

\displaystyle{\bm{\theta}}=\mathsf{S}\ominus\mathsf{R}

\displaystyle\triangleq\operatorname*{Log}(\mathsf{R}^{-1}\circ\mathsf{S})

\displaystyle\mathsf{R},\mathsf{S}\in SO(3),\leavevmode\nobreak\ {\bm{\theta}}\in{\mathbb{R}}^{3}\leavevmode\nobreak\ ,

(160)

对于四元数和旋转矩阵，其表达式为：

	$\displaystyle{\bm{\theta}}$	$\displaystyle=\,\,{\bf q}_{\mathsf{S}}\ominus{\bf q}_{\mathsf{R}}\,=\operatorname{Log}({\bf q}_{\mathsf{R}}^{}\otimes{\bf q}_{\mathsf{S}})$		(161)
	$\displaystyle{\bm{\theta}}$	$\displaystyle={\bf R}_{\mathsf{S}}\ominus{\bf R}_{\mathsf{R}}=\operatorname*{Log}({\bf R}_{\mathsf{R}}^{\top}\,{\bf R}_{\mathsf{S}})\leavevmode\nobreak\ .$		(162)

在这两种情况下，请注意尽管向量差值 ${\bm{\theta}}$ 通常假设为微小量，但上述定义适用于任意大小的 ${\bm{\theta}}$ （直至 $SO(3)$ 流形的首次覆盖范围，即角度 $\theta<\pi$ 以内）。

4.2 四种可能的导数定义

4.2.1 向量空间到向量空间的函数

标量与向量情形遵循经典的导数定义：给定函数 $f:{\mathbb{R}}^{m}\to{\mathbb{R}}^{n}$ ，我们使用 $\{+,-\}$ 将导数定义为

\displaystyle\frac{\partial f({\bf x})}{\partial{\bf x}}

\displaystyle\triangleq\lim_{\delta{\bf x}\to 0}\frac{f({\bf x}+\delta{\bf x})-f({\bf x})}{\delta{\bf x}}

\displaystyle\in{\mathbb{R}}^{n\times m}

(163)

欧拉积分生成形式如下的线性表达式

\displaystyle f({\bf x}+\Delta{\bf x})

\displaystyle\approx f({\bf x})+\frac{\partial f({\bf x})}{\partial{\bf x}}\Delta{\bf x}

\displaystyle\in{\mathbb{R}}^{n}

4.2.2 从 $SO(3)$ 到 $SO(3)$ 的函数

给定函数 $f:SO(3)\to SO(3)$ ，其中 $\mathsf{R}\in SO(3)$ ，以及一个局部小角度变化 ${\bm{\theta}}\in{\mathbb{R}}^{3}$ ，我们使用 $\{\oplus,\ominus\}$ 将导数定义为

	$\displaystyle\frac{\partial f(\mathsf{R})}{\partial{\bm{\theta}}}$	$\displaystyle\triangleq\lim_{\delta{\bm{\theta}}\to 0}\frac{f(\mathsf{R}\oplus\delta{\bm{\theta}})\ominus f(\mathsf{R})}{\delta{\bm{\theta}}}$	$\displaystyle\in{\mathbb{R}}^{3\times 3}$		(164)
		$\displaystyle=\lim_{\delta{\bm{\theta}}\to 0}\frac{\operatorname{Log}\big{(}f^{-1}(\mathsf{R})\,f(\mathsf{R}\operatorname{Exp}(\delta{\bm{\theta}}))\big{)}}{\delta{\bm{\theta}}}$			(165)

欧拉积分生成如下形式的表达式，

\displaystyle f(\mathsf{R}\oplus\Delta{\bm{\theta}})

\displaystyle\approx f(\mathsf{R})\,\oplus\,\frac{\partial f(\mathsf{R})}{\partial{\bm{\theta}}}\,\Delta{\bm{\theta}}\triangleq f(\mathsf{R})\operatorname*{Exp}\left(\frac{\partial f(\mathsf{R})}{\partial{\bm{\theta}}}\Delta{\bm{\theta}}\right)

\displaystyle\in SO(3)

4.2.3 向量空间到 $SO(3)$ 的函数

对于函数 $f:{\mathbb{R}}^{m}\to SO(3)$ 的情况，我们使用‘+’表示向量扰动，而‘ $\ominus$ ’表示 $SO(3)$ 差异。

	$\displaystyle\frac{\partial f({\bf x})}{\partial{\bf x}}$	$\displaystyle\triangleq\lim_{\delta{\bf x}\to 0}\frac{f({\bf x}+\delta{\bf x})\ominus f({\bf x})}{\delta{\bf x}}$	$\displaystyle\in{\mathbb{R}}^{3\times m}$		(166)
		$\displaystyle=\lim_{\delta{\bf x}\to 0}\frac{\operatorname*{Log}(f^{-1}({\bf x})f({\bf x}+\delta{\bf x}))}{\delta{\bf x}}$			(167)

欧拉积分生成如下形式的表达式，

\displaystyle f({\bf x}+\Delta{\bf x})

\displaystyle\approx f({\bf x})\,\oplus\,\frac{\partial f({\bf x})}{\partial{\bf x}}\,\Delta{\bf x}\triangleq f({\bf x})\,\operatorname*{Exp}\left(\frac{\partial f({\bf x})}{\partial{\bf x}}\Delta{\bf x}\right)

\displaystyle\in SO(3)

4.2.4 从 $SO(3)$ 到向量空间的函数

对于函数 $f:SO(3)\to{\mathbb{R}}^{n}$ 的情况，我们使用' $\oplus$ '表示 $SO(3)$ 扰动，用' $-$ '表示向量差值，

	$\displaystyle\frac{\partial f(\mathsf{R})}{\partial{\bm{\theta}}}$	$\displaystyle\triangleq\lim_{\delta{\bm{\theta}}\to 0}\frac{f(\mathsf{R}\oplus\delta{\bm{\theta}})-f(\mathsf{R})}{\delta{\bm{\theta}}}$	$\displaystyle\in{\mathbb{R}}^{n\times 3}$		(168)
		$\displaystyle=\lim_{\delta{\bm{\theta}}\to 0}\frac{f(\mathsf{R}\operatorname*{Exp}(\delta{\bm{\theta}}))-f(\mathsf{R})}{\delta{\bm{\theta}}}$			(169)

欧拉积分生成如下形式的表达式，

\displaystyle f(\mathsf{R}\oplus\delta{\bm{\theta}})

\displaystyle\approx f(\mathsf{R})+\frac{\partial f(\mathsf{R})}{\partial{\bm{\theta}}}\,\Delta{\bm{\theta}}\triangleq f(\mathsf{R})+\operatorname*{Exp}\left(\frac{\partial f(\mathsf{R})}{\partial{\bm{\theta}}}\Delta{\bm{\theta}}\right)

\displaystyle\in SO(3)

4.3 旋转的有用及极其有用的雅可比矩阵

考虑将向量 ${\bf a}$ 绕单位轴 ${\bf u}$ 旋转 $\theta$ 弧度。我们以三种等价形式表达旋转规范，即 ${\bm{\theta}}=\theta{\bf u}$ 、 ${\bf q}={\bf q}\{{\bm{\theta}}\}$ 和 ${\bf R}={\bf R}\{{\bm{\theta}}\}$ 。我们关注旋转结果相对于不同变量的雅可比矩阵。

4.3.1 关于向量的雅可比矩阵

向量 ${\bf a}$ 旋转后对其自身的导数计算较为简单，

\displaystyle\boxed{\frac{\partial({\bf q}\otimes{\bf a}\otimes{\bf q}*)}{\partial{\bf a}}=\frac{\partial({\bf R}\,{\bf a})}{\partial{\bf a}}={\bf R}}\leavevmode\nobreak\ .

(170)

4.3.2 关于四元数的雅可比矩阵

相反，关于四元数 ${\bf q}$ 的旋转导数较为复杂。为方便起见，我们采用更简洁的四元数表示法 ${\bf q}=[w\leavevmode\nobreak\ {\bf v}]=w+{\bf v}$ 。利用(34)、(33)及恒等式 ${\bf a}\times({\bf b}\times{\bf c})=({\bf c}\times{\bf b})\times{\bf a}=({\bf a}^{\top}{\bf c})\,{\bf b}-({\bf a}^{\top}{\bf b})\,{\bf c}$ ，我们推导出基于四元数的旋转(107)如下：

\displaystyle\begin{split}{\bf a}^{\prime}&={\bf q}\otimes{\bf a}\otimes{\bf q}*\\ &=(w+{\bf v})\otimes{\bf a}\otimes(w-{\bf v})\\ &=w^{2}{\bf a}+w({\bf v}\otimes{\bf a}-{\bf a}\otimes{\bf v})-{\bf v}\otimes{\bf a}\otimes{\bf v}\\ &=w^{2}{\bf a}+2w({\bf v}\!\times\!{\bf a})-\big{[}(-{\bf v}^{\top}{\bf a}+{\bf v}\!\times\!{\bf a})\otimes{\bf v}\big{]}\\ &=w^{2}{\bf a}+2w({\bf v}\!\times\!{\bf a})-\big{[}(-{\bf v}^{\top}{\bf a})\,{\bf v}+({\bf v}\!\times\!{\bf a})\otimes{\bf v}\big{]}\\ &=w^{2}{\bf a}+2w({\bf v}\!\times\!{\bf a})-\big{[}(-{\bf v}^{\top}{\bf a})\,{\bf v}-\cancel{({\bf v}\!\times\!{\bf a})^{\top}{\bf v}}+({\bf v}\!\times\!{\bf a})\!\times\!{\bf v}\big{]}\\ &=w^{2}{\bf a}+2w({\bf v}\!\times\!{\bf a})-\big{[}(-{\bf v}^{\top}{\bf a})\,{\bf v}+({\bf v}^{\top}{\bf v})\,{\bf a}-({\bf v}^{\top}{\bf a})\,{\bf v}\big{]}\\ &=w^{2}{\bf a}+2w({\bf v}\!\times\!{\bf a})+2({\bf v}^{\top}{\bf a})\,{\bf v}-({\bf v}^{\top}{\bf v})\,{\bf a}\leavevmode\nobreak\ .\end{split}

(171)

由此，我们可以提取导数 $\partial{\bf a}^{\prime}/\partial w$ 和 $\partial{\bf a}^{\prime}/\partial{\bf v}$ ，

	$\displaystyle\frac{\partial{\bf a}^{\prime}}{\partial w}$	$\displaystyle=2(w{\bf a}+{\bf v}\!\times\!{\bf a})$		(172)
	$\displaystyle\begin{split}\frac{\partial{\bf a}^{\prime}}{\partial{\bf v}}&=-2w\left[{\bf a}\right]_{\times}+2({\bf v}^{\top}{\bf a}\,{\bf I}+{\bf v}\,{\bf a}^{\top})-2{\bf a}\,{\bf v}^{\top}\\ &=2({\bf v}^{\top}{\bf a}\,{\bf I}+{\bf v}\,{\bf a}^{\top}-{\bf a}\,{\bf v}^{\top}-w\left[{\bf a}\right]_{\times})\leavevmode\nobreak\ ,\end{split}$			(173)

得到

\displaystyle\boxed{\frac{\partial({\bf q}\otimes{\bf a}\otimes{\bf q}*)}{\partial{\bf q}}=2\begin{bmatrix}\leavevmode\nobreak\ w\,{\bf a}+{\bf v}\!\times\!{\bf a}\leavevmode\nobreak\ \leavevmode\nobreak\ \big{|}\leavevmode\nobreak\ {\bf v}^{\top}{\bf a}\,{\bf I}_{3}+{\bf v}\,{\bf a}^{\top}-{\bf a}\,{\bf v}^{\top}-w\left[{\bf a}\right]_{\times}\leavevmode\nobreak\ \end{bmatrix}\in{\mathbb{R}}^{3\times 4}}\leavevmode\nobreak\ .

(174)

4.3.3 $SO(3)$ 的右雅可比矩阵

让我们考虑（见图 12）一个元素 $\mathsf{R}\in SO(3)$ 和一个旋转向量 ${\bm{\theta}}\in{\mathbb{R}}^{3}$ ，使得 $\mathsf{R}=\operatorname*{Exp}({\bm{\theta}})$ 。当 ${\bm{\theta}}$ 被改变一个量 $\delta{\bm{\theta}}$ 时，元素 $\mathsf{R}$ 会发生变化。在 $SO(3)$ 处的 $\mathsf{R}$ 切空间中用旋转向量 $\delta{\bm{\phi}}\in{\mathbb{R}}^{3}$ 表示 $\mathsf{R}$ 的变化，我们有（请参见图示，这里并非虚构）

\displaystyle\operatorname*{Exp}({\bm{\theta}})\oplus\delta{\bm{\phi}}=\operatorname*{Exp}({\bm{\theta}}+\delta{\bm{\theta}})

(175)

也可以写作，

\displaystyle\operatorname*{Exp}({\bm{\theta}})\circ\operatorname*{Exp}(\delta{\bm{\phi}})

\displaystyle=\operatorname*{Exp}({\bm{\theta}}+\delta{\bm{\theta}})\leavevmode\nobreak\ ,

(176)

甚至

\displaystyle\delta{\bm{\phi}}

\displaystyle=\operatorname*{Log}\Big{(}\operatorname*{Exp}({\bm{\theta}})^{-1}\circ\operatorname*{Exp}({\bm{\theta}}+\delta{\bm{\theta}})\Big{)}=\operatorname*{Exp}({\bm{\theta}}+\delta{\bm{\theta}})\ominus\operatorname*{Exp}({\bm{\theta}})\leavevmode\nobreak\ .

(177)

在极限情况下， $\delta{\bm{\phi}}$ 作为 $\delta{\bm{\theta}}$ 的函数变化定义了一个雅可比矩阵

\displaystyle\frac{\partial\delta{\bm{\phi}}}{\partial\delta{\bm{\theta}}}

\displaystyle=\lim_{\delta{\bm{\theta}}\to 0}\frac{\delta{\bm{\phi}}}{\delta{\bm{\theta}}}=\lim_{\delta{\bm{\theta}}\to 0}\frac{\operatorname*{Exp}({\bm{\theta}}+\delta{\bm{\theta}})\ominus\operatorname*{Exp}({\bm{\theta}})}{\delta{\bm{\theta}}}\leavevmode\nobreak\ ,

(178)

其表达式是（166）的特例，即它是函数 $f({\bm{\theta}})=\operatorname*{Exp}({\bm{\theta}})$ 从 ${\mathbb{R}}^{3}$ 到 $SO(3)$ 的导数。

该雅可比矩阵被称为 $SO(3)$ 的右雅可比矩阵，其定义为，

\displaystyle{\bf J}_{r}({\bm{\theta}})

\displaystyle\triangleq\frac{\partial\operatorname*{Exp}({\bm{\theta}})}{\partial{\bm{\theta}}}\leavevmode\nobreak\ .

(179)

其表达式独立于所使用的参数化方法，尽管确实可以针对每种参数化形式特别表达。利用(166)式可得，

$\displaystyle{\bf J}_{r}({\bm{\theta}})$	$\displaystyle=\lim_{\delta{\bm{\theta}}\to 0}\frac{\operatorname{Exp}({\bm{\theta}}+\delta{\bm{\theta}})\ominus\operatorname{Exp}({\bm{\theta}})}{\delta{\bm{\theta}}}$		(180)
	$\displaystyle=\lim_{\delta{\bm{\theta}}\to 0}\frac{\operatorname{Log}(\operatorname{Exp}({\bm{\theta}})^{\top}\operatorname*{Exp}({\bm{\theta}}+\delta{\bm{\theta}}))}{\delta{\bm{\theta}}}$	if using ${\bf R}$	(181)
	$\displaystyle=\lim_{\delta{\bm{\theta}}\to 0}\frac{\operatorname{Log}(\operatorname{Exp}({\bm{\theta}})^{}\otimes\operatorname{Exp}({\bm{\theta}}+\delta{\bm{\theta}}))}{\delta{\bm{\theta}}}$	$\displaystyle\textrm{if using ${\bf q}$}\leavevmode\nobreak\ .$	(182)

右雅可比矩阵及其逆矩阵可通过闭合形式计算 (Chirikjian,, 2012, 第 40 页)，

	$\displaystyle{\bf J}_{r}({\bm{\theta}})$	$\displaystyle={\bf I}-\frac{1-\cos{\left\\|{\bm{\theta}}\right\\|}}{{\left\\|{\bm{\theta}}\right\\|}^{2}}\left[{\bm{\theta}}\right]_{\times}+\frac{{\left\\|{\bm{\theta}}\right\\|}-\sin{\left\\|{\bm{\theta}}\right\\|}}{{\left\\|{\bm{\theta}}\right\\|}^{3}}\left[{\bm{\theta}}\right]_{\times}^{2}$		(183)
	$\displaystyle{\bf J}_{r}^{-1}({\bm{\theta}})$	$\displaystyle={\bf I}+\frac{1}{2}\left[{\bm{\theta}}\right]_{\times}+\left(\frac{1}{{\left\\|{\bm{\theta}}\right\\|}^{2}}-\frac{1+\cos{\left\\|{\bm{\theta}}\right\\|}}{2{\left\\|{\bm{\theta}}\right\\|}\sin{\left\\|{\bm{\theta}}\right\\|}}\right)\left[{\bm{\theta}}\right]_{\times}^{2}$		(184)

$SO(3)$ 的右雅可比矩阵具有以下性质：对于任意 ${\bm{\theta}}$ 和微小 $\delta{\bm{\theta}}$ ，

$\displaystyle\operatorname*{Exp}({\bm{\theta}}+\delta{\bm{\theta}})$	$\displaystyle\approx\operatorname{Exp}({\bm{\theta}})\operatorname{Exp}({\bf J}_{r}({\bm{\theta}})\delta{\bm{\theta}})$	(185)
$\displaystyle\operatorname{Exp}({\bm{\theta}})\operatorname{Exp}(\delta{\bm{\theta}})$	$\displaystyle\approx\operatorname*{Exp}({\bm{\theta}}+{\bf J}_{r}^{-1}({\bm{\theta}})\,\delta{\bm{\theta}})$	(186)
$\displaystyle\operatorname{Log}(\operatorname{Exp}({\bm{\theta}})\operatorname*{Exp}(\delta{\bm{\theta}}))$	$\displaystyle\approx{\bm{\theta}}+{\bf J}_{r}^{-1}({\bm{\theta}})\,\delta{\bm{\theta}}$	(187)

4.3.4 关于旋转向量的雅可比矩阵

向量 ${\bf a}^{\prime}={\bf R}\{{\bm{\theta}}\}\,{\bf a}$ 相对于旋转向量 ${\bm{\theta}}$ 的旋转是一个从 ${\mathbb{R}}^{3}$ 到 ${\mathbb{R}}^{3}$ 的函数。其对旋转向量 ${\bm{\theta}}$ 的导数运用了(163)式，并基于先前结果推导得出，使用了(185)式，

$\displaystyle\frac{\partial({\bf q}\otimes{\bf a}\otimes{\bf q}^{*})}{\partial\delta{\bm{\theta}}}=\frac{\partial({\bf R}\,{\bf a})}{\partial\delta{\bm{\theta}}}$	$\displaystyle=\lim_{\delta{\bm{\theta}}\to 0}\frac{{\bf R}\{{\bm{\theta}}+\delta{\bm{\theta}}\}\,{\bf a}-{\bf R}\{{\bm{\theta}}\}\,{\bf a}}{\delta{\bm{\theta}}}$	$\displaystyle\leftarrow(\ref{equ:derivative_vector})$
	$\displaystyle=\lim_{\delta{\bm{\theta}}\to 0}\frac{({\bf R}\{{\bm{\theta}}\}\operatorname*{Exp}({\bf J}_{r}({\bm{\theta}})\,\delta{\bm{\theta}})-{\bf R}\{{\bm{\theta}}\}){\bf a}}{\delta{\bm{\theta}}}$	$\displaystyle\leftarrow(\ref{equ:Jr1})$
	$\displaystyle=\lim_{\delta{\bm{\theta}}\to 0}\frac{({\bf R}\{{\bm{\theta}}\}({\bf I}+\left[{\bf J}_{r}({\bm{\theta}})\,\delta{\bm{\theta}}\right]_{\times})-{\bf R}\{{\bm{\theta}}\}){\bf a}}{\delta{\bm{\theta}}}$
	$\displaystyle=\lim_{\delta{\bm{\theta}}\to 0}\frac{{\bf R}\{{\bm{\theta}}\}\left[{\bf J}_{r}({\bm{\theta}})\,\delta{\bm{\theta}}\right]_{\times}{\bf a}}{\delta{\bm{\theta}}}$
	$\displaystyle=\lim_{\delta{\bm{\theta}}\to 0}-\frac{{\bf R}\{{\bm{\theta}}\}\left[{\bf a}\right]_{\times}{\bf J}_{r}({\bm{\theta}})\,\delta{\bm{\theta}}}{\delta{\bm{\theta}}}$
	$\displaystyle=-{\bf R}\{{\bm{\theta}}\}\left[{\bf a}\right]_{\times}{\bf J}_{r}({\bm{\theta}})\leavevmode\nobreak\ ,$

其中 ${\bf R}\{{\bm{\theta}}\}\triangleq\operatorname*{Exp}({\bm{\theta}})$ 。综上所述，

\displaystyle\boxed{\frac{\partial({\bf q}\otimes{\bf a}\otimes{\bf q}^{*})}{\partial\delta{\bm{\theta}}}=\frac{\partial({\bf R}\,{\bf a})}{\partial\delta{\bm{\theta}}}=-{\bf R}\{{\bm{\theta}}\}\left[{\bf a}\right]_{\times}{\bf J}_{r}({\bm{\theta}})}\leavevmode\nobreak\ .

(188)

4.3.5 旋转组合的雅可比矩阵

考虑 SO(3)组合运算 $\mathsf{P}=\mathsf{Q}\circ\mathsf{R}$ ，该运算既可用四元数形式也可用矩阵形式实现，

\displaystyle{\bf p}

\displaystyle={\bf q}_{\theta}\otimes{\bf r}_{\phi}

\displaystyle{\bf P}

\displaystyle={\bf Q}_{\theta}\,{\bf R}_{\phi}

(189)

其中下标表示切空间中向量扰动的名称。这些是从 $SO(3)$ 到 $SO(3)$ 的函数，因此我们使用(165)来写出导数，

	$\displaystyle\frac{\partial\mathsf{Q}\circ\mathsf{R}}{\partial\mathsf{Q}}=\frac{\partial{\bf q}_{\theta}\otimes{\bf r}_{\phi}}{\partial{\bm{\theta}}}=\frac{\partial{\bf Q}_{\theta}{\bf R}_{\phi}}{\partial{\bm{\theta}}}$	$\displaystyle=\lim_{\delta{\bm{\theta}}\to 0}\frac{(({\bf Q}_{\theta}\oplus\delta{\bm{\theta}}){\bf R}_{\phi})\ominus({\bf Q}_{\theta}{\bf R}_{\phi})}{\delta{\bm{\theta}}}$
		$\displaystyle=\lim_{\delta{\bm{\theta}}\to 0}\frac{\operatorname{Log}[({\bf Q}_{\theta}{\bf R}_{\phi})^{\top}({\bf Q}_{\theta}\operatorname{Exp}(\delta{\bm{\theta}}){\bf R}_{\phi})]}{\delta{\bm{\theta}}}$
		$\displaystyle=\lim_{\delta{\bm{\theta}}\to 0}\frac{\operatorname{Log}[{\bf R}_{\phi}^{\top}\operatorname{Exp}(\delta{\bm{\theta}}){\bf R}_{\phi}]}{\delta{\bm{\theta}}}$
		$\displaystyle=\lim_{\delta{\bm{\theta}}\to 0}\frac{\operatorname{Log}[\operatorname{Exp}({\bf R}_{\phi}^{\top}\delta{\bm{\theta}})]}{\delta{\bm{\theta}}}$
		$\displaystyle=\lim_{\delta{\bm{\theta}}\to 0}\frac{{\bf R}_{\phi}^{\top}\delta{\bm{\theta}}}{\delta{\bm{\theta}}}={\bf R}_{\phi}^{\top}$

	$\displaystyle\frac{\partial\mathsf{Q}\circ\mathsf{R}}{\partial\mathsf{R}}=\frac{\partial{\bf q}_{\theta}\otimes{\bf r}_{\phi}}{\partial{\bm{\phi}}}=\frac{\partial{\bf Q}_{\theta}{\bf R}_{\phi}}{\partial{\bm{\phi}}}$	$\displaystyle=\lim_{\delta{\bm{\phi}}\to 0}\frac{({\bf Q}_{\theta}({\bf R}_{\phi}\oplus\delta{\bm{\phi}}))\ominus({\bf Q}_{\theta}{\bf R}_{\phi})}{\delta{\bm{\phi}}}$
		$\displaystyle=\lim_{\delta{\bm{\phi}}\to 0}\frac{\operatorname{Log}[({\bf Q}_{\theta}{\bf R}_{\phi})^{\top}({\bf Q}_{\theta}{\bf R}_{\phi}\operatorname{Exp}(\delta{\bm{\phi}}))]}{\delta{\bm{\phi}}}$
		$\displaystyle=\lim_{\delta{\bm{\phi}}\to 0}\frac{\operatorname{Log}[\operatorname{Exp}(\delta{\bm{\phi}})]}{\delta{\bm{\phi}}}$
		$\displaystyle=\lim_{\delta{\bm{\phi}}\to 0}\frac{\delta{\bm{\phi}}}{\delta{\bm{\phi}}}={\bf I}$

4.4 扰动、不确定性与噪声

4.4.1 局部扰动

受扰动的方向 $\tilde{{\bf q}}$ 可以表示为未受扰动方向 ${\bf q}$ 与一个微小局部扰动 ${\Delta{\bf q}_{\mathcal{L}}}$ 的组合。根据哈密尔顿约定，该局部扰动出现在组合积的右侧 ——为便于比较，我们同时给出其矩阵等价形式，

\displaystyle\tilde{{\bf q}}

\displaystyle={\bf q}\otimes{\Delta{\bf q}_{\mathcal{L}}}\leavevmode\nobreak\ ,

\displaystyle\tilde{\bf R}

\displaystyle={\bf R}\,\Delta{\bf R}_{\mathcal{L}}\leavevmode\nobreak\ .

(190)

这些局部扰动 $\Delta{\bf q}_{\mathcal{L}}$ （或 $\Delta{\bf R}_{\mathcal{L}}$ ）可通过指数映射从其定义在切空间中的等效向量形式 $\Delta{\bm{\phi}}_{\mathcal{L}}={\bf u}\Delta\phi_{\mathcal{L}}$ 轻松获得。由此可得

\displaystyle\tilde{\bf q}_{\mathcal{L}}

\displaystyle={\bf q}_{\mathcal{L}}\otimes\operatorname*{Exp}(\Delta{\bm{\phi}}_{\mathcal{L}})\leavevmode\nobreak\ ,

\displaystyle\tilde{\bf R}_{\mathcal{L}}

\displaystyle={\bf R}_{\mathcal{L}}\!\cdot\!\operatorname*{Exp}(\Delta{\bm{\phi}}_{\mathcal{L}})

(191)

导出局部扰动的表达式

\displaystyle\Delta{\bm{\phi}}_{\mathcal{L}}=\operatorname*{Log}({\bf q}_{\mathcal{L}}^{*}\otimes\tilde{\bf q}_{\mathcal{L}})=\operatorname*{Log}({\bf R}_{\mathcal{L}}^{\top}\!\cdot\!\tilde{\bf R}_{\mathcal{L}})

(192)

当扰动角度 $\Delta\phi_{\mathcal{L}}$ 较小时，四元数和旋转矩阵形式的扰动可通过(101)式和(69)式的泰勒展开线性项进行近似，

\displaystyle{\Delta{\bf q}_{\mathcal{L}}}

\displaystyle\approx\begin{bmatrix}1\\ \frac{1}{2}\Delta{\bm{\phi}}_{\mathcal{L}}\end{bmatrix}\leavevmode\nobreak\ ,

\displaystyle\Delta{\bf R}_{\mathcal{L}}

\displaystyle\approx{\bf I}+\left[\Delta{\bm{\phi}}_{\mathcal{L}}\right]_{\times}\leavevmode\nobreak\ .

(193)

因此，扰动可在实际方向处与 $SO(3)$ 流形相切的局部向量空间 $\Delta{\bm{\phi}}_{\mathcal{L}}$ 中指定。例如，在此向量空间中表达这些扰动的协方差矩阵（即使用常规的 $3\times 3$ 协方差矩阵）将十分便利。

4.4.2 全局扰动

考虑全局定义的扰动及其相关导数不仅是可能的，而且确实颇具意义。全局扰动出现在组合积的左侧，即：

\displaystyle\tilde{\bf q}_{\mathcal{G}}

\displaystyle=\operatorname*{Exp}(\Delta{\bm{\phi}}_{\mathcal{G}})\otimes{\bf q}_{\mathcal{G}}\leavevmode\nobreak\ ,

\displaystyle\tilde{\bf R}_{\mathcal{G}}

\displaystyle=\operatorname*{Exp}(\Delta{\bm{\phi}}_{\mathcal{G}})\!\cdot\!{\bf R}_{\mathcal{G}}

(194)

从而得到全局扰动的表达式

\displaystyle\Delta{\bm{\phi}}_{\mathcal{G}}=\operatorname*{Log}(\tilde{\bf q}_{\mathcal{G}}\otimes{\bf q}_{\mathcal{G}}^{*})=\operatorname*{Log}(\tilde{\bf R}_{\mathcal{G}}\!\cdot\!{\bf R}_{\mathcal{G}}^{\top})

(195)

同样，这些扰动可以在向量空间 $\Delta{\bm{\phi}}_{\mathcal{G}}$ 中指定，该空间与 $SO(3)$ 流形在原点处相切。

4.5 时间导数

将局部扰动表达在向量空间中，我们便能轻松推导出时间导数的表达式。只需将 ${\bf q}={\bf q}(t)$ 视为原始状态， $\tilde{{\bf q}}={\bf q}(t+\Delta t)$ 视为扰动状态，然后应用导数的定义即可。

\displaystyle\frac{d{\bf q}(t)}{dt}\triangleq\lim_{\Delta t\to 0}\frac{{\bf q}(t+\Delta t)-{\bf q}(t)}{\Delta t}\leavevmode\nobreak\ ,

(196)

对于上述情况，有

\displaystyle{\bm{\omega}}_{\mathcal{L}}(t)\triangleq\frac{d{\bm{\phi}}_{\mathcal{L}}(t)}{dt}\triangleq\lim_{\Delta t\to 0}\frac{\Delta{\bm{\phi}}_{\mathcal{L}}}{\Delta t}\leavevmode\nobreak\ ,

(197)

其中， $\Delta{\bm{\phi}}_{\mathcal{L}}$ 作为局部角扰动，对应于由 ${\bf q}$ 定义的局部坐标系中的角速率向量。

四元数时间导数的推导如下（对于旋转矩阵可采用类似推理）

$\displaystyle\dot{{\bf q}}$	$\displaystyle\triangleq\lim_{\Delta t\to 0}\frac{{\bf q}(t+\Delta t)-{\bf q}(t)}{\Delta t}$
	$\displaystyle=\lim_{\Delta t\to 0}\frac{{\bf q}\otimes{\Delta{\bf q}_{\mathcal{L}}}-{\bf q}}{\Delta t}$
	$\displaystyle=\lim_{\Delta t\to 0}\frac{{\bf q}\otimes\left(\begin{bmatrix}1\\ \Delta{\bm{\phi}}_{\mathcal{L}}/2\end{bmatrix}-\begin{bmatrix}1\\ \bf 0\end{bmatrix}\right)}{\Delta t}$
	$\displaystyle=\lim_{\Delta t\to 0}\frac{{\bf q}\otimes\begin{bmatrix}0\\ \Delta{\bm{\phi}}_{\mathcal{L}}/2\end{bmatrix}}{\Delta t}$
	$\displaystyle=\frac{1}{2}\,{\bf q}\otimes\begin{bmatrix}0\\ {\bm{\omega}}_{\mathcal{L}}\end{bmatrix}\leavevmode\nobreak\ .$	(198)

定义

\displaystyle{\bm{\Omega}}({\bm{\omega}})\triangleq[{\bm{\omega}}]_{R}=\begin{bmatrix}0&-{\bm{\omega}}^{\top}\\ {\bm{\omega}}&-\left[{\bm{\omega}}\right]_{\times}\end{bmatrix}=\begin{bmatrix}0&-\omega_{x}&-\omega_{y}&-\omega_{z}\\ \omega_{x}&0&\omega_{z}&-\omega_{y}\\ \omega_{y}&-\omega_{z}&0&\omega_{x}\\ \omega_{z}&\omega_{y}&-\omega_{x}&0\end{bmatrix}\leavevmode\nobreak\ ,

(199)

我们从（198）和（17）式得出（同时给出其矩阵等效形式）

\displaystyle\dot{{\bf q}}

\displaystyle=\frac{1}{2}{\bm{\Omega}}({\bm{\omega}}_{\mathcal{L}})\,{\bf q}=\frac{1}{2}{\bf q}\otimes{\bm{\omega}}_{\mathcal{L}}\leavevmode\nobreak\ ,

\displaystyle\dot{\bf R}

\displaystyle={\bf R}\left[{\bm{\omega}}_{\mathcal{L}}\right]_{\times}\leavevmode\nobreak\ .

(200)

这些表达式显然与旋转群 $SO(3)$ 框架下推导的（99）和（67）式相同。但值得注意的是，此处我们能够明确将角速率 ${\bm{\omega}}_{\mathcal{L}}$ 关联到特定参考系——在本例中即由姿态 ${\bf q}$ 或 ${\bf R}$ 定义的局部坐标系。之所以实现这一点，是因为我们赋予了算子 ${\bf q}$ 和 ${\bf R}$ 精确的几何意义。从这个视角看，（200）式描述了一个参考系姿态的演化过程，其中角速率是在该参考系局部坐标系中表达的。

与全局扰动相关的时间导数可通过类似于(198)的推导得出，其结果为

\displaystyle\dot{\bf q}

\displaystyle=\frac{1}{2}\,{\bm{\omega}}_{\mathcal{G}}\otimes{\bf q}\leavevmode\nobreak\ ,

\displaystyle\dot{\bf R}

\displaystyle=\left[{\bm{\omega}}_{\mathcal{G}}\right]_{\times}{\bf R}\leavevmode\nobreak\ ,

(201)

其中

\displaystyle{\bm{\omega}}_{\mathcal{G}}(t)\triangleq\frac{d{\bm{\phi}}_{\mathcal{G}}(t)}{dt}

(202)

该式表示在全局坐标系中表达的角速率向量。方程(201)描述了当角速率在全局参考系中表达时，参考系姿态的演化过程。

4.5.1 全局到局部关系

从前文可以注意到局部角速率与全局角速率之间存在如下关系，

\displaystyle\frac{1}{2}\,{\bm{\omega}}_{\mathcal{G}}\otimes{\bf q}=\dot{\bf q}=\frac{1}{2}\,{\bf q}\otimes{\bm{\omega}}_{\mathcal{L}}\leavevmode\nobreak\ .

(203)

然后，后乘以共轭四元数我们得到

\displaystyle{\bm{\omega}}_{\mathcal{G}}={\bf q}\otimes{\bm{\omega}}_{\mathcal{L}}\otimes{\bf q}^{*}={\bf R}\,{\bm{\omega}}_{\mathcal{L}}\leavevmode\nobreak\ .

(204)

同理，考虑到对于微小 $\Delta t$ 有 $\Delta{\bm{\phi}}_{R}\approx{\bm{\omega}}\Delta t$ ，我们可得

\displaystyle\Delta{\bm{\phi}}_{\mathcal{G}}={\bf q}\otimes\Delta{\bm{\phi}}_{\mathcal{L}}\otimes{\bf q}^{*}={\bf R}\,\Delta{\bm{\phi}}_{\mathcal{L}}\leavevmode\nobreak\ .

(205)

这意味着，我们可以通过四元数或旋转矩阵对角速率向量 ${\bm{\omega}}$ 和微小角度扰动 $\Delta{\bm{\phi}}$ 进行坐标系转换，就像处理常规向量一样。通过设定 ${\bm{\omega}}={\bf u}\omega$ 或 $\Delta{\bm{\phi}}={\bf u}\Delta\phi$ ，并注意到旋转轴向量 ${\bf u}$ 遵循常规变换规律，亦可得出相同结论。

\displaystyle{\bf u}_{\mathcal{G}}={\bf q}\otimes{\bf u}_{\mathcal{L}}\otimes{\bf q}^{*}={\bf R}\,{\bf u}_{\mathcal{L}}\leavevmode\nobreak\ .

(206)

4.5.2 四元数乘积的时间导数

我们采用常规的乘积导数公式，

\displaystyle\dot{({{\bf q}_{1}\otimes{\bf q}_{2}})}

\displaystyle=\dot{{\bf q}_{1}}\otimes{{\bf q}_{2}}+{\bf q}_{1}\otimes\dot{{{\bf q}_{2}}}\leavevmode\nobreak\ ,

\displaystyle\dot{({\bf R}_{1}{\bf R}_{2})}

\displaystyle=\dot{\bf R}_{1}{\bf R}_{2}+{\bf R}_{1}\dot{\bf R}_{2}\leavevmode\nobreak\ ,

(207)

但需注意，由于四元数乘法不满足交换律，我们必须严格遵循操作数的顺序。这意味着 $\dot{({\bf q}^{2})}\neq 2\,{\bf q}\otimes\dot{\bf q}$ ，与标量情况不同，而是

\displaystyle\dot{({\bf q}^{2})}=\dot{\bf q}\otimes{\bf q}+{\bf q}\otimes\dot{\bf q}\leavevmode\nobreak\ .

(208)

4.5.3 涉及导数的其他实用表达式

可以推导出局部旋转角速度的表达式

\displaystyle{\bm{\omega}}_{\mathcal{L}}

\displaystyle=2\,{\bf q}^{*}\otimes\dot{{\bf q}}\leavevmode\nobreak\ ,

\displaystyle\left[{\bm{\omega}}_{\mathcal{L}}\right]_{\times}

\displaystyle={\bf R}^{\top}\,\dot{\bf R}\leavevmode\nobreak\ .

(209)

以及全局旋转速率，

\displaystyle{\bm{\omega}}_{\mathcal{G}}

\displaystyle=2\,\dot{{\bf q}}\otimes{\bf q}^{*}\leavevmode\nobreak\ ,

\displaystyle\left[{\bm{\omega}}_{\mathcal{G}}\right]_{\times}

\displaystyle=\dot{\bf R}\,{\bf R}^{\top}\leavevmode\nobreak\ .

(210)

4.6 旋转速率的时间积分

四元数形式下的旋转随时间累积是通过积分与旋转速率定义相对应的微分方程实现的：局部旋转速率定义对应方程(200)，全局定义对应(201)。在我们研究的场景中，角速率由局部传感器测量，因此在离散时间点 $t_{n}=n\Delta t$ 提供局部测量值 ${\bm{\omega}}(t_{n})$ 。本文仅聚焦于此情况，对应微分方程(200)为：

\displaystyle\dot{\bf q}(t)=\frac{1}{2}{\bf q}(t)\otimes{\bm{\omega}}(t)\leavevmode\nobreak\ .

(211)

我们开发了零阶和一阶积分方法（图 13 及 14），均基于 ${\bf q}(t_{n}+\Delta t)$ 在时间 $t=t_{n}$ 处的泰勒展开。记 ${\bf q}\triangleq{\bf q}(t)$ 与 ${\bf q}_{n}\triangleq{\bf q}(t_{n})$ ， ${\bm{\omega}}$ 同理。该泰勒级数展开式为：

\displaystyle\,{\bf q}_{n+1}=\,{\bf q}_{n}+\,\dot{\bf q}_{n}\Delta t+\frac{1}{2!}\,\ddot{\bf q}_{n}\Delta t^{2}+\frac{1}{3!}\,\dddot{{\bf q}}_{n}\Delta t^{3}+\frac{1}{4!}\,\ddddot{{\bf q}}_{n}\Delta t^{4}+\cdots\leavevmode\nobreak\ .

(212)

通过重复应用四元数导数的表达式（211）并结合 $\ddot{\bm{\omega}}=0$ ，可以轻松求得上述 ${\bf q}_{n}$ 的连续导数。我们得到


$\displaystyle\,\dot{\bf q}_{n}$	$\displaystyle=\frac{1}{2}\,{\bf q}_{n}\,{\bm{\omega}}_{n}$	(213a)
$\displaystyle\,\ddot{\bf q}_{n}$	$\displaystyle=\frac{1}{2^{2}}\,{\bf q}_{n}\,{\bm{\omega}}_{n}^{2}+\frac{1}{2}\,{\bf q}_{n}\,\dot{\bm{\omega}}$	(213b)
$\displaystyle\,\dddot{{\bf q}}_{n}$	$\displaystyle=\frac{1}{2^{3}}\,{\bf q}_{n}\,{\bm{\omega}}_{n}^{3}+\frac{1}{4}\,{\bf q}_{n}\,\dot{\bm{\omega}}\,{\bm{\omega}}_{n}+\frac{1}{2}\,{\bf q}\,{\bm{\omega}}_{n}\,\dot{\bm{\omega}}$	(213c)
$\displaystyle\,{\bf q}_{n}^{(i\,\geq\,4)}$	$\displaystyle=\frac{1}{2^{i}}\,{\bf q}_{n}\,{\bm{\omega}}_{n}^{i}+\cdots\leavevmode\nobreak\ ,$	(213d)

为简化符号表示，我们省略了 $\otimes$ 标记，即所有乘积及 $\,{\bm{\omega}}$ 的幂次均应按四元数乘积规则进行解释。

4.6.1 零阶积分

前向积分

当角速度 ${\bm{\omega}}_{n}$ 在时间段 $[t_{n},t_{n+1}]$ 内保持恒定时，我们有 $\dot{\bm{\omega}}=0$ ，且(212)式简化为：

\displaystyle\,{\bf q}_{n+1}=\,{\bf q}_{n}\otimes\left(1+\frac{1}{2}{\bm{\omega}}_{n}\Delta t+\frac{1}{2!}\Big{(}\frac{1}{2}{\bm{\omega}}_{n}\Delta t\Big{)}^{2}+\frac{1}{3!}\Big{(}\frac{1}{2}{\bm{\omega}}_{n}\Delta t\Big{)}^{3}+\cdots\right)\leavevmode\nobreak\ ,

(214)

此处可识别出指数 $e^{{\bm{\omega}}_{n}\Delta t/2}$ 的泰勒级数(40)。根据(101)式，该指数对应于表示增量旋转 $\Delta\theta={\bm{\omega}}_{n}\Delta t$ 的四元数：

\displaystyle e^{{\bm{\omega}}\Delta t/2}=\operatorname*{Exp}({\bm{\omega}}\Delta t)={\bf q}\{{\bm{\omega}}\Delta t\}=\begin{bmatrix}\cos({\left\|{\bm{\omega}}\right\|}\Delta t/2)\\ \frac{{\bm{\omega}}}{{\left\|{\bm{\omega}}\right\|}}\sin({\left\|{\bm{\omega}}\right\|}\Delta t/2)\end{bmatrix}\leavevmode\nobreak\ ,

因此，

\displaystyle\boxed{{\bf q}_{n+1}={\bf q}_{n}\otimes{\bf q}\{{\bm{\omega}}_{n}\Delta t\}}\leavevmode\nobreak\ .

(215)

后向积分

我们也可以认为，在时间段 $\Delta t$ 内的恒定速度对应于该时间段结束时测得的 ${\bm{\omega}}_{n+1}$ 速度。这可以通过对 ${\bf q}_{n}$ 在 $t_{n+1}$ 附近进行泰勒展开来类似地推导得出。

\displaystyle{{\bf q}_{n+1}\approx{\bf q}_{n}\otimes{\bf q}\{{\bm{\omega}}_{n+1}\Delta t\}}\leavevmode\nobreak\ .

(216)

在此需要指出的是，这是处理实时到达的运动测量数据时典型的积分方法，因为积分范围对应于最后一次测量（本例中为 $t_{n+1}$ ，参见图 14）。为了更清晰地表达这一点，我们可以重新标记时间索引，用 $\{n-1,n\}$ 代替 $\{n,n+1\}$ ，并表示为：

\displaystyle\boxed{{\bf q}_{n}={\bf q}_{n-1}\otimes{\bf q}\{{\bm{\omega}}_{n}\Delta t\}}\leavevmode\nobreak\ .

(217)

中值积分

类似地，如果将速度视为时间段 $\Delta t$ 内的中值速率（这并不一定是该时间段中点处的速度），

\displaystyle\,\overline{\bm{\omega}}=\frac{\,{\bm{\omega}}_{n+1}+\,{\bm{\omega}}_{n}}{2}\leavevmode\nobreak\ ,

(218)

我们得到，

\displaystyle\boxed{\,{\bf q}_{n+1}={\bf q}_{n}\otimes{\bf q}\{\,\overline{\bm{\omega}}\Delta t\}}\leavevmode\nobreak\ .

(219)

4.6.2 一阶积分

此时角速度 $\,{\bm{\omega}}(t)$ 随时间呈线性变化。其一阶导数为常数，所有更高阶导数均为零，

	$\displaystyle\,\dot{\bm{\omega}}$	$\displaystyle=\frac{\,{\bm{\omega}}_{n+1}-\,{\bm{\omega}}_{n}}{\Delta t}$		(220)
	$\displaystyle\ddot{\,}{\bm{\omega}}=\dddot{\,{\bm{\omega}}}=\cdots$	$\displaystyle=0\leavevmode\nobreak\ .$		(221)

我们可以用 $\,{\bm{\omega}}_{n}$ 和 $\,\dot{\bm{\omega}}$ 表示中值速率 $\,\overline{\bm{\omega}}$ ，

\displaystyle\,\overline{\bm{\omega}}=\,{\bm{\omega}}_{n}+\frac{1}{2}\,\dot{\bm{\omega}}\Delta t\leavevmode\nobreak\ ,

(222)

并推导出四元数导数(213)中出现的 $\,{\bm{\omega}}_{n}$ 幂次表达式，改用更便捷的 $\,\overline{\bm{\omega}}$ 和 $\,\dot{\bm{\omega}}$ 表示。


$\displaystyle\,{\bm{\omega}}_{n}$	$\displaystyle=\,\overline{\bm{\omega}}-\frac{1}{2}\,\dot{\bm{\omega}}\Delta t$	(223a)
$\displaystyle\,{\bm{\omega}}_{n}^{2}$	$\displaystyle=\,\overline{\bm{\omega}}^{2}-\frac{1}{2}\,\overline{\bm{\omega}}\,\dot{\bm{\omega}}\Delta t-\frac{1}{2}\,\dot{\bm{\omega}}\,\overline{\bm{\omega}}\Delta t+\frac{1}{4}\,\dot{\bm{\omega}}^{2}\Delta t^{2}$	(223b)
$\displaystyle\,{\bm{\omega}}_{n}^{3}$	$\displaystyle=\,\overline{\bm{\omega}}^{3}-\frac{3}{2}\,\overline{\bm{\omega}}^{2}\,\dot{\bm{\omega}}\Delta t+\frac{3}{4}\,\overline{\bm{\omega}}\,\dot{\bm{\omega}}^{2}\Delta t^{2}+\frac{1}{8}\,\dot{\bm{\omega}}^{3}\Delta t^{3}$	(223c)
$\displaystyle\,{\bm{\omega}}_{n}^{4}$	$\displaystyle=\,\overline{\bm{\omega}}^{4}+\cdots\leavevmode\nobreak\ .$	(223d)

将其代入四元数导数中，并在泰勒级数(212)中进行适当重排后可得：


$\displaystyle\,{\bf q}_{n+1}$	$\displaystyle=\,{\bf q}\left(1+\frac{1}{2}\,\overline{\bm{\omega}}\Delta t+\frac{1}{2!}\left(\frac{1}{2}\,\overline{\bm{\omega}}\Delta t\right)^{2}+\frac{1}{3!}\left(\frac{1}{2}\,\overline{\bm{\omega}}\Delta t\right)^{3}+\cdots\right)$	(224a)
	$\displaystyle+\>\,{\bf q}\left(-\frac{1}{4}\,\dot{\bm{\omega}}+\frac{1}{4}\,\dot{\bm{\omega}}\right)\Delta t^{2}$	(224b)
	$\displaystyle+\>\,{\bf q}\left(-\frac{1}{16}\,\overline{\bm{\omega}}\,\dot{\bm{\omega}}-\frac{1}{16}\,\dot{\bm{\omega}}\,\overline{\bm{\omega}}+\frac{1}{24}\,\dot{\bm{\omega}}\,\overline{\bm{\omega}}+\frac{1}{12}\,\overline{\bm{\omega}}\,\dot{\bm{\omega}}\right)\Delta t^{3}$	(224c)
	$\displaystyle+\>\,{\bf q}\,\bigg{(}\>\cdots\>\bigg{)}\Delta t^{4}\>+\>\cdots\leavevmode\nobreak\ ,$	(224d)

其中在(224a)中可识别出指数级数 $e^{\,\overline{\bm{\omega}}\Delta t/2}=\,{\bf q}\{\,\overline{\bm{\omega}}\Delta t\}$ ，(224b)项消失，而(224d)代表我们将忽略的高阶多重项。经简化后得到（现恢复常规 $\otimes$ 表示法）：

\displaystyle\,{\bf q}_{n+1}=\,{\bf q}_{n}\otimes\,{\bf q}\{\,\overline{\bm{\omega}}\Delta t\}+\frac{\Delta t^{3}}{48}\,{\bf q}_{n}\otimes(\,\overline{\bm{\omega}}\otimes\,\dot{\bm{\omega}}-\,\dot{\bm{\omega}}\otimes\,\overline{\bm{\omega}})+\cdots\leavevmode\nobreak\ .

(225)

将 $\,\dot{\bm{\omega}}$ 和 $\,\overline{\bm{\omega}}$ 替换为其定义（220）和（218）后可得，

\displaystyle\,{\bf q}_{n+1}=\,{\bf q}_{n}\otimes\,{\bf q}\{\,\overline{\bm{\omega}}\Delta t\}+\frac{\Delta t^{2}}{48}\,{\bf q}_{n}\otimes(\,{\bm{\omega}}_{n}\otimes\,{\bm{\omega}}_{n+1}-\,{\bm{\omega}}_{n+1}\otimes\,{\bm{\omega}}_{n})+\cdots\leavevmode\nobreak\ ,

(226)

该结果与 (Trawny 和 Roumeliotis，2005) 的结论等效，但采用哈密顿约定，并使用四元数乘积形式而非矩阵乘积形式。最后，由于 ${\bf a}_{v}\otimes{\bf b}_{v}-{\bf b}_{v}\otimes{\bf a}_{v}=2\,{\bf a}_{v}\!\times\!{\bf b}_{v}$ （参见(33)），我们得到替代形式：

\displaystyle\boxed{\,{\bf q}_{n+1}\approx{\bf q}_{n}\otimes\left({\bf q}\{\,\overline{\bm{\omega}}\Delta t\}+\frac{\Delta t^{2}}{24}\,\begin{bmatrix}0\\ \,{\bm{\omega}}_{n}\!\times\!\,{\bm{\omega}}_{n+1}\end{bmatrix}\right)}\leavevmode\nobreak\ .

(227)

在此表达式中，求和的第一项是中间零阶积分器（219）。第二项是一个二阶修正项，当 $\,{\bm{\omega}}_{n}$ 与 $\,{\bm{\omega}}_{n+1}$ 共线时（即当旋转轴从 $t_{n}$ 到 $t_{n+1}$ 未发生变化时），该项消失。

固定旋转轴情形

让我们记 ${\bm{\omega}}(t)={\bf u}(t)\,\omega(t)$ ，并称 ${\bf u}$ 为旋转轴。在旋转轴 ${\bf u}(t)={\bf u}$ 恒定的情况下，我们有 $\,{\bm{\omega}}_{n}\!\times\!\,{\bm{\omega}}_{n+1}=0$ ，因此，

\displaystyle\,{\bf q}_{n+1}={\bf q}_{n}\otimes{\bf q}\{{\bf u}\,\overline{\omega}\,\Delta t\}\leavevmode\nobreak\ .

(228)

这一结果实际上对于不限于 ${\bm{\omega}}(t)$ 一阶导数的情况也颇具意义。事实上，若旋转轴保持不变，旋转对四元数的无穷小贡献是可交换的，即，

\exp({\bf u}\,\omega_{1}\,\delta t_{1})\exp({\bf u}\,\omega_{2}\,\delta t_{2})=\exp({\bf u}\,\omega_{2}\,\delta t_{2})\exp({\bf u}\,\omega_{1}\,\delta t_{1})=\exp({\bf u}\,(\omega_{1}\delta t_{1}+\omega_{2}\delta t_{2}))\leavevmode\nobreak\ ,

因此我们得到恒等式，


$\displaystyle\,{\bf q}_{n+1}$	$\displaystyle={\bf q}_{n}\otimes\exp\left(\frac{{\bf u}}{2}\,\int_{t_{n}}^{t_{n+1}}\omega(t)\,\delta t\right)$	(229a)
	$\displaystyle={\bf q}_{n}\otimes\exp({\bf u}\,\Delta\theta_{n}/2)$	(229b)
	$\displaystyle={\bf q}_{n}\otimes{\bf q}\{{\bf u}\,\Delta\theta_{n}\}\leavevmode\nobreak\ .$	(229c)

其中 $\Delta\theta_{n}=\int_{t_{n}}^{t_{n+1}}\omega(t)dt\in{\mathbb{R}}$ 表示在时间间隔 $[t_{n},t_{n+1}]$ 内的总旋转角度。

旋转轴变化的情况

显然，(227)式中求和项的第二部分通过 $\,{\bm{\omega}}_{n}\!\times\!\,{\bm{\omega}}_{n+1}\neq 0$ 捕捉了旋转轴变化对积分方向的影响。在实际应用中，我们注意到，考虑到典型 IMU 采样时间 $\Delta t\leq 0.01s$ ，以及由于惯性作用导致的 $\,{\bm{\omega}}_{n}$ 与 $\,{\bm{\omega}}_{n+1}$ 近乎共线特性，这个二阶项的量级约为 $10^{-6}{\left\|{\bm{\omega}}\right\|}^{2}$ 或更小。更高阶的 $\,{\bm{\omega}}\Delta t$ 乘积项则更为微小，因此已被忽略。

还需注意的是，虽然所有零阶积分器通过构造都能得到单位四元数（因为它们被计算为两个单位四元数的乘积），但对于一阶积分器而言，由于式(227)中的求和运算，情况并非如此。因此，当使用一阶积分器时，即便如所述求和项很小，用户也应注意检查四元数范数随时间的变化，必要时通过 ${\bf q}\leftarrow{\bf q}/{\left\|{\bf q}\right\|}$ 形式的四元数更新进行重新归一化。唯有在恒定轴假设成立时，式(228)才同样成立，此时不再需要归一化操作。

5 基于 IMU 系统的误差状态运动学

5.1 动机

我们希望建立惯性系统的误差状态运动学方程，该系统通过整合加速度计和陀螺仪的读数（含偏置和噪声）进行工作，并采用哈密顿四元数表示空间中的姿态（attitude）。

加速度计和陀螺仪读数通常来自惯性测量单元(IMU)。对 IMU 读数进行积分会形成航位推算定位系统，这类系统会随时间产生漂移。要消除漂移，需要将此类信息与 GPS 或视觉等绝对位置读数进行融合。

误差状态卡尔曼滤波器（ESKF）是我们可用于此目的的工具之一。在卡尔曼滤波范式中，ESKF 最显著的优点如下 (Madyastha 等, 2011)：

•

方向误差状态是最小化的（即，其参数数量与自由度相同），避免了与过参数化（或冗余）相关的问题，以及由此导致的协方差矩阵奇异风险，这种风险通常源于强制约束。
•

误差状态系统始终在原点附近运行，因此远离可能的参数奇点、万向节锁定等问题，从而确保线性化有效性始终成立。
•

误差状态始终很小，这意味着所有二阶乘积均可忽略不计。这使得雅可比矩阵的计算非常简便快捷。某些雅可比矩阵甚至可能是常数或等于现有状态量值。
•

由于所有大信号动态都已整合至标称状态中，误差动态变化缓慢。这意味着我们可以以低于预测频率的速率应用卡尔曼滤波校正（这是观测误差的唯一手段）。

5.2 误差状态卡尔曼滤波器详解

在误差状态滤波器的表述中，我们讨论真实状态、标称状态和误差状态值，其中真实状态被表示为标称状态与误差状态的适当组合（线性求和、四元数乘积或矩阵乘积）。其核心思想是将标称状态视为大信号（可进行非线性积分），而将误差状态视为小信号（因此可线性积分且适用于线性高斯滤波）。

误差状态滤波器可解释如下。一方面，高频 IMU 数据 ${\bf u}_{m}$ 被积分至名义状态 ${\bf x}$ 。该名义状态未考虑噪声项 ${\bf w}$ 及其他可能的模型缺陷，因此会不断累积误差。这些误差被汇集于误差状态 $\delta{\bf x}$ 中，并通过误差状态卡尔曼滤波器（ESKF）进行估计——此时所有噪声与扰动均被纳入计算。误差状态由小信号量值构成，其演化函数由（时变）线性动态系统正确定义，其动态矩阵、控制矩阵及测量矩阵均根据名义状态值计算得出。在名义状态积分的同时，ESKF 对误差状态进行高斯估计预测。此时仅执行预测步骤，因为尚无其他测量数据可用于修正估计值。当非 IMU 的观测信息（例如 GPS、视觉等）到达时——这些信息能使误差具有可观测性且通常以远低于积分阶段的频率出现——滤波器执行修正步骤，从而获得误差状态的后验高斯估计。此后，将误差状态的均值注入名义状态，然后将其重置为零。误差状态的协方差矩阵会相应更新以反映这一重置操作。系统将如此持续运行。

5.3 连续时间系统运动学

所有相关变量的定义总结于表 3 中。关于约定有两个重要决策值得注意：

•

角速度 ${\bm{\omega}}$ 被定义为相对于名义四元数的局部量 。这使得我们可以直接使用陀螺仪测量值 ${\bm{\omega}}_{m}$ ，因为它们提供的是机体参考系下的角速度。
•

角度误差 $\delta{\bm{\theta}}$ 同样被定义为相对于标称姿态的局部量。这并非最优的处理方式，但符合大多数惯性测量单元集成研究中的选择——我们可称之为经典方法 。现有证据 (Li and Mourikis, 2012) 表明全局定义的角度误差具有更优特性。本文亦将在第 7 节探讨这一观点，但文中的主要推导、示例和算法仍基于这种局部定义的角度误差。

表 3：误差状态卡尔曼滤波器中的所有变量。

(¹) 符号 $\oplus$ 表示通用组合关系
幅值	真实值	标称值	误差	复合运算	测量值	噪声
完整状态(¹)	${\bf x}_{t}$	${\bf x}$	$\delta{\bf x}$	${\bf x}_{t}={\bf x}\oplus\delta{\bf x}$
位置	${\bf p}_{t}$	${\bf p}$	$\delta{\bf p}$	${\bf p}_{t}={\bf p}+\delta{\bf p}$
速度	${\bf v}_{t}$	${\bf v}$	$\delta{\bf v}$	${\bf v}_{t}={\bf v}+\delta{\bf v}$
四元数（^2,3）	${\bf q}_{t}$	${\bf q}$	${\delta{\bf q}}$	${\bf q}_{t}={\bf q}\otimes{\delta{\bf q}}$
旋转矩阵（^2,3）	${\bf R}_{t}$	${\bf R}$	$\delta{\bf R}$	${\bf R}_{t}={\bf R}\,\delta{\bf R}$
角度向量（⁴）			$\delta{\bm{\theta}}$	$\delta{\bf q}=e^{\delta{\bm{\theta}}/2}$
角度向量（⁴）			$\delta{\bm{\theta}}$	$\delta{\bf R}=e^{\left[\delta{\bm{\theta}}\right]_{\times}}$
加速度计偏置	${\bf a}_{bt}$	${\bf a}_{b}$	$\delta{\bf a}_{b}$	${\bf a}_{bt}={\bf a}_{b}+\delta{\bf a}_{b}$		${\bf a}_{w}$
陀螺仪偏置	${\bm{\omega}}_{bt}$	${\bm{\omega}}_{b}$	$\delta{\bm{\omega}}_{b}$	${\bm{\omega}}_{bt}={\bm{\omega}}_{b}+\delta{\bm{\omega}}_{b}$		${\bm{\omega}}_{w}$
重力矢量	${\bf g}_{t}$	${\bf g}$	$\delta{\bf g}$	${\bf g}_{t}={\bf g}+\delta{\bf g}$
加速度	${\bf a}_{t}$				${\bf a}_{m}$	${\bf a}_{n}$
角速率	${\bm{\omega}}_{t}$				${\bm{\omega}}_{m}$	${\bm{\omega}}_{n}$
(²) 表示非最小化表达形式
(³) 全局定义角度误差情况下的组合公式参见表 4
(⁴) 指数定义如(101)及(69, 77)式所示

5.3.1 真实状态运动学

真实运动学方程为


$\displaystyle\dot{\bf p}_{t}$	$\displaystyle={\bf v}_{t}$	(230a)
$\displaystyle\dot{\bf v}_{t}$	$\displaystyle={\bf a}_{t}$	(230b)
$\displaystyle\dot{{\bf q}_{t}}$	$\displaystyle=\frac{1}{2}{\bf q}_{t}\otimes{\bm{\omega}}_{t}$	(230c)
$\displaystyle\dot{\bf a}_{bt}$	$\displaystyle={\bf a}_{w}$	(230d)
$\displaystyle\dot{\bm{\omega}}_{bt}$	$\displaystyle={\bm{\omega}}_{w}$	(230e)
$\displaystyle\dot{\bf g}_{t}$	$\displaystyle=0$	(230f)

此处，真实加速度 ${\bf a}_{t}$ 和角速度 ${\bm{\omega}}_{t}$ 由 IMU 以带噪声的传感器读数 ${\bf a}_{m}$ 和 ${\bm{\omega}}_{m}$ 的形式在机体坐标系中获取，即 ²³²³23It is common practice to neglect the Earth’s rotation rate ${\bm{\omega}}_{\mathcal{E}}$ in the rotational kinematics described in (232), which would otherwise be ${\bm{\omega}}_{m}={\bm{\omega}}_{t}+{\bf R}_{t}^{\top}{\bm{\omega}}_{\mathcal{E}}+{\bm{\omega}}_{bt}+{\bm{\omega}}_{n}$ . Considering a non-null Earth rotation rate is, in the vast majority of practical cases, unjustifiably complicated. However, we notice that when employing high-end IMU sensors with very small noises and biases, a value of $\omega_{\mathcal{E}}=15^{\circ}$ /h $\approx 7.3\cdot 10^{-5}\,$ rad/s might become directly measurable; in such cases, in order to keep the IMU error model valid, the rate ${\bm{\omega}}_{\mathcal{E}}$ should not be neglected in the formulation.

	$\displaystyle{\bf a}_{m}$	$\displaystyle={\bf R}_{t}^{\top}({\bf a}_{t}-{\bf g}_{t})+{\bf a}_{bt}+{\bf a}_{n}$		(231)
	$\displaystyle{\bm{\omega}}_{m}$	$\displaystyle={\bm{\omega}}_{t}+{\bm{\omega}}_{bt}+{\bm{\omega}}_{n}$		(232)

通过 ${\bf R}_{t}\triangleq{\bf R}\{{\bf q}_{t}\}$ 。由此，真实值可以被分离出来（这意味着我们已经反转了测量方程），

	$\displaystyle{\bf a}_{t}$	$\displaystyle={\bf R}_{t}({\bf a}_{m}-{\bf a}_{bt}-{\bf a}_{n})+{\bf g}_{t}$		(233)
	$\displaystyle{\bm{\omega}}_{t}$	$\displaystyle={\bm{\omega}}_{m}-{\bm{\omega}}_{bt}-{\bm{\omega}}_{n}.$		(234)

将上述内容代入后得到运动学系统


$\displaystyle\dot{\bf p}_{t}$	$\displaystyle={\bf v}_{t}$	(235a)
$\displaystyle\dot{\bf v}_{t}$	$\displaystyle={\bf R}_{t}({\bf a}_{m}-{\bf a}_{bt}-{\bf a}_{n})+{\bf g}_{t}$	(235b)
$\displaystyle\dot{{\bf q}_{t}}$	$\displaystyle=\frac{1}{2}{\bf q}_{t}\otimes({\bm{\omega}}_{m}-{\bm{\omega}}_{bt}-{\bm{\omega}}_{n})$	(235c)
$\displaystyle\dot{\bf a}_{bt}$	$\displaystyle={\bf a}_{w}$	(235d)
$\displaystyle\dot{\bm{\omega}}_{bt}$	$\displaystyle={\bm{\omega}}_{w}$	(235e)
$\displaystyle\dot{\bf g}_{t}$	$\displaystyle=0$	(235f)

我们可以将其命名为 $\dot{\bf x}_{t}=f_{t}({\bf x}_{t},{\bf u},{\bf w})$ 。该系统具有状态 ${\bf x}_{t}$ ，由 IMU 噪声读数 ${\bf u}_{m}$ 控制，并受到白高斯噪声 ${\bf w}$ 的扰动，所有这些都由以下定义：

{\bf x}_{t}=\begin{bmatrix}{\bf p}_{t}\\ {\bf v}_{t}\\ {\bf q}_{t}\\ {\bf a}_{bt}\\ {\bm{\omega}}_{bt}\\ {\bf g}_{t}\end{bmatrix}\qquad\qquad{\bf u}=\begin{bmatrix}{\bf a}_{m}-{\bf a}_{n}\\ {\bm{\omega}}_{m}-{\bm{\omega}}_{n}\end{bmatrix}\qquad\qquad{\bf w}=\begin{bmatrix}{\bf a}_{w}\\ {\bm{\omega}}_{w}\end{bmatrix}\leavevmode\nobreak\ .

(236)

需注意的是，在上述公式体系中，重力矢量@g0#将由滤波器进行估计。该矢量遵循恒定演化方程（235f），这与已知恒定的物理量特性相符。系统始于固定且任意设定的初始姿态@g1#，由于该姿态通常不在水平面内，导致初始重力矢量通常未知。为简化起见，通常取@g2#，因此@g3#。我们估计的是在@g5#坐标系中表达的重力@g4#，而非水平坐标系中的@g6#，从而将初始姿态不确定性转化为重力方向的初始不确定性。这种处理旨在提升线性度：实际上，方程（235b）现在对承载所有不确定性的@g7#呈线性关系，而初始姿态@g8#被视为确知量，因此@g9#的初始状态不存在不确定性。一旦重力矢量完成估计，即可重建水平面，若需要还可将整个状态量及重建的运动轨迹重新定向以反映估计的水平基准。详见 (Lupton and Sukkarieh,, 2009) 的进一步论证。这当然是可选的，读者可以自由移除系统中所有与重力相关的方程，转而采用更经典的方法，即考虑 ${\bf g}\triangleq(0,0,-9.8xx)$ ，其中 $xx$ 表示实验现场重力向量的有效小数位数，以及不确定的初始方向 ${\bf q}_{0}$ 。

5.3.2 标称状态运动学

标称状态运动学对应于无噪声或扰动的建模系统，


$\displaystyle\dot{\bf p}$	$\displaystyle={\bf v}$	(237a)
$\displaystyle\dot{\bf v}$	$\displaystyle={\bf R}({\bf a}_{m}-{\bf a}_{b})+{\bf g}$	(237b)
$\displaystyle\dot{{\bf q}}$	$\displaystyle=\frac{1}{2}{\bf q}\otimes({\bm{\omega}}_{m}-{\bm{\omega}}_{b})$	(237c)
$\displaystyle\dot{\bf a}_{b}$	$\displaystyle=0$	(237d)
$\displaystyle\dot{\bm{\omega}}_{b}$	$\displaystyle=0$	(237e)
$\displaystyle\dot{\bf g}$	$\displaystyle=0.$	(237f)

5.3.3 误差状态运动学

目标是确定误差状态的线性化动态特性。针对每个状态方程，我们写出其组合形式（见表 3），求解误差状态并简化所有二阶无穷小量。下文给出完整的误差状态动态系统，随后进行注释和证明。


$\displaystyle\dot{\delta{\bf p}}$	$\displaystyle=\delta{\bf v}$	(238a)
$\displaystyle\dot{\delta{\bf v}}$	$\displaystyle=-{\bf R}\left[{\bf a}_{m}-{\bf a}_{b}\right]_{\times}\delta{\bm{\theta}}-{\bf R}\delta{\bf a}_{b}+\delta{\bf g}-{\bf R}{\bf a}_{n}$	(238b)
$\displaystyle\dot{\delta{\bm{\theta}}}$	$\displaystyle=-\left[{\bm{\omega}}_{m}-{\bm{\omega}}_{b}\right]_{\times}\delta{\bm{\theta}}-\delta{\bm{\omega}}_{b}-{\bm{\omega}}_{n}$	(238c)
$\displaystyle\dot{\delta{\bf a}_{b}}$	$\displaystyle={\bf a}_{w}$	(238d)
$\displaystyle\dot{\delta{\bm{\omega}}_{b}}$	$\displaystyle={\bm{\omega}}_{w}$	(238e)
$\displaystyle\dot{\delta{\bf g}}$	$\displaystyle=0.$	(238f)

方程（238a）、（238d）、（238e）和（238f）分别描述了位置误差、两种偏差误差以及重力误差，这些方程均由线性方程推导而来，其误差状态动力学较为简单。例如，考虑真实位置方程（235a）与标称位置方程（237a），根据表 3 中的组合关系 ${\bf p}_{t}={\bf p}+\delta{\bf p}$ ，求解 $\dot{\delta{\bf p}}$ 即可得到方程（238a）。

方程（238b）和（238c）分别对应速度误差与姿态误差，需要通过非线性方程（235b）和（235c）进行非平凡变换才能获得线性化动力学模型。其证明过程将在后续两节展开。

方程（238b）：线性速度误差

我们需要确定速度误差的动力学关系 $\dot{\delta{\bf v}}$ 。首先建立以下关系式：

	$\displaystyle{\bf R}_{t}$	$\displaystyle={\bf R}({\bf I}+\left[\delta{\bm{\theta}}\right]_{\times})+O({\left\\|\delta{\bm{\theta}}\right\\|}^{2})$		(239)
	$\displaystyle\dot{\bf v}$	$\displaystyle={\bf R}{\bf a}_{\mathcal{B}}+{\bf g},$		(240)

其中(239)是 ${\bf R}_{t}$ 的小信号近似，而在(240)中我们重写了(237b)，但引入了 ${\bf a}_{\mathcal{B}}$ 和 $\delta{\bf a}_{\mathcal{B}}$ ，定义为体坐标系下的大信号和小信号加速度，

	$\displaystyle{\bf a}_{\mathcal{B}}$	$\displaystyle\triangleq{\bf a}_{m}-{\bf a}_{b}$		(241)
	$\displaystyle\delta{\bf a}_{\mathcal{B}}$	$\displaystyle\triangleq-\delta{\bf a}_{b}-{\bf a}_{n}$		(242)

因此我们可以将惯性系中的真实加速度表示为大信号项与小信号项的叠加，

{\bf a}_{t}={\bf R}_{t}({\bf a}_{\mathcal{B}}+\delta{\bf a}_{\mathcal{B}})+{\bf g}+\delta{\bf g}.

(243)

我们通过以两种不同形式（左侧和右侧展开）书写 $\dot{\bf v}_{t}$ 的表达式(235b)来继续推导，其中忽略了 $O({\left\|\delta{\bm{\theta}}\right\|}^{2})$ 项，

	$\displaystyle\dot{\bf v}+\dot{\delta{\bf v}}=$	$\displaystyle\boxed{\dot{\bf v}_{t}}={\bf R}({\bf I}+\left[\delta{\bm{\theta}}\right]_{\times})({\bf a}_{\mathcal{B}}+\delta{\bf a}_{\mathcal{B}})+{\bf g}+\delta{\bf g}$
	$\displaystyle{\bf R}{\bf a}_{\mathcal{B}}+{\bf g}+\dot{\delta{\bf v}}=$	$\displaystyle\leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ ={\bf R}{\bf a}_{\mathcal{B}}+{\bf R}\delta{\bf a}_{\mathcal{B}}+{\bf R}\left[\delta{\bm{\theta}}\right]_{\times}{\bf a}_{\mathcal{B}}+{\bf R}\left[\delta{\bm{\theta}}\right]_{\times}\delta{\bf a}_{\mathcal{B}}+{\bf g}+\delta{\bf g}$

在左右两边消去 ${\bf R}{\bf a}_{\mathcal{B}}+{\bf g}$ 后，这导致

\dot{\delta{\bf v}}={\bf R}(\delta{\bf a}_{\mathcal{B}}+\left[\delta{\bm{\theta}}\right]_{\times}{\bf a}_{\mathcal{B}})+{\bf R}\left[\delta{\bm{\theta}}\right]_{\times}\delta{\bf a}_{\mathcal{B}}+\delta{\bf g}

(244)

消除二阶项并重新组织一些交叉乘积（使用 $\left[{\bf a}\right]_{\times}{\bf b}=-\left[{\bf b}\right]_{\times}{\bf a}$ ），我们得到

\dot{\delta{\bf v}}={\bf R}(\delta{\bf a}_{\mathcal{B}}-\left[{\bf a}_{\mathcal{B}}\right]_{\times}\delta{\bm{\theta}})+\delta{\bf g},

(245)

那么，回顾（241）和（242）式，

{\dot{\delta{\bf v}}={\bf R}(-\left[{\bf a}_{m}-{\bf a}_{b}\right]_{\times}\delta{\bm{\theta}}-\delta{\bf a}_{b}-{\bf a}_{n})+\delta{\bf g}}

(246)

经适当整理后可得线速度误差的动力学方程，

\boxed{\dot{\delta{\bf v}}=-{\bf R}\left[{\bf a}_{m}-{\bf a}_{b}\right]_{\times}\delta{\bm{\theta}}-{\bf R}\delta{\bf a}_{b}+\delta{\bf g}-{\bf R}{\bf a}_{n}}\ .

(247)

为了进一步简化该表达式，我们通常可以假设加速度计噪声是白噪声、不相关且各向同性的 ²⁴²⁴24This assumption cannot be made in cases where the three $XYZ$ accelerometers are not identical. ，

{\mathbb{E}}[{\bf a}_{n}]=0\qquad\qquad{\mathbb{E}}[{\bf a}_{n}{\bf a}_{n}^{\top}]=\sigma_{a}^{2}{\bf I},

(248)

即协方差椭球是一个以原点为中心的球体，这意味着其均值和协方差矩阵在旋转下保持不变（ 证明： ${\mathbb{E}}[{\bf R}{\bf a}_{n}]={\bf R}{\mathbb{E}}[{\bf a}_{n}]=0$ 和 ${\bf E}[({\bf R}{\bf a}_{n})({\bf R}{\bf a}_{n})^{\top}]={\bf R}{\mathbb{E}}[{\bf a}_{n}{\bf a}_{n}^{\top}]{\bf R}^{\top}={\bf R}\sigma_{a}^{2}{\bf I}{\bf R}^{\top}=\sigma_{a}^{2}{\bf I}$ ）。因此我们可以重新定义加速度计噪声向量，且完全不影响结果，根据

{\bf a}_{n}\leftarrow{\bf R}{\bf a}_{n}

(249)

从而得到

\boxed{\dot{\delta{\bf v}}=-{\bf R}\left[{\bf a}_{m}-{\bf a}_{b}\right]_{\times}\delta{\bm{\theta}}-{\bf R}\delta{\bf a}_{b}+\delta{\bf g}-{\bf a}_{n}}\ .

(250)

方程（238c）：姿态误差。

我们希望确定角误差的动态特性 $\dot{\delta{\bm{\theta}}}$ 。我们从以下关系出发

	$\displaystyle\dot{{\bf q}_{t}}$	$\displaystyle=\frac{1}{2}{\bf q}_{t}\otimes{\bm{\omega}}_{t}$		(251)
	$\displaystyle\dot{{\bf q}}$	$\displaystyle=\frac{1}{2}{\bf q}\otimes{\bm{\omega}},$		(252)

这些就是四元数导数的真实定义与标称定义。

如同处理加速度时那样，为清晰起见，我们将角速率中的大信号项与小信号项分组，

	$\displaystyle{\bm{\omega}}$	$\displaystyle\triangleq{\bm{\omega}}_{m}-{\bm{\omega}}_{b}$		(253)
	$\displaystyle\delta{\bm{\omega}}$	$\displaystyle\triangleq-\delta{\bm{\omega}}_{b}-{\bm{\omega}}_{n},$		(254)

因此 ${\bm{\omega}}_{t}$ 可以表示为标称部分与误差部分之和

{\bm{\omega}}_{t}={\bm{\omega}}+\delta{\bm{\omega}}.

(255)

我们通过两种不同方法（左展开与右展开）计算 $\dot{{\bf q}_{t}}$

	$\displaystyle\dot{{({\bf q}\otimes{\delta{\bf q}})}}=$	$\displaystyle\boxed{\dot{{\bf q}_{t}}}=\frac{1}{2}{\bf q}_{t}\otimes{\bm{\omega}}_{t}$
	$\displaystyle\dot{{\bf q}}\otimes{\delta{\bf q}}+{\bf q}\otimes\dot{{\delta{\bf q}}}=$	$\displaystyle\leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ =\frac{1}{2}{\bf q}\otimes{\delta{\bf q}}\otimes{\bm{\omega}}_{t}$
	$\displaystyle\frac{1}{2}{\bf q}\otimes{\bm{\omega}}\otimes{\delta{\bf q}}+{\bf q}\otimes\dot{{\delta{\bf q}}}=$

简化主导项 ${\bf q}$ 并分离出 $\dot{{\delta{\bf q}}}$ 后可得

$\displaystyle\begin{bmatrix}0\\ \dot{\delta{\bm{\theta}}}\end{bmatrix}=\boxed{2\dot{{\delta{\bf q}}}}$	$\displaystyle={\delta{\bf q}}\otimes{\bm{\omega}}_{t}-{\bm{\omega}}\otimes{\delta{\bf q}}$
	$\displaystyle=[{\bf q}]_{R}({\bm{\omega}}_{t}){\delta{\bf q}}-[{\bf q}]_{L}({\bm{\omega}}){\delta{\bf q}}$
	$\displaystyle=\begin{bmatrix}0&-({\bm{\omega}}_{t}-{\bm{\omega}})^{\top}\\ ({\bm{\omega}}_{t}-{\bm{\omega}})&-\left[{\bm{\omega}}_{t}+{\bm{\omega}}\right]_{\times}\end{bmatrix}\begin{bmatrix}1\\ \delta{\bm{\theta}}/2\end{bmatrix}+O({\left\\|\delta{\bm{\theta}}\right\\|}^{2})$
	$\displaystyle=\begin{bmatrix}0&-\delta{\bm{\omega}}^{\top}\\ \delta{\bm{\omega}}&-\left[2{\bm{\omega}}+\delta{\bm{\omega}}\right]_{\times}\end{bmatrix}\begin{bmatrix}1\\ \delta{\bm{\theta}}/2\end{bmatrix}+O({\left\\|\delta{\bm{\theta}}\right\\|}^{2})$	(256)

最终导出一个标量等式和一个向量等式


$\displaystyle 0$	$\displaystyle=\delta{\bm{\omega}}^{\top}\delta{\bm{\theta}}+O(\|\delta{\bm{\theta}}\|^{2})$	(257a)
$\displaystyle\dot{\delta{\bm{\theta}}}$	$\displaystyle=\delta{\bm{\omega}}-\left[{\bm{\omega}}\right]_{\times}\delta{\bm{\theta}}-\frac{1}{2}\left[\delta{\bm{\omega}}\right]_{\times}\delta{\bm{\theta}}+O({\left\\|\delta{\bm{\theta}}\right\\|}^{2}).$	(257b)

第一个方程得出 $\delta{\bm{\omega}}^{\top}\delta{\bm{\theta}}=O({\left\|\delta{\bm{\theta}}\right\|}^{2})$ ，它由二阶无穷小量构成，实用性不强。第二个方程在忽略所有二阶项后得到，

{\dot{\delta{\bm{\theta}}}=-\left[{\bm{\omega}}\right]_{\times}\delta{\bm{\theta}}}+\delta{\bm{\omega}}

(258)

最终，回顾(253)和(254)，我们得到角度误差的线性化动力学方程，

\boxed{\dot{\delta{\bm{\theta}}}=-\left[{\bm{\omega}}_{m}-{\bm{\omega}}_{b}\right]_{\times}\delta{\bm{\theta}}-\delta{\bm{\omega}}_{b}-{\bm{\omega}}_{n}}\ .

(259)

5.4 离散时间系统运动学

上述微分方程需要被积分成差分方程以适应离散时间间隔 $\Delta t>0$ 。积分方法可能各不相同：某些情况下可采用精确的闭式解，其他情况下则可能使用不同精度的数值积分方法。具体积分方法详见附录。

需对以下子系统进行积分：

1.

标称状态。
2.
误差状态
1. (a)
  
  确定性部分：状态动力学与控制。
2. (b)
  
  随机部分：噪声与扰动。

5.4.1 标称状态运动学

我们可以将名义状态差分方程表示为


$\displaystyle{\bf p}$	$\displaystyle\leftarrow{\bf p}+{\bf v}\,\Delta t+\frac{1}{2}({\bf R}({\bf a}_{m}-{\bf a}_{b})+{\bf g})\,\Delta t^{2}$	(260a)
$\displaystyle{\bf v}$	$\displaystyle\leftarrow{\bf v}+({\bf R}({\bf a}_{m}-{\bf a}_{b})+{\bf g})\,\Delta t$	(260b)
$\displaystyle{\bf q}$	$\displaystyle\leftarrow{\bf q}\otimes{\bf q}\{({\bm{\omega}}_{m}-{\bm{\omega}}_{b})\,\Delta t\}$	(260c)
$\displaystyle{\bf a}_{b}$	$\displaystyle\leftarrow{\bf a}_{b}$	(260d)
$\displaystyle{\bm{\omega}}_{b}$	$\displaystyle\leftarrow{\bm{\omega}}_{b}$	(260e)
$\displaystyle{\bf g}$	$\displaystyle\leftarrow{\bf g}\leavevmode\nobreak\ ,$	(260f)

其中 $x\leftarrow f(x,\bullet)$ 表示 $x_{k+1}=f(x_{k},\bullet_{k})$ 类型的时间更新， ${\bf R}\triangleq{\bf R}\{{\bf q}\}$ 是与当前标称姿态 ${\bf q}$ 相关联的旋转矩阵，而根据(101)， ${\bf q}\{v\}$ 是与旋转 $v$ 对应的四元数。

我们也可以采用更精确的积分方法，更多信息请参阅附录。

5.4.2 误差状态运动学

确定性部分按常规方式积分（此处我们遵循附录 C.2 中的方法），而随机部分的积分则产生随机脉冲（参见附录 E），因此，


$\displaystyle\delta{\bf p}$	$\displaystyle\leftarrow\delta{\bf p}+\delta{\bf v}\,\Delta t$	(261a)
$\displaystyle\delta{\bf v}$	$\displaystyle\leftarrow\delta{\bf v}+(-{\bf R}\left[{\bf a}_{m}-{\bf a}_{b}\right]_{\times}\delta{\bm{\theta}}-{\bf R}\delta{\bf a}_{b}+\delta{\bf g})\Delta t+{\bf v}_{\bf i}$	(261b)
$\displaystyle\delta{\bm{\theta}}$	$\displaystyle\leftarrow{\bf R}^{\top}\{({\bm{\omega}}_{m}-{\bm{\omega}}_{b})\Delta t\}\delta{\bm{\theta}}-\delta{\bm{\omega}}_{b}\Delta t+{\bm{\theta}}_{\bf i}$	(261c)
$\displaystyle\delta{\bf a}_{b}$	$\displaystyle\leftarrow\delta{\bf a}_{b}+{\bf a}_{\bf i}$	(261d)
$\displaystyle\delta{\bm{\omega}}_{b}$	$\displaystyle\leftarrow\delta{\bm{\omega}}_{b}+{\bm{\omega}}_{\bf i}$	(261e)
$\displaystyle\delta{\bf g}$	$\displaystyle\leftarrow\delta{\bf g}\leavevmode\nobreak\ .$	(261f)

此处， ${\bf v}_{\bf i}$ 、 ${\bm{\theta}}_{\bf i}$ 、 ${\bf a}_{\bf i}$ 和 ${\bm{\omega}}_{\bf i}$ 表示作用于速度、姿态及偏差估计的随机脉冲，由高斯白噪声过程建模。其均值为零，协方差矩阵通过对 ${\bf a}_{n}$ 、 ${\bm{\omega}}_{n}$ 、 ${\bf a}_{w}$ 和 ${\bm{\omega}}_{w}$ 的协方差在步长时间 $\Delta t$ 内积分获得（参见附录 E）。

$\displaystyle{\bf V}_{\bf i}$	$\displaystyle=\sigma_{\tilde{\bf a}_{n}}^{2}\Delta t^{2}{\bf I}\quad$	$\displaystyle[m^{2}/s^{2}]$	(262)
$\displaystyle\Theta_{\bf i}$	$\displaystyle=\sigma_{\tilde{\bm{\omega}}_{n}}^{2}\Delta t^{2}{\bf I}\quad$	$\displaystyle[rad^{2}]$	(263)
$\displaystyle{\bf A}_{\bf i}$	$\displaystyle=\sigma_{{\bf a}_{w}}^{2}\Delta t{\bf I}\quad$	$\displaystyle[m^{2}/s^{4}]$	(264)
$\displaystyle{\bm{\Omega}}_{\bf i}$	$\displaystyle=\sigma_{{\bm{\omega}}_{w}}^{2}\Delta t{\bf I}\quad$	$\displaystyle[rad^{2}/s^{2}]$	(265)

其中 $\sigma_{\tilde{\bf a}_{n}}[m/s^{2}]$ 、 $\sigma_{\tilde{\bm{\omega}}_{n}}[rad/s]$ 、 $\sigma_{{\bf a}_{w}}[m/s^{2}\sqrt{s}]$ 和 $\sigma_{{\bm{\omega}}_{w}}[rad/s\sqrt{s}]$ 需根据 IMU 数据手册信息或实验测量确定。

5.4.3 误差状态雅可比矩阵与扰动矩阵

雅可比矩阵可通过直接观察前一节中的误差状态差分方程获得。

为以紧凑形式表示这些方程，我们定义标称状态向量 ${\bf x}$ 、误差状态向量 $\delta{\bf x}$ 、输入向量 ${\bf u}_{m}$ 及扰动脉冲向量 ${\bf i}$ 如下（具体推导与论证参见附录 E.1）：

{\bf x}=\begin{bmatrix}{\bf p}\\ {\bf v}\\ {\bf q}\\ {\bf a}_{b}\\ {\bm{\omega}}_{b}\\ {\bf g}\end{bmatrix}\quad,\quad\delta{\bf x}=\begin{bmatrix}\delta{\bf p}\\ \delta{\bf v}\\ \delta{\bm{\theta}}\\ \delta{\bf a}_{b}\\ \delta{\bm{\omega}}_{b}\\ \delta{\bf g}\end{bmatrix}\quad,\quad{\bf u}_{m}=\begin{bmatrix}{\bf a}_{m}\\ {\bm{\omega}}_{m}\end{bmatrix}\quad,\quad{\bf i}=\begin{bmatrix}{\bf v}_{\bf i}\\ {\bm{\theta}}_{\bf i}\\ {\bf a}_{\bf i}\\ {\bm{\omega}}_{\bf i}\end{bmatrix}

(266)

此时误差状态系统为

\delta{\bf x}\leftarrow f({\bf x},\delta{\bf x},{\bf u}_{m},{\bf i})={\bf F}_{\bf x}({\bf x},{\bf u}_{m})\!\cdot\!\delta{\bf x}+{\bf F}_{\bf i}\!\cdot\!{\bf i},

(267)

ESKF 预测方程可表示为：

	$\displaystyle\hat{\delta{\bf x}}$	$\displaystyle\leftarrow{\bf F}_{\bf x}({\bf x},{\bf u}_{m})\!\cdot\!\hat{\delta{\bf x}}$		(268)
	$\displaystyle{\bf P}$	$\displaystyle\leftarrow{\bf F}_{\bf x}\,{\bf P}\,{\bf F}_{\bf x}^{\top}+{\bf F}_{\bf i}\,{\bf Q}_{\bf i}\,{\bf F}_{\bf i}^{\top}\leavevmode\nobreak\ ,$		(269)

式中 $\delta{\bf x}\sim\mathcal{N}\{\hat{\delta{\bf x}},{\bf P}\}$ ²⁵²⁵25 $x\sim\mathcal{N}\{\mu,\Sigma\}$ means that $x$ is a Gaussian random variable with mean and covariances matrix specified by $\mu$ and $\Sigma$ . ； ${\bf F}_{\bf x}$ 和 ${\bf F}_{\bf i}$ 表示 $f()$ 对误差向量及扰动向量的雅可比矩阵； ${\bf Q}_{\bf i}$ 为扰动脉冲的协方差矩阵。

上述雅可比矩阵和协方差矩阵的表达式将在下文详细说明。此处出现的所有状态相关值均直接取自标称状态。

{\bf F}_{\bf x}=\left.\frac{\partial f}{\partial\delta{\bf x}}\right|_{{\bf x},{\bf u}_{m}}=\begin{bmatrix}{\bf I}&{\bf I}\Delta t&0&0&0&0\\ 0&{\bf I}&-{\bf R}\left[{\bf a}_{m}-{\bf a}_{b}\right]_{\times}\Delta t&-{\bf R}\Delta t&0&{\bf I}\Delta t\\ 0&0&{\bf R}^{\top}\{({\bm{\omega}}_{m}-{\bm{\omega}}_{b})\Delta t\}&0&-{\bf I}\Delta t&0\\ 0&0&0&{\bf I}&0&0\\ 0&0&0&0&{\bf I}&0\\ 0&0&0&0&0&{\bf I}\\ \end{bmatrix}

(270)

{\bf F}_{\bf i}=\left.\frac{\partial f}{\partial{\bf i}}\right|_{{\bf x},{\bf u}_{m}}=\begin{bmatrix}0&0&0&0\\ {\bf I}&0&0&0\\ 0&{\bf I}&0&0\\ 0&0&{\bf I}&0\\ 0&0&0&{\bf I}\\ 0&0&0&0\end{bmatrix}\quad,\quad{\bf Q}_{\bf i}=\begin{bmatrix}{\bf V}_{\bf i}&0&0&0\\ 0&{\bm{\Theta}}_{\bf i}&0&0\\ 0&0&{\bf A}_{\bf i}&0\\ 0&0&0&{\bm{\Omega}}_{\bf i}\end{bmatrix}\leavevmode\nobreak\ .

(271)

需特别注意 ${\bf F}_{\bf x}$ 为系统转移矩阵，可通过多种方法以不同精度近似求解。此处展示其最简形式（欧拉形式），更多实现方式参见附录 B 至 D。

还需注意，由于误差均值 $\delta{\bf x}$ 初始化为零，线性方程（268）始终返回零值。当然，你应在代码中跳过该行（268）。不过我建议你仍编写该行，但将其注释掉以确保没有遗漏任何内容。

最后请注意，你绝对不应跳过协方差预测（269）！！实际上，项 ${\bf F}_{\bf i}\,{\bf Q}_{\bf i}\,{\bf F}_{\bf i}^{\top}$ 非零，因此该协方差会持续增长——这正是任何预测步骤中应有的表现。

6 IMU 与互补传感器数据的融合

当接收到除 IMU 外的其他信息（如 GPS 或视觉数据）时，我们着手修正 ESKF。在精心设计的系统中，这将使 IMU 偏差可观测，并允许 ESKF 准确估计这些偏差。存在多种组合可能，最流行的包括 GPS+IMU、单目视觉+IMU 以及立体视觉+IMU。近年来，视觉传感器与 IMU 的组合引起了广泛关注，并因此催生了大量科研活动。这些视觉+IMU 配置在 GPS 拒止环境中极具应用价值，既可部署于移动设备（通常是智能手机），也可应用于无人机等小型敏捷平台。

虽然迄今为止 IMU 信息一直用于 ESKF 的预测环节，但其他类型的信息则用于修正滤波器，从而观测 IMU 的偏差误差。修正过程包含三个步骤：

1.

通过滤波器修正观测误差状态，
2.

将观测误差注入标称状态，且
3.

重置误差状态。

这些步骤将在后续章节中展开说明。

6.1 通过滤波器校正观测误差状态

假设我们有一个传感器，它提供依赖于状态的信息，例如

{\bf y}=h({\bf x}_{t})+v\leavevmode\nobreak\ ,

(272)

其中 $h()$ 是系统状态（真实状态）的一般非线性函数， $v$ 是具有协方差 ${\bf V}$ 的高斯白噪声，

v\sim\mathcal{N}\{0,{\bf V}\}\leavevmode\nobreak\ .

(273)

我们的滤波器正在估计误差状态，因此滤波器校正方程 ²⁶²⁶26We give the simplest form of the covariance update, ${\bf P}\leftarrow({\bf I}-{\bf K}{\bf H}){\bf P}$ . This form is known to have poor numerical stability, as its outcome is not guaranteed to be symmetric nor positive definite. The reader is free to use more stable forms such as 1) the symmetric form ${\bf P}\leftarrow{\bf P}-{\bf K}({\bf H}{\bf P}{\bf H}^{\top}+{\bf V}){\bf K}^{\top}$ and 2) the symmetric and positive Joseph form ${\bf P}\leftarrow({\bf I}-{\bf K}{\bf H}){\bf P}({\bf I}-{\bf K}{\bf H})^{\top}+{\bf K}{\bf V}{\bf K}^{\top}$ . ，

$\displaystyle{\bf K}$	$\displaystyle={\bf P}{\bf H}^{\top}({\bf H}{\bf P}{\bf H}^{\top}+{\bf V})^{-1}$	(274)
$\displaystyle\hat{\delta{\bf x}}$	$\displaystyle\leftarrow{\bf K}({\bf y}-h(\hat{\bf x}_{t}))$	(275)
$\displaystyle{\bf P}$	$\displaystyle\leftarrow({\bf I}-{\bf K}{\bf H}){\bf P}$	(276)

需要定义关于误差状态 $\delta{\bf x}$ 的雅可比矩阵 ${\bf H}$ ，并在最佳真实状态估计 $\hat{\bf x}_{t}={\bf x}\oplus\hat{\delta{\bf x}}$ 处求值。由于此阶段误差状态均值为零（我们尚未观测到它），我们有 $\hat{\bf x}_{t}={\bf x}$ ，因此可以使用标称误差 ${\bf x}$ 作为求值点，从而得到

{\bf H}\equiv\left.\frac{\partial h}{\partial\delta{\bf x}}\right|_{{\bf x}}\leavevmode\nobreak\ .

(277)

6.1.1 滤波器校正的雅可比矩阵计算

上述雅可比矩阵可通过多种方式计算。最具启发性的方法是利用链式法则：

{\bf H}\triangleq\left.\frac{\partial h}{\partial\delta{\bf x}}\right|_{{\bf x}}=\left.\frac{\partial h}{\partial{\bf x}_{t}}\right|_{{\bf x}}\left.\frac{\partial{\bf x}_{t}}{\partial\delta{\bf x}}\right|_{{\bf x}}={\bf H}_{\bf x}\ {\bf X}_{\delta{\bf x}}\leavevmode\nobreak\ .

(278)

此处 ${\bf H}_{\bf x}\triangleq\left.\frac{\partial h}{\partial{\bf x}_{t}}\right|_{{\bf x}}$ 是 $h()$ 关于其自身参数的标准雅可比矩阵（即常规 EKF 中使用的雅可比矩阵）。链式法则的第一部分取决于所用特定传感器的测量函数，此处不作展示。

第二部分 ${\bf X}_{\delta{\bf x}}\triangleq\left.\frac{\partial{\bf x}_{t}}{\partial\delta{\bf x}}\right|_{{\bf x}}$ 是真实状态相对于误差状态的雅可比矩阵。这部分可以在此推导，因为它仅依赖于误差状态卡尔曼滤波器（ESKF）的状态组合。我们得到以下导数：

{\bf X}_{\delta{\bf x}}=\begin{bmatrix}\frac{\partial({\bf p}+\delta{\bf p})}{\partial\delta{\bf p}}&&&&&\\ &\frac{\partial({\bf v}+\delta{\bf v})}{\partial\delta{\bf v}}&&&0&\\ &&\frac{\partial({\bf q}\otimes\delta{\bf q})}{\partial\delta{\bm{\theta}}}&&&\\ &&&\frac{\partial({\bf a}_{b}+\delta{\bf a}_{b})}{\partial\delta{\bf a}_{b}}&&\\ &0&&&\frac{\partial({\bm{\omega}}_{b}+\delta{\bm{\omega}}_{b})}{\partial\delta{\bm{\omega}}_{b}}&\\ &&&&&\frac{\partial({\bf g}+\delta{\bf g})}{\partial\delta{\bf g}}\end{bmatrix}

(279)

这将导致除四元数项 ${\bf Q}_{\delta{\bm{\theta}}}=\partial({\bf q}\otimes\delta{\bf q})/\partial\delta{\bm{\theta}}$ （例如 $\frac{\partial({\bf p}+\delta{\bf p})}{\partial\delta{\bf p}}={\bf I}_{3}$ ）之外的所有 $3\times 3$ 块均为单位矩阵（如 $4\times 3$ 所示）。因此我们得到如下形式：

{\bf X}_{\delta{\bf x}}\triangleq\left.\frac{\partial{\bf x}_{t}}{\partial\delta{\bf x}}\right|_{{\bf x}}=\begin{bmatrix}{\bf I}_{6}&0&0\\ 0&{\bf Q}_{\delta{\bm{\theta}}}&0\\ 0&0&{\bf I}_{9}\end{bmatrix}

(280)

运用链式法则、方程（17–19）以及极限 $\delta{\bf q}\underset{\delta{\bm{\theta}}\to 0}{\longrightarrow}\begin{bmatrix}1\\ \frac{1}{2}\delta{\bm{\theta}}\end{bmatrix}$ ，四元数项 ${\bf Q}_{\delta{\bm{\theta}}}$ 可按如下方式推导得出：

	$\displaystyle{\bf Q}_{\delta{\bm{\theta}}}\triangleq\left.\frac{\partial({\bf q}\otimes\delta{\bf q})}{\partial\delta{\bm{\theta}}}\right\|_{{\bf q}}$	$\displaystyle=\left.\frac{\partial({\bf q}\otimes\delta{\bf q})}{\partial\delta{\bf q}}\right\|_{{\bf q}}\left.\frac{\partial\delta{\bf q}}{\partial\delta{\bm{\theta}}}\right\|_{\hat{\delta{\bm{\theta}}}=0}$
		$\displaystyle=\left.\frac{\partial([{\bf q}]_{L}\delta{\bf q})}{\partial\delta{\bf q}}\right\|_{{\bf q}}\left.\frac{\partial\begin{bmatrix}1\\ \frac{1}{2}\delta{\bm{\theta}}\end{bmatrix}}{\partial\delta{\bm{\theta}}}\right\|_{\hat{\delta{\bm{\theta}}}=0}$
		$\displaystyle=[{\bf q}]_{L}\,\frac{1}{2}\begin{bmatrix}0&0&0\\ 1&0&0\\ 0&1&0\\ 0&0&1\\ \end{bmatrix}\leavevmode\nobreak\ ,$

这将导出

{\bf Q}_{\delta{\bm{\theta}}}=\frac{1}{2}\begin{bmatrix}-q_{x}&-q_{y}&-q_{z}\\ q_{w}&-q_{z}&q_{y}\\ q_{z}&q_{w}&-q_{x}\\ -q_{y}&q_{x}&q_{w}\\ \end{bmatrix}\leavevmode\nobreak\ .

(281)

•

6.2 观测误差注入标称状态

在 ESKF 更新后，名义状态通过适当的组合运算（加法或四元数乘法，参见表 3）与观测到的误差状态进行更新：

{\bf x}\leftarrow{\bf x}\oplus\hat{\delta{\bf x}}\leavevmode\nobreak\ ,

(282)

即，


$\displaystyle{\bf p}$	$\displaystyle\leftarrow{\bf p}+\hat{\delta{\bf p}}$	(283a)
$\displaystyle{\bf v}$	$\displaystyle\leftarrow{\bf v}+\hat{\delta{\bf v}}$	(283b)
$\displaystyle{\bf q}$	$\displaystyle\leftarrow{\bf q}\otimes{\bf q}\{\hat{\delta{\bm{\theta}}}\}$	(283c)
$\displaystyle{\bf a}_{b}$	$\displaystyle\leftarrow{\bf a}_{b}+\hat{\delta{\bf a}_{b}}$	(283d)
$\displaystyle{\bm{\omega}}_{b}$	$\displaystyle\leftarrow{\bm{\omega}}_{b}+\hat{\delta{\bm{\omega}}_{b}}$	(283e)
$\displaystyle{\bf g}$	$\displaystyle\leftarrow{\bf g}+\hat{\delta{\bf g}}$	(283f)

6.3 误差状态卡尔曼滤波重置

将误差注入名义状态后，误差状态均值 $\hat{\delta{\bf x}}$ 将被重置。这对于姿态部分尤为重要，因为新的姿态误差将相对于新名义状态的姿态坐标系进行局部表达。为使 ESKF 更新完整，误差协方差需根据此调整进行相应更新。

我们称误差重置函数为 $g()$ ，其表达式如下：

\delta{\bf x}\leftarrow g(\delta{\bf x})=\delta{\bf x}\ominus\hat{\delta{\bf x}}\leavevmode\nobreak\ ,

(284)

其中 $\ominus$ 代表 $\oplus$ 的复合逆。因此，ESKF 误差重置操作可表示为：

	$\displaystyle\hat{\delta{\bf x}}$	$\displaystyle\leftarrow 0$		(285)
	$\displaystyle{\bf P}$	$\displaystyle\leftarrow{\bf G}\,{\bf P}\,{\bf G}^{\top}\leavevmode\nobreak\ .$		(286)

此处 ${\bf G}$ 为由下式定义的雅可比矩阵：

{\bf G}\triangleq\left.\frac{\partial g}{\partial\delta{\bf x}}\right|_{\hat{\delta{\bf x}}}\leavevmode\nobreak\ .

(287)

与前述更新雅可比矩阵的情况类似，该雅可比矩阵在所有对角块上均为单位矩阵，仅方向误差块例外。此处给出完整表达式，并在下一节中推导方向误差块 $\partial\delta{\bm{\theta}}^{+}/\partial\delta{\bm{\theta}}={\bf I}-\left[\frac{1}{2}\hat{\delta{\bm{\theta}}}\right]_{\times}$ 。

{\bf G}=\begin{bmatrix}{\bf I}_{6}&0&0\\ 0&{\bf I}-\left[\frac{1}{2}\hat{\delta{\bm{\theta}}}\right]_{\times}&0\\ 0&0&{\bf I}_{9}\end{bmatrix}\leavevmode\nobreak\ .

(288)

在大多数情况下，误差项 $\hat{\delta{\bm{\theta}}}$ 可被忽略，仅需计算雅可比矩阵 ${\bf G}={\bf I}_{18}$ ，从而实现简单的误差重置。这是误差状态卡尔曼滤波器(ESKF)多数实现采用的方式。本文提供的表达式应能产生更精确的结果，对于减少里程计系统的长期误差漂移可能具有重要意义。

6.3.1 针对姿态误差的重置操作雅可比矩阵

我们需要推导新角误差 $\delta{\bm{\theta}}^{+}$ 相对于旧误差 $\delta{\bm{\theta}}$ 和观测误差 $\hat{\delta{\bm{\theta}}}$ 的表达式。考虑以下事实：

•

真实方位在误差重置时不会改变，即 ${\bf q}_{t}^{+}={\bf q}_{t}$ 。由此可得：

${\bf q}^{+}\otimes\delta{\bf q}^{+}={\bf q}\otimes\delta{\bf q}\leavevmode\nobreak\ .$ (289)
•

观测误差均值已被注入名义状态（参见(283c)和(123)）：

${\bf q}^{+}={\bf q}\otimes\hat{\delta{\bf q}}\leavevmode\nobreak\ .$ (290)

结合这两个恒等式，我们得到了 $\delta{\bf q}^{+}$ 的表达式，

\delta{\bf q}^{+}=({\bf q}^{+})^{*}\otimes{\bf q}\otimes\delta{\bf q}=({\bf q}\otimes\hat{\delta{\bf q}})^{*}\otimes{\bf q}\otimes\delta{\bf q}=\hat{\delta{\bf q}}^{*}\otimes\delta{\bf q}=[\hat{\delta{\bf q}}^{*}]_{L}\cdot\delta{\bf q}\leavevmode\nobreak\ .

(291)

考虑到上述恒等式可以展开为

\begin{bmatrix}1\\ \frac{1}{2}\delta{\bm{\theta}}^{+}\end{bmatrix}=\begin{bmatrix}1&\frac{1}{2}\hat{\delta{\bm{\theta}}}^{\top}\\ -\frac{1}{2}\hat{\delta{\bm{\theta}}}&{\bf I}-\left[\frac{1}{2}\hat{\delta{\bm{\theta}}}\right]_{\times}\end{bmatrix}\cdot\begin{bmatrix}1\\ \frac{1}{2}\delta{\bm{\theta}}\end{bmatrix}+\mathcal{O}({\left\|\delta{\bm{\theta}}\right\|}^{2})\leavevmode\nobreak\ ,

(292)

由此得到一个标量方程和一个向量方程，


$\displaystyle\frac{1}{4}\hat{\delta{\bm{\theta}}}^{\top}\delta{\bm{\theta}}$	$\displaystyle=\mathcal{O}({\left\\|\delta{\bm{\theta}}\right\\|}^{2})$	(293a)
$\displaystyle\delta{\bm{\theta}}^{+}$	$\displaystyle=-\hat{\delta{\bm{\theta}}}+\left({\bf I}-\left[\frac{1}{2}\hat{\delta{\bm{\theta}}}\right]_{\times}\right)\delta{\bm{\theta}}+\mathcal{O}({\left\\|\delta{\bm{\theta}}\right\\|}^{2})\leavevmode\nobreak\ ,$	(293b)

其中第一个关系式信息量不大，因为它仅表达了无穷小量之间的关系。从第二个方程可以证明 $\hat{\delta{\bm{\theta}}}^{+}=0$ ，这正是重置操作所预期的结果。通过简单观察即可得到雅可比矩阵，

\boxed{\frac{\partial\delta{\bm{\theta}}^{+}}{\partial\delta{\bm{\theta}}}={\bf I}-\left[\frac{1}{2}\hat{\delta{\bm{\theta}}}\right]_{\times}}\leavevmode\nobreak\ .

(294)

7 采用全局角度误差的误差状态卡尔曼滤波器

本节探讨将角度误差定义在全局坐标系而非此前使用的局部坐标系所带来的影响。我们将重述第 5 节和第 6 节的推导过程，并特别说明与新定义存在差异的子章节。

角度误差的全局定义 $\delta{\bm{\theta}}$ 意味着左侧合成 ，即，

{\bf q}_{t}=\delta{\bf q}\ \otimes{\bf q}={\bf q}\{\delta{\bm{\theta}}\}\otimes{\bf q}\leavevmode\nobreak\ .

出于完备性考虑需要说明的是：无论角度误差是否采用全局定义，我们仍保持角速度矢量 ${\bm{\omega}}$ 的局部定义，即连续时间下的 $\dot{\bf q}=\frac{1}{2}{\bf q}\otimes{\bm{\omega}}$ ，以及离散时间下的 ${\bf q}\leftarrow{\bf q}\otimes{\bf q}\{{\bm{\omega}}\Delta t\}$ 。这样处理是出于便利性考虑，因为陀螺仪提供的角速度测量值始终基于载体坐标系（即局部坐标系）。

7.1 连续时间下的系统运动学

7.1.1 真实状态与标称状态的运动学

真实运动学与标称运动学不涉及误差，其方程保持不变。

7.1.2 误差状态运动学

我们首先写出误差状态运动学的方程，随后进行相关注释与证明。


$\displaystyle\dot{\delta{\bf p}}$	$\displaystyle=\delta{\bf v}$	(295a)
$\displaystyle\dot{\delta{\bf v}}$	$\displaystyle=-\left[{\bf R}({\bf a}_{m}-b{\bf a}_{b})\right]_{\times}\delta{\bm{\theta}}-{\bf R}\delta{\bf a}_{b}+\delta{\bf g}-{\bf R}{\bf a}_{n}$	(295b)
$\displaystyle\dot{\delta{\bm{\theta}}}$	$\displaystyle=-{\bf R}\delta{\bm{\omega}}_{b}-{\bf R}{\bm{\omega}}_{n}$	(295c)
$\displaystyle\dot{\delta{\bf a}_{b}}$	$\displaystyle={\bf a}_{w}$	(295d)
$\displaystyle\dot{\delta{\bm{\omega}}_{b}}$	$\displaystyle={\bm{\omega}}_{w}$	(295e)
$\displaystyle\dot{\delta{\bf g}}$	$\displaystyle=0\leavevmode\nobreak\ ,$	(295f)

其中，除 $\dot{\delta{\bf v}}$ 和 $\dot{\delta{\bm{\theta}}}$ 外的所有方程均为平凡表达式。非平凡表达式将在下文中展开推导。

方程(295b)：线速度误差

我们需要确定速度误差的动力学关系 $\dot{\delta{\bf v}}$ 。首先建立以下关系式：

	$\displaystyle{\bf R}_{t}$	$\displaystyle=({\bf I}+\left[\delta{\bm{\theta}}\right]_{\times}){\bf R}+O({\left\\|\delta{\bm{\theta}}\right\\|}^{2})$		(296)
	$\displaystyle\dot{\bf v}$	$\displaystyle={\bf R}{\bf a}_{\mathcal{B}}+{\bf g}\leavevmode\nobreak\ ,$		(297)

其中(296)式是采用全局定义误差对 ${\bf R}_{t}$ 进行小信号近似的结果，而在(297)式中我们引入了机体坐标系下的大信号加速度 ${\bf a}_{\mathcal{B}}$ 与小信号加速度 $\delta{\bf a}_{\mathcal{B}}$ ，其定义见(241)与(242)式，这与局部定义情形下的处理方式一致。

我们通过以两种不同形式（左侧和右侧展开）书写 $\dot{\bf v}_{t}$ 的表达式(235b)来继续推导，其中忽略了 $O({\left\|\delta{\bm{\theta}}\right\|}^{2})$ 项，

	$\displaystyle\dot{\bf v}+\dot{\delta{\bf v}}=$	$\displaystyle\boxed{\dot{\bf v}_{t}}=({\bf I}+\left[\delta{\bm{\theta}}\right]_{\times}){\bf R}({\bf a}_{\mathcal{B}}+\delta{\bf a}_{\mathcal{B}})+{\bf g}+\delta{\bf g}$
	$\displaystyle{\bf R}{\bf a}_{\mathcal{B}}+{\bf g}+\dot{\delta{\bf v}}=$	$\displaystyle\leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ ={\bf R}{\bf a}_{\mathcal{B}}+{\bf R}\delta{\bf a}_{\mathcal{B}}+\left[\delta{\bm{\theta}}\right]_{\times}{\bf R}{\bf a}_{\mathcal{B}}+\left[\delta{\bm{\theta}}\right]_{\times}{\bf R}\delta{\bf a}_{\mathcal{B}}+{\bf g}+\delta{\bf g}$

在左右两边消去 ${\bf R}{\bf a}_{\mathcal{B}}+{\bf g}$ 后，这导致

\dot{\delta{\bf v}}={\bf R}\delta{\bf a}_{\mathcal{B}}+\left[\delta{\bm{\theta}}\right]_{\times}{\bf R}({\bf a}_{\mathcal{B}}+\delta{\bf a}_{\mathcal{B}})+\delta{\bf g}

(298)

消除二阶项并重新组织一些交叉乘积（使用 $\left[{\bf a}\right]_{\times}{\bf b}=-\left[{\bf b}\right]_{\times}{\bf a}$ ），我们得到

\dot{\delta{\bf v}}={\bf R}\delta{\bf a}_{\mathcal{B}}-\left[{\bf R}{\bf a}_{\mathcal{B}}\right]_{\times}\delta{\bm{\theta}}+\delta{\bf g}\leavevmode\nobreak\ ,

(299)

最后，回顾(241)与(242)式并重新整理，可得速度误差导数的表达式：

\boxed{{\dot{\delta{\bf v}}=-\left[{\bf R}({\bf a}_{m}-{\bf a}_{b})\right]_{\times}\delta{\bm{\theta}}-{\bf R}\delta{\bf a}_{b}+\delta{\bf g}-{\bf R}{\bf a}_{n}}}

(300)

方程（295c）：方向误差。

我们首先写出四元数导数的真实定义与名义定义，

	$\displaystyle\dot{{\bf q}_{t}}$	$\displaystyle=\frac{1}{2}{\bf q}_{t}\otimes{\bm{\omega}}_{t}$		(301)
	$\displaystyle\dot{{\bf q}}$	$\displaystyle=\frac{1}{2}{\bf q}\otimes{\bm{\omega}}\leavevmode\nobreak\ ,$		(302)

需特别说明的是，此处采用全局定义的角误差，即

{\bf q}_{t}=\delta{\bf q}\otimes{\bf q}\leavevmode\nobreak\ .

(303)

如同处理局部定义误差的情况一样，我们也将大信号与小信号角速率（253–254）进行分组。我们通过两种不同方式（左展开与右展开）计算 $\dot{{\bf q}_{t}}$ ，

	$\displaystyle\dot{{({\delta{\bf q}}\otimes{\bf q})}}=$	$\displaystyle\boxed{\dot{{\bf q}_{t}}}=\frac{1}{2}{\bf q}_{t}\otimes{\bm{\omega}}_{t}$
	$\displaystyle\dot{\delta{\bf q}}\otimes{{\bf q}}+\delta{\bf q}\otimes\dot{{{\bf q}}}=$	$\displaystyle\leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ =\frac{1}{2}\delta{\bf q}\otimes{\bf q}\otimes{\bm{\omega}}_{t}$
	$\displaystyle\dot{\delta{\bf q}}\otimes{{\bf q}}+\frac{1}{2}\delta{\bf q}\otimes{\bf q}\otimes{\bm{\omega}}=$

得到 ${\bm{\omega}}_{t}={\bm{\omega}}+\delta{\bm{\omega}}$ 后，可简化为

\dot{\delta{\bf q}}\otimes{{\bf q}}=\frac{1}{2}\delta{\bf q}\otimes{\bf q}\otimes\delta{\bm{\omega}}\leavevmode\nobreak\ .

(304)

将左右项右乘 ${\bf q}^{*}$ ，并回顾 ${\bf q}\otimes\delta{\bm{\omega}}\otimes{\bf q}^{*}\equiv{\bf R}\delta{\bm{\omega}}$ ，可进一步推导如下，

$\displaystyle\dot{\delta{\bf q}}$	$\displaystyle=\frac{1}{2}\delta{\bf q}\otimes{\bf q}\otimes\delta{\bm{\omega}}\otimes{\bf q}^{*}$
	$\displaystyle=\frac{1}{2}\delta{\bf q}\otimes({\bf R}\delta{\bm{\omega}})$
	$\displaystyle=\frac{1}{2}\delta{\bf q}\otimes\delta{\bm{\omega}}_{G}\leavevmode\nobreak\ ,$	(305)

其中 $\delta{\bm{\omega}}_{G}\triangleq{\bf R}\delta{\bm{\omega}}$ 表示全局坐标系下表达的小信号角速率。接着，

$\displaystyle\begin{bmatrix}0\\ \dot{\delta{\bm{\theta}}}\end{bmatrix}=\boxed{2\dot{{\delta{\bf q}}}}$	$\displaystyle={\delta{\bf q}}\otimes\delta{\bm{\omega}}_{G}$
	$\displaystyle=\Omega(\delta{\bm{\omega}}_{G})\,\delta{\bf q}$
	$\displaystyle=\begin{bmatrix}0&-\delta{\bm{\omega}}_{G}^{\top}\\ \delta{\bm{\omega}}_{G}&-\left[\delta{\bm{\omega}}_{G}\right]_{\times}\end{bmatrix}\begin{bmatrix}1\\ \delta{\bm{\theta}}/2\end{bmatrix}+O({\left\\|\delta{\bm{\theta}}\right\\|}^{2})\leavevmode\nobreak\ ,$	(306)

最终导出一个标量等式和一个向量等式


$\displaystyle 0$	$\displaystyle=\delta{\bm{\omega}}_{G}^{\top}\delta{\bm{\theta}}+O(\|\delta{\bm{\theta}}\|^{2})$	(307a)
$\displaystyle\dot{\delta{\bm{\theta}}}$	$\displaystyle=\delta{\bm{\omega}}_{G}-\frac{1}{2}\left[\delta{\bm{\omega}}_{G}\right]_{\times}\delta{\bm{\theta}}+O({\left\\|\delta{\bm{\theta}}\right\\|}^{2}).$	(307b)

第一个方程得出 $\delta{\bm{\omega}}_{G}^{\top}\delta{\bm{\theta}}=O({\left\|\delta{\bm{\theta}}\right\|}^{2})$ ，它由二阶无穷小量构成，实用性不强。第二个方程在忽略所有二阶项后得到，

\dot{\delta{\bm{\theta}}}=\delta{\bm{\omega}}_{G}={\bf R}\delta{\bm{\omega}}\leavevmode\nobreak\ .

(308)

最后，回顾(254)，我们得到了全局角度误差的线性化动力学方程，

\boxed{\dot{\delta{\bm{\theta}}}=-{\bf R}\delta{\bm{\omega}}_{b}-{\bf R}{\bm{\omega}}_{n}}\ .

(309)

7.2 离散时间下的系统运动学

7.2.1 标称状态

标称状态方程不涉及误差项，因此与姿态误差在局部定义时的情况相同。

7.2.2 误差状态

采用欧拉积分法，我们得到以下差分方程组，


$\displaystyle\delta{\bf p}$	$\displaystyle\leftarrow\delta{\bf p}+\delta{\bf v}\,\Delta t$	(310a)
$\displaystyle\delta{\bf v}$	$\displaystyle\leftarrow\delta{\bf v}+(-\left[{\bf R}({\bf a}_{m}-{\bf a}_{b})\right]_{\times}\delta{\bm{\theta}}-{\bf R}\delta{\bf a}_{b}+\delta{\bf g})\Delta t+{\bf v}_{\bf i}$	(310b)
$\displaystyle\delta{\bm{\theta}}$	$\displaystyle\leftarrow\delta{\bm{\theta}}-{\bf R}\delta{\bm{\omega}}_{b}\Delta t+{\bm{\theta}}_{\bf i}$	(310c)
$\displaystyle\delta{\bf a}_{b}$	$\displaystyle\leftarrow\delta{\bf a}_{b}+{\bf a}_{\bf i}$	(310d)
$\displaystyle\delta{\bm{\omega}}_{b}$	$\displaystyle\leftarrow\delta{\bm{\omega}}_{b}+{\bm{\omega}}_{\bf i}$	(310e)
$\displaystyle\delta{\bf g}$	$\displaystyle\leftarrow\delta{\bf g}.$	(310f)

7.2.3 误差状态雅可比矩阵与扰动矩阵

通过直接观察上述方程即可获得状态转移矩阵，

{\bf F}_{\bf x}=\begin{bmatrix}{\bf I}&{\bf I}\Delta t&0&0&0&0\\ 0&{\bf I}&\boxed{-\left[{\bf R}({\bf a}_{m}-{\bf a}_{b})\right]_{\times}\Delta t}&-{\bf R}\Delta t&0&{\bf I}\Delta t\\ 0&0&\boxed{{\bf I}}&0&\boxed{-{\bf R}\Delta t}&0\\ 0&0&0&{\bf I}&0&0\\ 0&0&0&0&{\bf I}&0\\ 0&0&0&0&0&{\bf I}\\ \end{bmatrix}\leavevmode\nobreak\ .

(311)

我们观察到相对于局部定义的角误差情况有三处变化（将上述雅可比矩阵中的框选项与（270）式中的对应项进行比较）；这些变化总结在表 4 中。

在考虑各向同性噪声及附录 E 的推导后，扰动雅可比矩阵与扰动矩阵保持不变，

{\bf F}_{\bf i}=\begin{bmatrix}0&0&0&0\\ {\bf I}&0&0&0\\ 0&{\bf I}&0&0\\ 0&0&{\bf I}&0\\ 0&0&0&{\bf I}\\ 0&0&0&0\end{bmatrix}\quad,\quad{\bf Q}_{\bf i}=\begin{bmatrix}{\bf V}_{\bf i}&0&0&0\\ 0&{\bm{\Theta}}_{\bf i}&0&0\\ 0&0&{\bf A}_{\bf i}&0\\ 0&0&0&{\bm{\Omega}}_{\bf i}\end{bmatrix}\leavevmode\nobreak\ .

(312)

7.3 与互补传感器数据的融合

当考虑全局角度误差时，涉及误差状态卡尔曼滤波（ESKF）机制的融合方程仅需微调。我们通过 ESKF 校正环节修订了误差状态观测中的这些变化，包括将误差注入标称状态的过程以及重置步骤。

7.3.1 误差状态观测

与局部误差定义唯一的不同之处在于观测函数中涉及姿态与角度误差关系的雅可比矩阵块。下文将详细推导这一新的矩阵块。

利用（17–19）式及一阶近似 $\delta{\bf q}\rightarrow\begin{bmatrix}1\\ \frac{1}{2}\delta{\bm{\theta}}\end{bmatrix}$ ，四元数项 ${\bf Q}_{\delta{\bm{\theta}}}$ 可按如下方式推导得出，


$\displaystyle{\bf Q}_{\delta{\bm{\theta}}}\triangleq\left.\frac{\partial(\delta{\bf q}\otimes{\bf q})}{\partial\delta{\bm{\theta}}}\right\|_{{\bf q}}$	$\displaystyle=\left.\frac{\partial(\delta{\bf q}\otimes{\bf q})}{\partial\delta{\bf q}}\right\|_{{\bf q}}\left.\frac{\partial\delta{\bf q}}{\partial\delta{\bm{\theta}}}\right\|_{\hat{\delta{\bm{\theta}}}=0}$	(313a)
	$\displaystyle=[{\bf q}]_{R}\,\frac{1}{2}\begin{bmatrix}0&0&0\\ 1&0&0\\ 0&1&0\\ 0&0&1\\ \end{bmatrix}$	(313b)
	$\displaystyle=\frac{1}{2}\begin{bmatrix}-q_{x}&-q_{y}&-q_{z}\\ q_{w}&q_{z}&-q_{y}\\ -q_{z}&q_{w}&q_{x}\\ q_{y}&-q_{x}&q_{w}\\ \end{bmatrix}\leavevmode\nobreak\ .$	(313c)

7.3.2 观测误差向标称状态的注入

标称状态与误差状态的组合 ${\bf x}\leftarrow{\bf x}\oplus\hat{\delta{\bf x}}$ 表示如下，


$\displaystyle{\bf p}$	$\displaystyle\leftarrow{\bf p}+\delta{\bf p}$	(314a)
$\displaystyle{\bf v}$	$\displaystyle\leftarrow{\bf v}+\delta{\bf v}$	(314b)
$\displaystyle{\bf q}$	$\displaystyle\leftarrow{\bf q}\{\hat{\delta{\bm{\theta}}}\}\otimes{\bf q}$	(314c)
$\displaystyle{\bf a}_{b}$	$\displaystyle\leftarrow{\bf a}_{b}+\delta{\bf a}_{b}$	(314d)
$\displaystyle{\bm{\omega}}_{b}$	$\displaystyle\leftarrow{\bm{\omega}}_{b}+\delta{\bm{\omega}}_{b}$	(314e)
$\displaystyle{\bf g}$	$\displaystyle\leftarrow{\bf g}+\delta{\bf g}\leavevmode\nobreak\ .$	(314f)

其中仅四元数更新方程受到影响。这总结在表 4 中。

7.3.3 ESKF 重置

根据以下公式重置 ESKF 误差均值并更新协方差矩阵，

	$\displaystyle\hat{\delta{\bf x}}$	$\displaystyle\leftarrow 0$		(315)
	$\displaystyle{\bf P}$	$\displaystyle\leftarrow{\bf G}{\bf P}{\bf G}^{\top}$		(316)

其雅可比矩阵为

{\bf G}=\begin{bmatrix}{\bf I}_{6}&0&0\\ 0&{\bf I}+\left[\hat{\frac{1}{2}\delta{\bm{\theta}}}\right]_{\times}&0\\ 0&0&{\bf I}_{9}\end{bmatrix}

(317)

其非平凡项推导如下。我们的目标是获得新角误差 $\delta{\bm{\theta}}^{+}$ 相对于旧误差 $\delta{\bm{\theta}}$ 的表达式。我们考虑以下事实：

•

真实方位在误差重置时不会改变，即 ${\bf q}_{t}^{+}\equiv{\bf q}_{t}$ 。由此可得：

$\delta{\bf q}^{+}\otimes{\bf q}^{+}=\delta{\bf q}\otimes{\bf q}\leavevmode\nobreak\ .$ (318)
•

观测误差均值已被注入名义状态（参见(283c)和(123)）：

${\bf q}^{+}=\hat{\delta{\bf q}}\otimes{\bf q}\leavevmode\nobreak\ .$ (319)

结合这两个恒等式，我们得到了新姿态误差相对于旧误差及观测误差 $\hat{\delta{\bf q}}$ 的表达式。

\delta{\bf q}^{+}=\delta{\bf q}\otimes\hat{\delta{\bf q}}^{*}=[\hat{\delta{\bf q}}^{*}]_{R}\cdot\delta{\bf q}\leavevmode\nobreak\ .

(320)

考虑到上述恒等式可以展开为

\begin{bmatrix}1\\ \frac{1}{2}\delta{\bm{\theta}}^{+}\end{bmatrix}=\begin{bmatrix}1&\frac{1}{2}\hat{\delta{\bm{\theta}}}^{\top}\\ -\frac{1}{2}\hat{\delta{\bm{\theta}}}&{\bf I}+\left[\frac{1}{2}\hat{\delta{\bm{\theta}}}\right]_{\times}\end{bmatrix}\cdot\begin{bmatrix}1\\ \frac{1}{2}\delta{\bm{\theta}}\end{bmatrix}+\mathcal{O}({\left\|\delta{\bm{\theta}}\right\|}^{2})

(321)

由此得到一个标量方程和一个向量方程，


$\displaystyle\frac{1}{4}\hat{\delta{\bm{\theta}}}^{\top}\delta{\bm{\theta}}$	$\displaystyle=\mathcal{O}({\left\\|\delta{\bm{\theta}}\right\\|}^{2})$	(322a)
$\displaystyle\delta{\bm{\theta}}^{+}$	$\displaystyle=-\hat{\delta{\bm{\theta}}}+\left({\bf I}+\left[\frac{1}{2}\hat{\delta{\bm{\theta}}}\right]_{\times}\right)\delta{\bm{\theta}}+\mathcal{O}({\left\\|\delta{\bm{\theta}}\right\\|}^{2})$	(322b)

\boxed{\frac{\partial\delta{\bm{\theta}}^{+}}{\partial\delta{\bm{\theta}}}={\bf I}+\left[\frac{1}{2}\hat{\delta{\bm{\theta}}}\right]_{\times}}\leavevmode\nobreak\ .

(323)

与局部误差情况的差异总结在表 4 中。

表 4：与方向误差定义相关的算法修改。

上下文	项目	局部角度误差	全局角度误差
误差组合	${\bf q}_{t}$	${\bf q}_{t}={\bf q}\otimes\delta{\bf q}$	${\bf q}_{t}=\delta{\bf q}\otimes{\bf q}$
欧拉积分	$\partial\delta{\bf v}^{+}/\partial\delta{\bm{\theta}}$	$-{\bf R}\left[{\bf a}_{m}-{\bf a}_{b}\right]_{\times}\Delta t$	$-\left[{\bf R}({\bf a}_{m}-{\bf a}_{b})\right]_{\times}\Delta t$
	$\partial\delta{\bm{\theta}}^{+}/\partial\delta{\bm{\theta}}$	${\bf R}^{\top}\{({\bm{\omega}}_{m}-{\bm{\omega}}_{b})\Delta t\}$	${\bf I}$
	$\partial\delta{\bm{\theta}}^{+}/\partial\delta{\bm{\omega}}_{b}$	$-{\bf I}\Delta t$	$-{\bf R}\Delta t$
误差观测	${\bf Q}_{\delta{\bm{\theta}}}$	$\frac{1}{2}\begin{bmatrix}-q_{x}&-q_{y}&-q_{z}\\ q_{w}&-q_{z}&q_{y}\\ q_{z}&q_{w}&-q_{x}\\ -q_{y}&q_{x}&q_{w}\\ \end{bmatrix}$	$\frac{1}{2}\begin{bmatrix}-q_{x}&-q_{y}&-q_{z}\\ q_{w}&q_{z}&-q_{y}\\ -q_{z}&q_{w}&q_{x}\\ q_{y}&-q_{x}&q_{w}\\ \end{bmatrix}$
误差注入		${\bf q}\leftarrow{\bf q}\otimes{\bf q}\{\hat{\delta{\bm{\theta}}}\}$	${\bf q}\leftarrow{\bf q}\{\hat{\delta{\bm{\theta}}}\}\otimes{\bf q}$
误差重置	$\partial\delta{\bm{\theta}}^{+}/\partial\delta{\bm{\theta}}$	${\bf I}-\left[\frac{1}{2}\hat{\delta{\bm{\theta}}}\right]_{\times}$	${\bf I}+\left[\frac{1}{2}\hat{\delta{\bm{\theta}}}\right]_{\times}$

附录 A 龙格-库塔数值积分方法

我们的目标是积分如下形式的非线性微分方程

\dot{\bf x}=f(t,{\bf x})

(324)

在有限时间间隔 $\Delta t$ 内，为了将其转换为差分方程，即，

{\bf x}(t+\Delta t)={\bf x}(t)+\int_{t}^{t+\Delta t}f(\tau,{\bf x}(\tau))d\tau\leavevmode\nobreak\ ,

(325)

或者说，如果我们假设 $t_{n}=n\Delta t$ 和 ${\bf x}_{n}\triangleq{\bf x}(t_{n})$ ，

{\bf x}_{n+1}={\bf x}_{n}+\int_{n\Delta t}^{(n+1)\Delta t}f(\tau,{\bf x}(\tau))d\tau\leavevmode\nobreak\ .

(326)

最常用的方法族之一是龙格-库塔方法（以下简称 RK）。这些方法通过多次迭代来估计区间内的导数，然后利用该导数进行步长 $\Delta t$ 的积分。

在接下来的章节中，将介绍几种龙格-库塔方法，从最简单到最通用的形式，并按其最常用的名称进行命名。

注：此处所有材料均摘自英文维基百科的龙格-库塔方法条目。

A.1 欧拉方法

欧拉方法假设导数 $f(\cdot)$ 在区间内保持恒定，因此

\boxed{{\bf x}_{n+1}={\bf x}_{n}+\Delta t\!\cdot\!f(t_{n},{\bf x}_{n})\leavevmode\nobreak\ .}

(327)

作为一种通用的龙格-库塔方法，这对应于单阶段方法，其计算过程可描述如下：首先在初始点处计算导数，

k_{1}=f(t_{n},{\bf x}_{n})\leavevmode\nobreak\ ,

(328)

并利用它计算终点处的积分值，

{\bf x}_{n+1}={\bf x}_{n}+\Delta t\!\cdot\!k_{1}\leavevmode\nobreak\ .

(329)

A.2 中点方法

中点法假设导数为区间中点处的导数，并通过一次迭代计算 ${\bf x}$ 在该中点处的值，即

\boxed{{\bf x}_{n+1}={\bf x}_{n}+\Delta t\!\cdot\!f\Big{(}t_{n}+\frac{1}{2}\Delta t\ ,\ {\bf x}_{n}+\frac{1}{2}\Delta t\!\cdot\!f(t_{n},{\bf x}_{n})\Big{)}}\leavevmode\nobreak\ .

(330)

中点法可以解释为如下两步法。首先，使用欧拉法积分至中点，采用先前定义的 $k_{1}$ ，

	$\displaystyle k_{1}$	$\displaystyle=f(t_{n},{\bf x}_{n})$		(331)
	$\displaystyle{\bf x}(t_{n}+\tfrac{1}{2}\Delta t)$	$\displaystyle={\bf x}_{n}+\frac{1}{2}\Delta t\!\cdot\!k_{1}\leavevmode\nobreak\ .$		(332)

然后利用该值计算中点处的导数 $k_{2}$ ，从而完成积分

	$\displaystyle k_{2}$	$\displaystyle=f(t_{n}+\tfrac{1}{2}\Delta t\ ,\ {\bf x}(t_{n}+\tfrac{1}{2}\Delta t))$		(333)
	$\displaystyle{\bf x}_{n+1}$	$\displaystyle={\bf x}_{n}+\Delta t\!\cdot\!k_{2}\leavevmode\nobreak\ .$		(334)

A.3 龙格-库塔四阶方法

该方法通常简称为龙格-库塔法。该方法假设在区间的起点、中点和终点处对 $f()$ 进行评估取值，并通过四个阶段或迭代计算积分，依次获得四个导数 $k_{1}\dots k_{4}$ 。这些导数或斜率随后进行加权平均，得到区间内导数的四阶估计值。

相较于前述两种单步公式方法，RK4 方法更适合被描述为一个小型算法。RK4 积分步骤如下：

\boxed{{\bf x}_{n+1}={\bf x}_{n}+\frac{\Delta t}{6}\Big{(}k_{1}+2k_{2}+2k_{3}+k_{4}\Big{)}}\leavevmode\nobreak\ ,

(335)

即增量是通过假设一个斜率为各斜率 $k_{1},k_{2},k_{3},k_{4}$ 的加权平均来计算，

$\displaystyle k_{1}$	$\displaystyle=f(t_{n},{\bf x}_{n})$	(336)
$\displaystyle k_{2}$	$\displaystyle=f\Big{(}t_{n}+\frac{1}{2}\Delta t\ ,\ {\bf x}_{n}+\frac{\Delta t}{2}k_{1}\Big{)}$	(337)
$\displaystyle k_{3}$	$\displaystyle=f\Big{(}t_{n}+\frac{1}{2}\Delta t\ ,\ {\bf x}_{n}+\frac{\Delta t}{2}k_{2}\Big{)}$	(338)
$\displaystyle k_{4}$	$\displaystyle=f\Big{(}t_{n}+\Delta t\ ,\ {\bf x}_{n}+\Delta t\!\cdot\!k_{3}\Big{)}\leavevmode\nobreak\ .$	(339)

不同斜率的解释如下：

•

$k_{1}$ 为区间起点处的斜率，采用 ${\bf x}_{n}$ （欧拉法）；
•

$k_{2}$ 为区间中点处的斜率，采用 ${\bf x}_{n}+\tfrac{1}{2}\Delta t\!\cdot\!k_{1}$ （中点法）；
•

$k_{3}$ 再次表示中点处的斜率，但此时使用的是 ${\bf x}_{n}+\tfrac{1}{2}\Delta t\!\cdot\!k_{2}$ ；
•

$k_{4}$ 表示区间终点处的斜率，使用的是 ${\bf x}_{n}+\Delta t\!\cdot\!k_{3}$ 。

A.4 通用龙格-库塔方法

更精细的龙格-库塔方法是可能实现的，其目标在于减小误差和/或增强稳定性。这些方法的一般形式为

\boxed{{\bf x}_{n+1}={\bf x}_{n}+\Delta t\sum_{i=1}^{s}b_{i}k_{i}}\leavevmode\nobreak\ ,

(340)

其中

k_{i}=f\Big{(}t_{n}+\Delta t\!\cdot\!c_{i},{\bf x}_{n}+\Delta t\sum_{j=1}^{s}a_{ij}k_{j}\Big{)}\leavevmode\nobreak\ ,

(341)

即迭代次数（方法的阶数）为 $s$ ，平均权重由 $b_{i}$ 定义，计算时间点由 $c_{i}$ 确定，而斜率 $k_{i}$ 则通过值 $a_{ij}$ 计算得出。根据项 $a_{ij}$ 的结构不同，可以得到显式或隐式的 RK 方法。

•

在显式方法中，所有 $k_{i}$ 都是按顺序计算的，即仅使用先前计算的值。这意味着矩阵 $[a_{ij}]$ 是一个对角元为零的下三角矩阵( 即 $a_{ij}=0$ 对于 $j\geq i$ )。欧拉法、中点法和 RK4 法都属于显式方法。
•

隐式方法具有完整的 $[a_{ij}]$ 矩阵，需要通过求解线性方程组来确定所有 $k_{i}$ 。因此其计算成本更高，但相比显式方法能够提高精度和稳定性。

更多详细信息请参阅专业文档。

附录 B 闭式积分方法

在许多情况下，积分步骤可以得到闭式表达式。我们现在考虑一阶线性微分方程的情况，

\dot{\bf x}(t)={\bf A}\!\cdot\!{\bf x}(t)\leavevmode\nobreak\ ,

(342)

也就是说，该关系在区间内是线性且恒定的。在此类情况下，在区间 $[t_{n},t_{n}+\Delta t]$ 上的积分将得到

{\bf x}_{n+1}=e^{{\bf A}\!\cdot\!\Delta t}{\bf x}_{n}=\Phi{\bf x}_{n}\leavevmode\nobreak\ ,

(343)

其中， $\Phi$ 被称为转移矩阵。该转移矩阵的泰勒展开式为

\Phi=e^{{\bf A}\!\cdot\!\Delta t}={\bf I}+{\bf A}\Delta t+\frac{1}{2}{\bf A}^{2}\Delta t^{2}+\frac{1}{3!}{\bf A}^{3}\Delta t^{3}+\dots=\sum_{k=0}^{\infty}\frac{1}{k!}{\bf A}^{k}\Delta t^{k}\leavevmode\nobreak\ .

(344)

在针对已知的 ${\bf A}$ 实例展开此级数时，有时可以识别结果中的已知级数。这使得能够以闭合形式写出最终的积分结果。以下列举几个示例。

B.1 角度误差的积分

例如，考虑不含偏差和噪声的角误差动力学（即方程 238c 的简化形式），

\dot{\delta{\bm{\theta}}}=-\left[{\bm{\omega}}\right]_{\times}\delta{\bm{\theta}}

(345)

其转移矩阵可表示为泰勒级数展开式，

	$\displaystyle\Phi$	$\displaystyle=e^{-\left[{\bm{\omega}}\right]_{\times}\Delta t}$		(346)
		$\displaystyle={\bf I}-\left[{\bm{\omega}}\right]_{\times}\Delta t+\frac{1}{2}\left[{\bm{\omega}}\right]_{\times}^{2}\Delta t^{2}-\frac{1}{3!}\left[{\bm{\omega}}\right]_{\times}^{3}\Delta t^{3}+\frac{1}{4!}\left[{\bm{\omega}}\right]_{\times}^{4}\Delta t^{4}-\dots$		(347)

现定义旋转单位轴 ${\bm{\omega}}\Delta t\triangleq{\bf u}\Delta\theta$ 及其转角，并应用方程 75，通过合并同类项可得

$\displaystyle\Phi$	$\displaystyle={\bf I}-\left[{\bf u}\right]_{\times}\Delta\theta+\frac{1}{2}\left[{\bf u}\right]_{\times}^{2}\Delta\theta^{2}-\frac{1}{3!}\left[{\bf u}\right]_{\times}^{3}\Delta\theta^{3}+\frac{1}{4!}\left[{\bf y}\right]_{\times}^{4}\Delta\theta^{4}-\dots$
	$\displaystyle={\bf I}-\left[{\bf u}\right]_{\times}\left(\Delta\theta-\frac{\Delta\theta^{3}}{3!}+\frac{\Delta\theta^{5}}{5!}-\cdots\right)+\left[{\bf u}\right]_{\times}^{2}\left(\frac{\Delta\theta^{2}}{2!}-\frac{\Delta\theta^{4}}{4!}+\frac{\Delta\theta^{6}}{6!}-\cdots\right)$
	$\displaystyle={\bf I}-\left[{\bf u}\right]_{\times}\sin\Delta\theta+\left[{\bf u}\right]_{\times}^{2}(1-\cos\Delta\theta)\leavevmode\nobreak\ ,$	(348)

此为闭合形式的解析解。

该解对应于罗德里格斯旋转公式 77 所描述的旋转矩阵 $\Phi={\bf R}\{-{\bf u}\Delta\theta\}={\bf R}\{{\bm{\omega}}\Delta t\}^{\top}$ ，此结果亦可直接观察方程 346 并回顾方程 69 得出。因此我们将其写作最终闭合解形式，

\boxed{\Phi={\bf R}\{{\bm{\omega}}\Delta t\}^{\top}}\leavevmode\nobreak\ .

(349)

B.2 简化 IMU 示例

考虑由误差状态动力学支配的简化 IMU 驱动系统，


$\displaystyle\dot{\delta{\bf p}}$	$\displaystyle=\delta{\bf v}$	(350a)
$\displaystyle\dot{\delta{\bf v}}$	$\displaystyle=-{\bf R}\left[{\bf a}\right]_{\times}\,\delta{\bm{\theta}}$	(350b)
$\displaystyle\dot{\delta{\bm{\theta}}}$	$\displaystyle=-\left[{\bm{\omega}}\right]_{\times}\delta{\bm{\theta}}\leavevmode\nobreak\ ,$	(350c)

其中 $({\bf a},{\bm{\omega}})$ 代表 IMU 读数，我们忽略了重力和传感器偏差。该系统由状态向量和动态矩阵定义，

{\bf x}=\begin{bmatrix}\delta{\bf p}\\ \delta{\bf v}\\ \delta{\bm{\theta}}\end{bmatrix}\qquad{\bf A}=\begin{bmatrix}0&{\bf P}_{\bf v}&0\\ 0&0&{\bf V}_{\bm{\theta}}\\ 0&0&\Theta_{\bm{\theta}}\end{bmatrix}\leavevmode\nobreak\ .

(351)

随着

$\displaystyle{\bf P}_{\bf v}$	$\displaystyle={\bf I}$	(352)
$\displaystyle{\bf V}_{\bm{\theta}}$	$\displaystyle=-{\bf R}\left[{\bf a}\right]_{\times}$	(353)
$\displaystyle\Theta_{\bm{\theta}}$	$\displaystyle=-\left[{\bm{\omega}}\right]_{\times}$	(354)

其步长时间 $\Delta t$ 的积分为 ${\bf x}_{n+1}=e^{({\bf A}\Delta t)}\!\cdot\!{\bf x}_{n}=\Phi\!\cdot\!{\bf x}_{n}$ 。转移矩阵 $\Phi$ 允许进行泰勒展开（344），按 ${\bf A}\Delta t$ 的递增幂次展开。我们可以写出 ${\bf A}$ 的几个幂次项以展示其一般形式，

{\bf A}\!=\!\begin{bmatrix}0&{\bf P}_{\bf v}&0\\ 0&0&{\bf V}_{\bm{\theta}}\\ 0&0&\Theta_{\bm{\theta}}\end{bmatrix}\!,{\bf A}^{2}\!=\!\begin{bmatrix}0&0&{\bf P}_{v}{\bf V}_{\bm{\theta}}\\ 0&0&{\bf V}_{\bm{\theta}}\Theta_{\bm{\theta}}\\ 0&0&\Theta_{\bm{\theta}}^{2}\end{bmatrix}\!,{\bf A}^{3}\!=\!\begin{bmatrix}0&0&{\bf P}_{v}{\bf V}_{\bm{\theta}}\Theta_{\bm{\theta}}\\ 0&0&{\bf V}_{\bm{\theta}}\Theta_{\bm{\theta}}^{2}\\ 0&0&\Theta_{\bm{\theta}}^{3}\end{bmatrix}\!,{\bf A}^{4}\!=\!\begin{bmatrix}0&0&{\bf P}_{v}{\bf V}_{\bm{\theta}}\Theta_{\bm{\theta}}^{2}\\ 0&0&{\bf V}_{\bm{\theta}}\Theta_{\bm{\theta}}^{3}\\ 0&0&\Theta_{\bm{\theta}}^{4}\end{bmatrix}\!,

(355)

由此可见，当 $k>1$ 时，

{\bf A}^{k>1}=\begin{bmatrix}0&0&{\bf P}_{v}{\bf V}_{\bm{\theta}}\Theta_{\bm{\theta}}^{k-2}\\ 0&0&{\bf V}_{\bm{\theta}}\Theta_{\bm{\theta}}^{k-1}\\ 0&0&\Theta_{\bm{\theta}}^{k}\end{bmatrix}

(356)

我们可以观察到， ${\bf A}$ 的递增幂次项包含固定部分和 $\Theta_{\bm{\theta}}$ 的递增幂次。这些幂次可能导出封闭形式的解，如前一节所示。

我们将矩阵 $\Phi$ 划分如下，

\Phi=\begin{bmatrix}{\bf I}&\Phi_{{\bf p}{\bf v}}&\Phi_{{\bf p}{\bm{\theta}}}\\ 0&{\bf I}&\Phi_{{\bf v}{\bm{\theta}}}\\ 0&0&\Phi_{{\bm{\theta}}{\bm{\theta}}}\end{bmatrix}\leavevmode\nobreak\ ,

(357)

让我们逐步推进，逐一探索 $\Phi$ 的所有非零分块。

平凡对角项

从对角线上的前两项开始，如所示，它们是单位矩阵。

旋转对角项

接下来是旋转对角项 $\Phi_{{\bm{\theta}}{\bm{\theta}}}$ ，它将新的角度误差与旧的角度误差相关联。对该项进行完整的泰勒级数展开可得

\Phi_{{\bm{\theta}}{\bm{\theta}}}=\sum_{k=0}^{\infty}\frac{1}{k!}\Theta_{\bm{\theta}}^{k}\Delta t^{k}=\sum_{k=0}^{\infty}\frac{1}{k!}\left[-{\bm{\omega}}\right]_{\times}^{k}\Delta t^{k}\leavevmode\nobreak\ ,

(358)

正如我们在前一节所见，这对应于我们熟知的旋转矩阵，

\boxed{\Phi_{{\bm{\theta}}{\bm{\theta}}}={\bf R}\{{\bm{\omega}}\Delta t\}^{\top}}\leavevmode\nobreak\ .

(359)

位置-速度项

最简单的非对角项是 $\Phi_{{\bf p}{\bf v}}$ ，其表达式为

\boxed{\Phi_{{\bf p}{\bf v}}={\bf P}_{\bf v}\Delta t={\bf I}\Delta t}\leavevmode\nobreak\ .

(360)

速度-角度项

现在让我们转向项 $\Phi_{{\bf v},{\bm{\theta}}}$ ，通过写出其级数展开式，

\Phi_{{\bf v}{\bm{\theta}}}={\bf V}_{\bm{\theta}}\Delta t+\frac{1}{2}{\bf V}_{\bm{\theta}}\Theta_{\bm{\theta}}\Delta t^{2}+\frac{1}{3!}{\bf V}_{\bm{\theta}}\Theta_{\bm{\theta}}^{2}\Delta t^{3}+\cdots

(361)

\Phi_{{\bf v}{\bm{\theta}}}=\Delta t{\bf V}_{\bm{\theta}}({\bf I}+\frac{1}{2}\Theta_{\bm{\theta}}\Delta t+\frac{1}{3!}\Theta_{\bm{\theta}}^{2}\Delta t^{2}+\cdots)

(362)

这简化为

\Phi_{{\bf v}{\bm{\theta}}}=\Delta t{\bf V}_{\bm{\theta}}\left({\bf I}+\sum_{k\geq 1}\frac{(\Theta_{\bm{\theta}}\Delta t)^{k}}{(k+1)!}\right)

(363)

此时我们有两种选择。我们可以将级数截断至第一显著项，得到 $\Phi_{{\bf v}{\bm{\theta}}}={\bf V}_{\bm{\theta}}\Delta t$ ，但这将不是一个闭合形式。下一节将展示使用这种简化方法的结果。或者，我们可以提取公因子 ${\bf V}_{\bm{\theta}}$ 并重写为

\Phi_{{\bf v}{\bm{\theta}}}={\bf V}_{\bm{\theta}}\Sigma_{1}

(364)

随着

\Sigma_{1}={\bf I}\Delta t+\frac{1}{2}\Theta_{\bm{\theta}}\Delta t^{2}+\frac{1}{3!}\Theta_{\bm{\theta}}^{2}\Delta t^{3}+\cdots\leavevmode\nobreak\ .

(365)

该级数 $\Sigma_{1}$ 与我们为 $\Phi_{{\bm{\theta}}{\bm{\theta}}}$ 所写的级数（358）相似，但存在两点差异：

•

$\Theta_{\bm{\theta}}$ 在 $\Sigma_{1}$ 中的幂次与有理系数 $\tfrac{1}{k!}$ 不匹配，且与 $\Delta t$ 的幂次不一致。事实上，我们在此指出， $\Sigma_{1}$ 中的下标“1”表示每个成员中都缺少一个 $\Theta_{\bm{\theta}}$ 的幂次。
•

序列开头缺少了一些项。同样，下标“1”表明缺少了这样一项。

第一个问题可以通过将(75)式应用于(354)式来解决，从而得到恒等式

\Theta_{\bm{\theta}}=\frac{\left[{\bm{\omega}}\right]_{\times}^{3}}{{\left\|{\bm{\omega}}\right\|}^{2}}=\frac{-\Theta_{\bm{\theta}}^{3}}{{\left\|{\bm{\omega}}\right\|}^{2}}\leavevmode\nobreak\ .

(366)

该表达式使我们能够将级数中 $\Theta_{\bm{\theta}}$ 的指数增加二，并在 ${\bm{\omega}}\neq 0$ 条件下表示为

\Sigma_{1}={\bf I}\Delta t-\frac{\Theta_{\bm{\theta}}}{{\left\|{\bm{\omega}}\right\|}^{2}}\left(\frac{1}{2}\Theta_{\bm{\theta}}^{2}\Delta t^{2}+\frac{1}{3!}\Theta_{\bm{\theta}}^{3}\Delta t^{3}+\dots\right)\leavevmode\nobreak\ ,

(367)

否则为 $\Sigma_{1}={\bf I}\Delta t$ 。新级数中的所有幂次均与正确的系数相匹配。当然，如前所述，部分项存在缺失。这第二个问题可通过增补缺失项并代之以闭合形式的完整级数来解决。

\Sigma_{1}={\bf I}\Delta t-\frac{\Theta_{\bm{\theta}}}{{\left\|{\bm{\omega}}\right\|}^{2}}\left({\bf R}\{{\bm{\omega}}\Delta t\}^{\top}-{\bf I}-\Theta_{\bm{\theta}}\Delta t\right)\leavevmode\nobreak\ ,

(368)

这是一个在 ${\bm{\omega}}\neq 0$ 条件下有效的闭式解。因此，我们最终可以写成


$\displaystyle-{\bf R}\left[{\bf a}\right]_{\times}\Delta t$	$\displaystyle{\bm{\omega}}\to 0$	(369a)
$\displaystyle-{\bf R}\left[{\bf a}\right]_{\times}\left({\bf I}\Delta t+\frac{\left[{\bm{\omega}}\right]_{\times}}{{\left\\|{\bm{\omega}}\right\\|}^{2}}\left({\bf R}\{{\bm{\omega}}\Delta t\}^{\top}-{\bf I}+\left[{\bm{\omega}}\right]_{\times}\Delta t\right)\right)$	$\displaystyle{\bm{\omega}}\neq 0$	(369b)

位置-角度交叉项

让我们最终处理 $\Phi_{{\bf p}{\bm{\theta}}}$ 项。其泰勒展开式为，

\Phi_{{\bf p}{\bm{\theta}}}=\frac{1}{2}{\bf P}_{\bf v}{\bf V}_{\bm{\theta}}\Delta t^{2}+\frac{1}{3!}{\bf P}_{\bf v}{\bf V}_{\bm{\theta}}\Theta_{\bm{\theta}}\Delta t^{3}+\frac{1}{4!}{\bf P}_{\bf v}{\bf V}_{\bm{\theta}}\Theta_{\bm{\theta}}^{2}\Delta t^{4}+\cdots

(370)

我们提取出常数项后得到，

\Phi_{{\bf p}{\bm{\theta}}}={\bf P}_{\bf v}{\bf V}_{\bm{\theta}}\ \Sigma_{2}\leavevmode\nobreak\ ,

(371)

随着

\Sigma_{2}=\frac{1}{2}{\bf I}\Delta t^{2}+\frac{1}{3!}\Theta_{\bm{\theta}}\Delta t^{3}+\frac{1}{4!}\Theta_{\bm{\theta}}^{2}\Delta t^{4}+\cdots\leavevmode\nobreak\ .

(372)

此处我们注意到 $\Sigma_{2}$ 中的下标"2"具有如下解释：

•

该级数的每一项都缺少 $\Theta_{\bm{\theta}}$ 的两次幂
•

该级数的前两项缺失。

我们再次利用(366)来提升 $\Theta_{\bm{\theta}}$ 的指数

\Sigma_{2}=\frac{1}{2}{\bf I}\Delta t^{2}-\frac{1}{{\left\|{\bm{\omega}}\right\|}^{2}}\left(\frac{1}{3!}\Theta_{\bm{\theta}}^{3}\Delta t^{3}+\frac{1}{4!}\Theta_{\bm{\theta}}^{4}\Delta t^{4}+\cdots\right)\leavevmode\nobreak\ .

(373)

我们将不完整级数替换为其闭合形式，

\Sigma_{2}=\frac{1}{2}{\bf I}\Delta t^{2}-\frac{1}{{\left\|{\bm{\omega}}\right\|}^{2}}\left({\bf R}\{{\bm{\omega}}\Delta t\}^{\top}-{\bf I}-\Theta_{\bm{\theta}}\Delta t-\frac{1}{2}\Theta_{\bm{\theta}}^{2}\Delta t^{2}\right)\leavevmode\nobreak\ ,

(374)

最终得到结果


$\displaystyle-{\bf R}\left[{\bf a}\right]_{\times}\frac{\Delta t^{2}}{2}$	$\displaystyle{\bm{\omega}}\to 0$	(375a)
$\displaystyle-{\bf R}\left[{\bf a}\right]_{\times}\left(\frac{1}{2}{\bf I}\Delta t^{2}-\frac{1}{{\left\\|{\bm{\omega}}\right\\|}^{2}}\left({\bf R}\{{\bm{\omega}}\Delta t\}^{\top}-\sum_{k=0}^{2}\frac{(-\left[{\bm{\omega}}\right]_{\times}\Delta t)^{k}}{k!}\right)\right)$	$\displaystyle\omega\neq 0$	(375b)

B.3 完整 IMU 示例

为了将简化 IMU 示例中展示的方法推广到一般情况，我们需要更细致地考察完整 IMU 案例。

考虑完整 IMU 系统(238)，其可表述为

\dot{\delta{\bf x}}={\bf A}\delta{\bf x}+{\bf B}{\bf w}\leavevmode\nobreak\ ,

(376)

其离散时间积分需要用到转移矩阵

\Phi=\sum_{k=0}^{\infty}\frac{1}{k!}{\bf A}^{k}\Delta t^{k}={\bf I}+{\bf A}\Delta t+\frac{1}{2}{\bf A}^{2}\Delta t^{2}+\dots\leavevmode\nobreak\ ,

(377)

我们想要计算的。动态矩阵 ${\bf A}$ 是块稀疏的，其块结构可通过检查原始方程（238）轻松确定，

{\bf A}=\begin{bmatrix}0&{\bf P}_{\bf v}&0&0&0&0\\ 0&0&{\bf V}_{\bm{\theta}}&{\bf V}_{\bf a}&0&{\bf V}_{\bf g}\\ 0&0&\Theta_{\bm{\theta}}&0&\Theta_{\bm{\omega}}&0\\ 0&0&0&0&0&0\\ 0&0&0&0&0&0\\ 0&0&0&0&0&0\end{bmatrix}\leavevmode\nobreak\ .

(378)

如前所述，让我们写出 ${\bf A}$ 的若干幂次，

	$\displaystyle{\bf A}^{2}$	$\displaystyle=\begin{bmatrix}\leavevmode\nobreak\ \leavevmode\nobreak\ 0\leavevmode\nobreak\ \leavevmode\nobreak\ &\leavevmode\nobreak\ \leavevmode\nobreak\ 0\leavevmode\nobreak\ \leavevmode\nobreak\ &\leavevmode\nobreak\ {\bf P}_{\bf v}{\bf V}_{\bm{\theta}}\leavevmode\nobreak\ &{\bf P}_{\bf v}{\bf V}_{\bf a}&0&{\bf P}_{\bf v}{\bf V}_{\bf g}\\ 0&0&\leavevmode\nobreak\ {\bf V}_{\bm{\theta}}\Theta_{\bm{\theta}}\leavevmode\nobreak\ &0&\leavevmode\nobreak\ {\bf V}_{\bm{\theta}}\Theta_{\bm{\omega}}\leavevmode\nobreak\ &0\\ 0&0&\Theta_{\bm{\theta}}^{2}&0&\Theta_{\bm{\theta}}\Theta_{\bm{\omega}}&0\\ 0&0&0&0&0&0\\ 0&0&0&0&0&0\\ 0&0&0&0&0&0\end{bmatrix}$
	$\displaystyle{\bf A}^{3}$	$\displaystyle=\begin{bmatrix}\leavevmode\nobreak\ \leavevmode\nobreak\ 0\leavevmode\nobreak\ \leavevmode\nobreak\ &\leavevmode\nobreak\ \leavevmode\nobreak\ 0\leavevmode\nobreak\ \leavevmode\nobreak\ &{\bf P}_{\bf v}{\bf V}_{\bm{\theta}}\Theta_{\bm{\theta}}&\leavevmode\nobreak\ 0\leavevmode\nobreak\ &\leavevmode\nobreak\ {\bf P}_{\bf v}{\bf V}_{\bm{\theta}}\Theta_{\bm{\omega}}\leavevmode\nobreak\ &\leavevmode\nobreak\ \leavevmode\nobreak\ 0\leavevmode\nobreak\ \leavevmode\nobreak\ \\ 0&0&{\bf V}_{\bm{\theta}}\Theta_{\bm{\theta}}^{2}&0&{\bf V}_{\bm{\theta}}\Theta_{\bm{\theta}}\Theta_{\bm{\omega}}&0\\ 0&0&\Theta_{\bm{\theta}}^{3}&0&\Theta_{\bm{\theta}}^{2}\Theta_{\bm{\omega}}&0\\ 0&0&0&0&0&0\\ 0&0&0&0&0&0\\ 0&0&0&0&0&0\end{bmatrix}$
	$\displaystyle{\bf A}^{4}$	$\displaystyle=\begin{bmatrix}\leavevmode\nobreak\ \leavevmode\nobreak\ 0\leavevmode\nobreak\ \leavevmode\nobreak\ &\leavevmode\nobreak\ \leavevmode\nobreak\ 0\leavevmode\nobreak\ \leavevmode\nobreak\ &{\bf P}_{\bf v}{\bf V}_{\bm{\theta}}\Theta_{\bm{\theta}}^{2}&\leavevmode\nobreak\ 0\leavevmode\nobreak\ &{\bf P}_{\bf v}{\bf V}_{\bm{\theta}}\Theta_{\bm{\theta}}\Theta_{\bm{\omega}}&\leavevmode\nobreak\ \leavevmode\nobreak\ 0\leavevmode\nobreak\ \leavevmode\nobreak\ \\ 0&0&{\bf V}_{\bm{\theta}}\Theta_{\bm{\theta}}^{3}&0&{\bf V}_{\bm{\theta}}\Theta_{\bm{\theta}}^{2}\Theta_{\bm{\omega}}&0\\ 0&0&\Theta_{\bm{\theta}}^{4}&0&\Theta_{\bm{\theta}}^{3}\Theta_{\bm{\omega}}&0\\ 0&0&0&0&0&0\\ 0&0&0&0&0&0\\ 0&0&0&0&0&0\end{bmatrix}\leavevmode\nobreak\ .$

基本上，我们观察到以下现象，

•

${\bf A}$ 对角线上唯一的旋转项 $\Theta_{\bm{\theta}}$ ，在幂次序列 ${\bf A}^{k}$ 中向右上方传播。所有未受此传播影响的项均消失。这种传播在以下三个方面影响了序列 $\{{\bf A}^{k}\}$ 的结构：
•

${\bf A}$ 的幂次稀疏性在第3次幂后趋于稳定。也就是说，对于高于3次的 ${\bf A}$ 幂次，不再有新的非零块出现或消失。
•

左上角的 $3\times 3$ 块对应于前例中的简化 IMU 模型，与该示例相比未发生变化。因此，之前推导的闭式解仍然适用。
•

与陀螺仪偏差误差相关的项（第五列的那些项）引入了类似的 $\Theta_{\bm{\theta}}$ 幂级数，可采用简化示例中使用的相同技术求解。

此刻我们关注的是寻找一种通用方法，来构建转移矩阵 $\Phi$ 闭式元素的框架。让我们回顾先前关于级数 $\Sigma_{1}$ 和 $\Sigma_{2}$ 的论述。

•

下标对应于级数各项中 $\Theta_{\bm{\theta}}$ 的缺失幂次。
•

下标与级数起始处缺失的项数一致。

基于这些性质，我们引入由 ²⁷²⁷27Note that, being ${\bf X}$ a square matrix that is not necessarily invertible (as it is the case for ${\bf X}=\Theta_{\bm{\theta}}$ ), we are not allowed to rearrange the definition of $\Sigma_{n}$ with $\Sigma_{n}={\bf X}^{-n}\sum_{k=n}^{\infty}\frac{1}{k!}(y{\bf X})^{k}$ . 定义的级数 $\Sigma_{n}({\bf X},y)$ ，

\Sigma_{n}({\bf X},y)\triangleq\sum_{k=n}^{\infty}\frac{1}{k!}{\bf X}^{k-n}y^{k}=\sum_{k=0}^{\infty}\frac{1}{(k+n)!}{\bf X}^{k}y^{\,k+n}=y^{n}\sum_{k=0}^{\infty}\frac{1}{(k+n)!}{\bf X}^{k}y^{k}

(379)

其中求和从 $n$ 项开始，且各项缺少矩阵 ${\bf X}$ 的 $n$ 次幂。由此立即得出 $\Sigma_{1}$ 和 $\Sigma_{2}$ 满足

\Sigma_{n}=\Sigma_{n}(\Theta_{\bm{\theta}},\Delta t)\leavevmode\nobreak\ ,

(380)

且 $\Sigma_{0}={\bf R}\{{\bm{\omega}}\Delta t\}^{\top}$ 。现在可将转移矩阵（377）表示为这些级数的函数，

\Phi=\begin{bmatrix}{\bf I}&{\bf P}_{\bf v}\Delta t&{\bf P}_{\bf v}{\bf V}_{\bm{\theta}}\Sigma_{2}&\tfrac{1}{2}{\bf P}_{\bf v}{\bf V}_{\bf a}\Delta t^{2}&{\bf P}_{\bf v}{\bf V}_{\bm{\theta}}\Sigma_{3}{\bm{\theta}}_{\bm{\omega}}&\tfrac{1}{2}{\bf P}_{\bf v}{\bf V}_{\bf g}\Delta t^{2}\\ 0&{\bf I}&{\bf V}_{\bm{\theta}}\Sigma_{1}&{\bf V}_{\bf a}\Delta t&{\bf V}_{\bm{\theta}}\Sigma_{2}{\bm{\theta}}_{\bm{\omega}}&{\bf V}_{\bf g}\Delta t\\ 0&0&\Sigma_{0}&0&\Sigma_{1}{\bm{\theta}}_{\bm{\omega}}&0\\ 0&0&0&{\bf I}&0&0\\ 0&0&0&0&{\bf I}&0\\ 0&0&0&0&0&{\bf I}\\ \end{bmatrix}\leavevmode\nobreak\ .

(381)

我们的问题现已转化为寻找 $\Sigma_{n}$ 的通用闭式表达式。让我们观察目前已获得的闭式结果，

$\displaystyle\Sigma_{0}$	$\displaystyle={\bf R}\{{\bm{\omega}}\Delta t\}^{\top}$	(382)
$\displaystyle\Sigma_{1}$	$\displaystyle={\bf I}\Delta t-\frac{\Theta_{\bm{\theta}}}{{\left\\|{\bm{\omega}}\right\\|}^{2}}\left({\bf R}\{{\bm{\omega}}\Delta t\}^{\top}-{\bf I}-\Theta_{\bm{\theta}}\Delta t\right)$	(383)
$\displaystyle\Sigma_{2}$	$\displaystyle=\frac{1}{2}{\bf I}\Delta t^{2}-\frac{1}{{\left\\|{\bm{\omega}}\right\\|}^{2}}\left({\bf R}\{{\bm{\omega}}\Delta t\}^{\top}-{\bf I}-\Theta_{\bm{\theta}}\Delta t-\frac{1}{2}\Theta_{\bm{\theta}}^{2}\Delta t^{2}\right)\leavevmode\nobreak\ .$	(384)

为了推导 $\Sigma_{3}$ ，我们需要两次应用恒等式（366）（因为我们缺少三次幂，而每次应用（366）仅能使该数值增加二），从而得到

\Sigma_{3}=\frac{1}{3!}{\bf I}\Delta t^{3}+\frac{\Theta_{\bm{\theta}}}{{\left\|{\bm{\omega}}\right\|}^{4}}\left(\frac{1}{4!}\Theta_{\bm{\theta}}^{4}\Delta t^{4}+\frac{1}{5!}\Theta_{\bm{\theta}}^{5}\Delta t^{5}+\dots\right)\leavevmode\nobreak\ ,

(385)

这将导出

\Sigma_{3}=\frac{1}{3!}{\bf I}\Delta t^{3}+\frac{\Theta_{\bm{\theta}}}{{\left\|{\bm{\omega}}\right\|}^{4}}\left({\bf R}\{{\bm{\omega}}\Delta t\}^{\top}-{\bf I}-\Theta_{\bm{\theta}}\Delta t-\frac{1}{2}\Theta_{\bm{\theta}}^{2}\Delta t^{2}-\frac{1}{3!}\Theta_{\bm{\theta}}^{3}\Delta t^{3}\right)\leavevmode\nobreak\ .

(386)

通过仔细检查级数 $\Sigma_{0}\dots\Sigma_{3}$ ，我们现在可以推导出 $\Sigma_{n}$ 的通用闭式表达式，如下所示，


$\displaystyle\frac{1}{n!}{\bf I}\Delta t^{n}$	$\displaystyle{\bm{\omega}}\to 0$	(387a)
$\displaystyle{\bf R}\{{\bm{\omega}}\Delta t\}^{\top}$	$\displaystyle n=0$	(387b)
$\displaystyle\frac{1}{n!}{\bf I}\Delta t^{n}-\frac{(-1)^{\tfrac{n+1}{2}}\left[{\bm{\omega}}\right]_{\times}}{{\left\\|{\bm{\omega}}\right\\|}^{n+1}}\left({\bf R}\{{\bm{\omega}}\Delta t\}^{\top}-\sum_{k=0}^{n}\frac{(-\left[{\bm{\omega}}\right]_{\times}\Delta t)^{k}}{k!}\right)$	$\displaystyle n\text{ odd}$	(387c)
$\displaystyle\frac{1}{n!}{\bf I}\Delta t^{n}+\frac{(-1)^{\tfrac{n}{2}}}{{\left\\|{\bm{\omega}}\right\\|}^{n}}\left({\bf R}\{{\bm{\omega}}\Delta t\}^{\top}-\sum_{k=0}^{n}\frac{(-\left[{\bm{\omega}}\right]_{\times}\Delta t)^{k}}{k!}\right)$	$\displaystyle n\text{ even}$	(387d)

将 $\Sigma_{n},\ n\in\{0,1,2,3\}$ 的适当值代入（381）的对应位置后，转移矩阵 $\Phi$ 的最终结果便立即得出。

值得注意的是，这些出现在 $\Sigma_{n}$ 新表达式中的级数现在具有有限项数，因此可以实际计算。也就是说，只要 $n<\infty$ （这总是成立）， $\Sigma_{n}$ 的表达式就是闭合形式。在当前示例中，如(381)所示，我们有 $n\leq 3$ 。

附录 C 基于截断级数的近似计算方法

在前文章节中，我们已推导出以线性化误差状态形式表达的复杂 IMU 驱动动态系统状态转移矩阵的闭式解。虽然闭式解始终具有研究价值，但高阶误差对实际算法性能的影响程度尚不明确。这一观点对于 IMU 积分误差以较高频率被观测（并补偿）的系统尤为重要，例如视觉-惯性或 GPS-惯性融合方案。

本节我们设计了近似求解转移矩阵的方法。这些方法基于相同的假设前提——转移矩阵可表示为泰勒级数展开式，随后在最具显著性的项处截断该级数。截断操作既可针对整个系统实施，亦可分块进行。

C.1 系统级截断

C.1.1 一阶截断：有限差分法

一种典型且广泛适用于此类系统的积分方法是

\dot{\bf x}=f(t,{\bf x})

该方法基于有限差分法计算导数，即

\dot{\bf x}\triangleq\lim_{\delta t\to 0}\frac{{\bf x}(t+\delta t)-{\bf x}(t)}{\delta t}\approx\frac{{\bf x}_{n+1}-{\bf x}_{n}}{\Delta t}\textordfeminine.

(388)

这直接导致了

{\bf x}_{n+1}\approx{\bf x}_{n}+\Delta t\,f(t_{n},{\bf x}_{n})\leavevmode\nobreak\ ,

(389)

这正是欧拉积分方法的表述。在积分区间开始时对函数 $f()$ 进行线性化处理会导致

	${\bf x}_{n+1}\approx{\bf x}_{n}+\Delta t\,{\bf A}\,{\bf x}_{n}\leavevmode\nobreak\ ,$	(390a)
where ${\bf A}\triangleq\frac{\partial f}{\partial{\bf x}}{(t_{n},{\bf x}_{n})}$ is a Jacobian matrix. This is strictly equivalent to writing the exponential solution to the linearized differential equation and truncating the series at the linear term (i.e., the following relation is identical to the previous one),
	${\bf x}_{n+1}=e^{{\bf A}\Delta t}{\bf x}_{n}\approx({\bf I}+\Delta t\,{\bf A})\,{\bf x}_{n}\leavevmode\nobreak\ .$	(390b)

这意味着欧拉方法（附录 A.1）、有限差分法和一阶系统泰勒截断方法都是相同的。我们得到近似转移矩阵，

\boxed{\Phi\approx{\bf I}+\Delta t{\bf A}}\leavevmode\nobreak\ .

(391)

对于 B.2 节的简化 IMU 示例，有限差分法得到的近似转移矩阵为

\Phi\approx\begin{bmatrix}{\bf I}&{\bf I}\Delta t&0\\ 0&{\bf I}&-{\bf R}\left[{\bf a}\right]_{\times}\Delta t\\ 0&0&{\bf I}-\left[{\bm{\omega}}\Delta t\right]_{\times}\end{bmatrix}\leavevmode\nobreak\ .

(392)

然而，我们从 B.1 节已经知道旋转项具有紧凑的闭合形式解 $\Phi_{{\bm{\theta}}{\bm{\theta}}}={\bf R}({\bm{\omega}}\Delta t)^{\top}$ 。根据此解重写转移矩阵更为便利，

\Phi\approx\begin{bmatrix}{\bf I}&{\bf I}\Delta t&0\\ 0&{\bf I}&-{\bf R}\left[{\bf a}\right]_{\times}\Delta t\\ 0&0&{\bf R}\{{\bm{\omega}}\Delta t\}^{\top}\end{bmatrix}\leavevmode\nobreak\ .

(393)

C.1.2 N 阶截断

采用更高阶截断将提升近似转移矩阵的精度。特别值得关注的是能最大限度利用结果稀疏性的截断阶数，即超过该阶数后不再出现新非零项的情况。

对于第 B.2 节的简化 IMU 示例，该阶数为 2，结果为

{\bf\Phi}\approx{\bf I}+{\bf A}\Delta t+\frac{1}{2}{\bf A}^{2}\Delta t^{2}=\begin{bmatrix}{\bf I}&{\bf I}\Delta t&-\frac{1}{2}{\bf R}\left[{\bf a}\right]_{\times}\Delta t^{2}\\ 0&{\bf I}&-{\bf R}\left[{\bf a}\right]_{\times}({\bf I}-\frac{1}{2}\left[{\bm{\omega}}\right]_{\times}\Delta t)\Delta t\\ 0&0&{\bf R}\{{\bm{\omega}}\Delta t\}^{\top}\end{bmatrix}\leavevmode\nobreak\ .

(394)

在第 B.3 节的完整 IMU 示例中，阶数为 3，结果为

{\bf\Phi}\approx{\bf I}+{\bf A}\Delta t+\frac{1}{2}{\bf A}^{2}\Delta t^{2}+\frac{1}{6}{\bf A}^{3}\Delta t^{3}\leavevmode\nobreak\ ,

(395)

此处因篇幅限制未给出完整形式。读者可参阅第 B.3 节中 ${\bf A}$ 、 ${\bf A}^{2}$ 和 ${\bf A}^{3}$ 的表达式。

C.2 分块截断

将转移矩阵各分块的泰勒级数截断至首项显著项，即可获得与前述闭合形式相当接近的近似解。这意味着，不同于前文对 ${\bf A}$ 整体幂级数的截断处理，我们需对各分块单独考量。因此，截断分析需基于分块逐一进行。以下通过两个示例加以说明。

对于第 B.2 节的简化 IMU 示例，我们得到了级数 $\Sigma_{1}$ 和 $\Sigma_{2}$ ，可将其截断如下

	$\displaystyle\Sigma_{1}$	$\displaystyle={\bf I}\Delta t$	$\displaystyle+\frac{1}{2}\Theta_{\bm{\theta}}\Delta t^{2}$	$\displaystyle+\cdots$		$\displaystyle\approx{\bf I}\Delta t$			(396)
	$\displaystyle\Sigma_{2}$	$\displaystyle={}$	$\displaystyle\frac{1}{2}{\bf I}\Delta t^{2}$	$\displaystyle+\frac{1}{3!}\Theta_{\bm{\theta}}\Delta t^{3}$	$\displaystyle+\cdots$	$\displaystyle\approx\ \frac{1}{2}{\bf I}\Delta t^{2}$	$\displaystyle\leavevmode\nobreak\ .$		(397)

由此得到近似转移矩阵

\Phi\approx\begin{bmatrix}{\bf I}&{\bf I}\Delta t&-\frac{1}{2}{\bf R}\left[{\bf a}\right]_{\times}\Delta t^{2}\\ 0&{\bf I}&-{\bf R}\left[{\bf a}\right]_{\times}\Delta t\\ 0&0&{\bf R}({\bm{\omega}}\Delta t)^{\top}\end{bmatrix}\leavevmode\nobreak\ ,

(398)

该结果比前述系统全局一阶截断更精确（因为新增了右上角项），但仍易于推导计算，特别是与第 B 节给出的闭合形式相比。需注意我们已对最低阶项（即 $\Phi_{{\bm{\theta}}{\bm{\theta}}}={\bf R}({\bm{\omega}}\Delta t)^{\top}$ ）采用了闭合形式。

一般情况下，除 $\Sigma_{0}$ 外，其余 $\Sigma_{n}$ 均可仅取其级数首项近似，即

\boxed{\Sigma_{0}={\bf R}\{{\bm{\omega}}\Delta t\}^{\top}\leavevmode\nobreak\ ,\qquad\Sigma_{n>0}\approx\frac{1}{n!}{\bf I}\Delta t^{n}}\leavevmode\nobreak\ .

(399)

完整 IMU 示例中，将上述 $\Sigma_{n}$ 代入式(381)即可得到近似状态转移矩阵，

\Phi\approx\begin{bmatrix}{\bf I}&{\bf I}\Delta t&-\frac{1}{2}{\bf R}\left[{\bf a}\right]_{\times}\Delta t^{2}&-\tfrac{1}{2}{\bf R}\Delta t^{2}&\frac{1}{3!}{\bf R}\left[{\bf a}\right]_{\times}\Delta t^{3}&\tfrac{1}{2}{\bf I}\Delta t^{2}\\ 0&{\bf I}&-{\bf R}\left[{\bf a}\right]_{\times}\Delta t&-{\bf R}\Delta t&\frac{1}{2}{\bf R}\left[{\bf a}\right]_{\times}\Delta t^{2}&{\bf I}\Delta t\\ 0&0&{\bf R}\{{\bm{\omega}}\Delta t\}^{\top}&0&-{\bf I}\Delta t&0\\ 0&0&0&{\bf I}&0&0\\ 0&0&0&0&{\bf I}&0\\ 0&0&0&0&0&{\bf I}\\ \end{bmatrix}

(400)

其中（参见(238)）

{\bf a}={\bf a}_{m}-{\bf a}_{b}\leavevmode\nobreak\ ,\quad{\bm{\omega}}={\bm{\omega}}_{m}-{\bm{\omega}}_{b}\leavevmode\nobreak\ ,\quad{\bf R}={\bf R}\{{\bf q}\}\leavevmode\nobreak\ ,

此处我们已将矩阵块替换为其适当值（另见 238），

{\bf P}_{\bf v}={\bf I}\leavevmode\nobreak\ ,\quad{\bf V}_{\bm{\theta}}=-{\bf R}\left[{\bf a}\right]_{\times}\leavevmode\nobreak\ ,\quad{\bf V}_{\bf a}=-{\bf R}\leavevmode\nobreak\ ,\quad{\bf V}_{\bf g}={\bf I}\leavevmode\nobreak\ ,\quad\Theta_{\bm{\theta}}=-\left[{\bm{\omega}}\right]_{\times}\leavevmode\nobreak\ ,\quad\Theta_{\bm{\omega}}=-{\bf I}

该方法的一个轻微简化是将矩阵中每个块的阶数限制在某个最大值 $n$ 。对于 $n=1$ 的情况，

\Phi\approx\begin{bmatrix}{\bf I}&{\bf I}\Delta t&0&0&0&0\\ 0&{\bf I}&-{\bf R}\left[{\bf a}\right]_{\times}\Delta t&-{\bf R}\Delta t&0&{\bf I}\Delta t\\ 0&0&{\bf R}\{{\bm{\omega}}\Delta t\}^{\top}&0&-{\bf I}\Delta t&0\\ 0&0&0&{\bf I}&0&0\\ 0&0&0&0&{\bf I}&0\\ 0&0&0&0&0&{\bf I}\\ \end{bmatrix}\leavevmode\nobreak\ ,

(401)

即为欧拉方法；而当 $n=2$ 时，

\Phi\approx\begin{bmatrix}{\bf I}&{\bf I}\Delta t&-\frac{1}{2}{\bf R}\left[{\bf a}\right]_{\times}\Delta t^{2}&-\tfrac{1}{2}{\bf R}\Delta t^{2}&0&\tfrac{1}{2}{\bf I}\Delta t^{2}\\ 0&{\bf I}&-{\bf R}\left[{\bf a}\right]_{\times}\Delta t&-{\bf R}\Delta t&\frac{1}{2}{\bf R}\left[{\bf a}\right]_{\times}\Delta t^{2}&{\bf I}\Delta t\\ 0&0&{\bf R}\{{\bm{\omega}}\Delta t\}^{\top}&0&-{\bf I}\Delta t&0\\ 0&0&0&{\bf I}&0&0\\ 0&0&0&0&{\bf I}&0\\ 0&0&0&0&0&{\bf I}\\ \end{bmatrix}\leavevmode\nobreak\ .

(402)

对于 $n\geq 3$ 的情况则对应完整形式 400。

附录 D 基于龙格-库塔积分的状态转移矩阵

另一种近似求解状态转移矩阵的方法是采用龙格-库塔积分法。当动态矩阵 ${\bf A}$ 在积分区间内不能视为常数时，这种方法可能是必要的，即，

\dot{\bf x}(t)={\bf A}(t){\bf x}(t)\leavevmode\nobreak\ .

(403)

让我们重写连续时间和离散时间下定义同一系统的两个关系式。它们涉及动态矩阵 ${\bf A}$ 和转移矩阵 $\Phi$ ，

	$\displaystyle\dot{\bf x}(t)$	$\displaystyle={\bf A}(t)\!\cdot\!{\bf x}(t)$		(404)
	$\displaystyle{\bf x}(t_{n}+\tau)$	$\displaystyle=\Phi(t_{n}+\tau\|t_{n})\!\cdot\!{\bf x}(t_{n})\leavevmode\nobreak\ .$		(405)

这些方程允许我们通过以下两种方式展开 $\dot{\bf x}(t_{n}+\tau)$ （左展开与右展开，请注意小圆点表示时间导数），

$\displaystyle\dot{(\Phi(t_{n}+\tau\|t_{n}){\bf x}(t_{n}))}=$	$\displaystyle\boxed{\dot{\bf x}(t_{n}+\tau)}={\bf A}(t_{n}+\tau){\bf x}(t_{n}+\tau)$
$\displaystyle\dot{\Phi}(t_{n}+\tau\|t_{n}){\bf x}(t_{n})+\Phi(t_{n}+\tau\|t_{n})\dot{\bf x}(t_{n})=$	$\displaystyle\leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ \leavevmode\nobreak\ ={\bf A}(t_{n}+\tau)\Phi(t_{n}+\tau\|t_{n}){\bf x}(t_{n})$
$\displaystyle\dot{\Phi}(t_{n}+\tau\|t_{n}){\bf x}(t_{n})=$		(406)

此处（406）源于注意到 $\dot{\bf x}(t_{n})=\dot{\bf x}_{n}=0$ 是采样值。因此，

\dot{\Phi}(t_{n}+\tau|t_{n})={\bf A}(t_{n}+\tau)\Phi(t_{n}+\tau|t_{n})

(407)

这与方程（404）是相同的常微分方程，只是现在应用于转移矩阵而非状态向量。需注意，由于恒等式 ${\bf x}(t_{n})=\Phi_{t_{n}|t_{n}}{\bf x}(t_{n})$ 的存在，区间起始处的转移矩阵 $t=t_{n}$ 始终为单位矩阵。

\Phi_{t_{n}|t_{n}}={\bf I}\leavevmode\nobreak\ .

(408)

使用 RK4 与 $f(t,\Phi(t))={\bf A}(t)\Phi(t)$ 时，我们有

\Phi\triangleq\Phi(t_{n}+\Delta t|t_{n})={\bf I}+\frac{\Delta t}{6}({\bf K}_{1}+2{\bf K}_{2}+2{\bf K}_{3}+{\bf K}_{4})

(409)

随着

$\displaystyle{\bf K}_{1}$	$\displaystyle={\bf A}(t_{n})$	(410)
$\displaystyle{\bf K}_{2}$	$\displaystyle={\bf A}\Big{(}t_{n}+\frac{1}{2}\Delta t\Big{)}\Big{(}{\bf I}+\frac{\Delta t}{2}{\bf K}_{1}\Big{)}$	(411)
$\displaystyle{\bf K}_{3}$	$\displaystyle={\bf A}\Big{(}t_{n}+\frac{1}{2}\Delta t\Big{)}\Big{(}{\bf I}+\frac{\Delta t}{2}{\bf K}_{2}\Big{)}$	(412)
$\displaystyle{\bf K}_{4}$	$\displaystyle={\bf A}\Big{(}t_{n}+\Delta t\Big{)}\Big{(}{\bf I}+\Delta t\!\cdot\!{\bf K}_{3}\Big{)}\leavevmode\nobreak\ .$	(413)

D.1 误差状态示例

让我们考虑针对非线性时变系统的误差状态卡尔曼滤波器

\dot{\bf x}_{t}(t)=f(t,{\bf x}_{t}(t),{\bf u}(t))

(414)

其中 ${\bf x}_{t}$ 表示真实状态， ${\bf u}$ 为控制输入。该真实状态是由标称状态 ${\bf x}$ 与误差状态 $\delta{\bf x}$ 通过 $\oplus$ 运算构成的复合状态，

{\bf x}_{t}(t)={\bf x}(t)\oplus\delta{\bf x}(t)

(415)

其误差状态动力学呈现线性形式，且随时间变化取决于标称状态 ${\bf x}$ 与控制量 ${\bf u}$ ，即，

\dot{\delta{\bf x}}={\bf A}({\bf x}(t),{\bf u}(t))\!\cdot\!\delta{\bf x}

(416)

也就是说，式(403)中的误差状态动态矩阵具有 ${\bf A}(t)={\bf A}({\bf x}(t),{\bf u}(t))$ 的形式。误差状态转移矩阵的动力学可表述为，

\dot{\Phi}(t_{n}+\tau|t_{n})={\bf A}({\bf x}(t),{\bf u}(t))\!\cdot\!\Phi(t_{n}+\tau|t_{n})\leavevmode\nobreak\ .

(417)

为了对此方程进行龙格-库塔积分，我们需要在龙格-库塔评估点处的 ${\bf x}(t)$ 和 ${\bf u}(t)$ 值，对于 RK4 方法这些点是 $\{t_{n},t_{n}+\Delta t/2,t_{n}+\Delta t\}$ 。从简单的部分开始，评估点处的控制输入 ${\bf u}(t)$ 可通过当前与上次测量值的线性插值获得，

$\displaystyle{\bf u}(t_{n})$	$\displaystyle={\bf u}_{n}$	(418)
$\displaystyle{\bf u}(t_{n}+\Delta t/2)$	$\displaystyle=\frac{{\bf u}_{n}+{\bf u}_{n+1}}{2}$	(419)
$\displaystyle{\bf u}(t_{n}+\Delta t)$	$\displaystyle={\bf u}_{n+1}$	(420)

名义状态动力学应采用最佳可行的积分方法进行计算。例如，使用四阶龙格-库塔(RK4)积分法时，

	$\displaystyle{\bf k}_{1}$	$\displaystyle=f({\bf x}_{n},{\bf u}_{n})$
	$\displaystyle{\bf k}_{2}$	$\displaystyle=f({\bf x}_{n}+\frac{\Delta t}{2}{\bf k}_{1},\frac{{\bf u}_{n}+{\bf u}_{n+1}}{2})$
	$\displaystyle{\bf k}_{3}$	$\displaystyle=f({\bf x}_{n}+\frac{\Delta t}{2}{\bf k}_{2},\frac{{\bf u}_{n}+{\bf u}_{n+1}}{2})$
	$\displaystyle{\bf k}_{4}$	$\displaystyle=f({\bf x}_{n}+\Delta t{\bf k}_{3},{\bf u}_{n+1})$
	$\displaystyle{\bf k}$	$\displaystyle=({\bf k}_{1}+2{\bf k}_{2}+2{\bf k}_{3}+{\bf k}_{4})/6\leavevmode\nobreak\ ,$

由此得到评估点处的估计值，

$\displaystyle{\bf x}(t_{n})$	$\displaystyle={\bf x}_{n}$	(421)
$\displaystyle{\bf x}(t_{n}+\Delta t/2)$	$\displaystyle={\bf x}_{n}+\frac{\Delta t}{2}{\bf k}$	(422)
$\displaystyle{\bf x}(t_{n}+\Delta t)$	$\displaystyle={\bf x}_{n}+\Delta t\,{\bf k}\leavevmode\nobreak\ .$	(423)

我们注意到此处 ${\bf x}(t_{n}+\Delta t/2)=\frac{{\bf x}_{n}+{\bf x}_{n+1}}{2}$ 采用了与控制信号相同的线性插值方法。考虑到龙格-库塔更新的线性特性，这并不令人意外。

无论我们通过何种方式获得标称状态值，现在都可以计算用于转移矩阵积分的 RK4 矩阵，

	$\displaystyle{\bf K}_{1}$	$\displaystyle={\bf A}({\bf x}_{n},{\bf u}_{n})$
	$\displaystyle{\bf K}_{2}$	$\displaystyle={\bf A}\Big{(}{\bf x}_{n}+\frac{\Delta t}{2}{\bf k},\frac{{\bf u}_{n}+{\bf u}_{n+1}}{2}\Big{)}\Big{(}{\bf I}+\frac{\Delta t}{2}{\bf K}_{1}\Big{)}$
	$\displaystyle{\bf K}_{3}$	$\displaystyle={\bf A}\Big{(}{\bf x}_{n}+\frac{\Delta t}{2}{\bf k},\frac{{\bf u}_{n}+{\bf u}_{n+1}}{2}\Big{)}\Big{(}{\bf I}+\frac{\Delta t}{2}{\bf K}_{2}\Big{)}$
	$\displaystyle{\bf K}_{4}$	$\displaystyle={\bf A}\Big{(}{\bf x}_{n}+\Delta t{\bf k},{\bf u}_{n+1}\Big{)}\Big{(}{\bf I}+\Delta t{\bf K}_{3}\Big{)}$
	$\displaystyle{\bf K}$	$\displaystyle=({\bf K}_{1}+2{\bf K}_{2}+2{\bf K}_{3}+{\bf K}_{4})/6$

最终得到，

\boxed{\Phi\triangleq\Phi_{t_{n}+\Delta t|t_{n}}={\bf I}+\Delta t\,{\bf K}}

(424)

附录 E 随机噪声与扰动的积分方法

我们现在的目标是提供适当的方法来对动态系统中的随机变量进行积分。当然，我们无法对未知的随机值进行积分，但为了不确定性传播，我们可以对其方差和协方差进行积分。这对于建立连续特性系统（在连续时间中指定）但以离散方式估计的估计器中的协方差矩阵是必要的。

考虑连续时间动态系统，

\dot{\bf x}=f({\bf x},{\bf u},{\bf w})\leavevmode\nobreak\ ,

(425)

其中 ${\bf x}$ 为状态向量， ${\bf u}$ 是包含噪声 $\tilde{\bf u}$ 的控制信号向量，因此控制测量值为 ${\bf u}_{m}={\bf u}+\tilde{\bf u}$ ，而 ${\bf w}$ 为随机扰动向量。噪声与扰动均假定为白高斯过程，其特性由以下参数确定：

\tilde{\bf u}\sim\mathcal{N}\{0,{\bf U}^{c}\}\quad,\quad{\bf w}^{c}\sim\mathcal{N}\{0,{\bf W}^{c}\}\leavevmode\nobreak\ ,

(426)

其中上标 $\bullet^{c}$ 表示连续时间不确定性规范，这是我们希望进行积分的对象。

控制信号中的噪声水平 $\tilde{\bf u}$ 与随机扰动 ${\bf w}$ 在本质上存在重要差异：

•

在离散化过程中，控制信号在时间点 $n\Delta t$ 处采样，具有 ${\bf u}_{m,n}\triangleq{\bf u}_{m}(n\Delta t)={\bf u}(n\Delta t)+\tilde{\bf u}(n\Delta t)$ 。测量部分显然被视为在积分区间内保持恒定，即 ${\bf u}_{m}(t)={\bf u}_{m,n}$ ，因此采样时刻 $n\Delta t$ 的噪声水平也保持恒定，

$\tilde{\bf u}(t)=\tilde{\bf u}(n\Delta t)=\tilde{\bf u}_{n},\quad n\Delta t<t<(n+1)\Delta t\leavevmode\nobreak\ .$ (427)
•

扰动 ${\bf w}$ 从不被采样。

因此，这两个随机过程在 $\Delta t$ 上的积分结果存在差异。让我们具体分析这一现象。

连续时间误差状态动态方程（425）可线性化为

\dot{\delta{\bf x}}={\bf A}\delta{\bf x}+{\bf B}\tilde{\bf u}+{\bf C}{\bf w}\leavevmode\nobreak\ ,

(428)

随着

{\bf A}\triangleq\left.\frac{\partial f}{\partial\delta{\bf x}}\right|_{{\bf x},{\bf u}_{m}}\quad,\quad{\bf B}\triangleq\left.\frac{\partial f}{\partial\tilde{\bf u}}\right|_{{\bf x},{\bf u}_{m}}\quad,\quad{\bf C}\triangleq\left.\frac{\partial f}{\partial{\bf w}}\right|_{{\bf x},{\bf u}_{m}}\leavevmode\nobreak\ ,

(429)

并在采样周期 $\Delta t$ 内进行积分，得到

	$\displaystyle\delta{\bf x}_{n+1}$	$\displaystyle=\delta{\bf x}_{n}+\int_{n\Delta t}^{(n+1)\Delta t}\left({\bf A}\delta{\bf x}(\tau)+{\bf B}\tilde{\bf u}(\tau)+{\bf C}{\bf w}^{c}(\tau)\right)d\tau$		(430)
		$\displaystyle=\delta{\bf x}_{n}+\int_{n\Delta t}^{(n+1)\Delta t}{\bf A}\delta{\bf x}(\tau)d\tau+\int_{n\Delta t}^{(n+1)\Delta t}{\bf B}\tilde{\bf u}(\tau)d\tau+\int_{n\Delta t}^{(n+1)\Delta t}{\bf C}{\bf w}^{c}(\tau)d\tau$		(431)

该表达式包含三个性质迥异的项。可按如下方式进行积分：

1.

由附录 B 可知，动态部分经积分得到状态转移矩阵
其中Φ=e𝐀ΔtΦ=e^{{\bf A}\Delta t}可通过闭式解或不同精度等级的近似方法计算得出。
2.

由式(427)可得
这意味着测量噪声在采样后以确定性方式被积分，因为其在积分区间内的行为是已知的。
3.
根据概率论可知，连续高斯白噪声在时间间隔Δt 内的积分将产生离散高斯白噪声脉冲𝐰𝑛。我们注意到，与前述测量噪声不同，该扰动在积分区间内不具有确定性行为，因此必须采用随机积分方法处理。

因此，离散时间误差状态动态系统可表述为

\delta{\bf x}_{n+1}={\bf F}_{\bf x}\delta{\bf x}_{n}+{\bf F}_{\bf u}\tilde{\bf u}_{n}+{\bf F}_{\bf w}{\bf w}_{n}

(435)

其状态转移矩阵、控制矩阵及扰动矩阵分别由下式给出

{\bf F}_{\bf x}=\Phi=e^{{\bf A}\Delta t}\quad,\quad{\bf F}_{\bf u}={\bf B}\Delta t\quad,\quad{\bf F}_{\bf w}={\bf C}\quad,\quad

(436)

其中噪声与扰动水平定义为

\tilde{\bf u}_{n}\sim\mathcal{N}\{0,{\bf U}\}\quad,\quad{\bf w}_{n}\sim\mathcal{N}\{0,{\bf W}\}

(437)

随着

{\bf U}={\bf U}^{c}\quad,\quad{\bf W}={\bf W}^{c}\Delta t\leavevmode\nobreak\ .

(438)

表 5：积分对系统矩阵及协方差矩阵的影响。

描述	连续时间 $t$	离散时间 $n\Delta t$
状态	$\dot{\bf x}=f^{c}({\bf x},{\bf u},{\bf w})$	${\bf x}_{n+1}=f({\bf x}_{n},{\bf u}_{n},{\bf w}_{n})$
误差状态	$\dot{\delta{\bf x}}={\bf A}\delta{\bf x}+{\bf B}\tilde{\bf u}+{\bf C}{\bf w}$	$\delta{\bf x}_{n+1}={\bf F}_{\bf x}\delta{\bf x}_{n}+{\bf F}_{\bf u}\tilde{\bf u}_{n}+{\bf F}_{\bf w}{\bf w}_{n}$
系统矩阵	${\bf A}$	${\bf F}_{\bf x}=\Phi=e^{{\bf A}\Delta t}$
控制矩阵	${\bf B}$	${\bf F}_{\bf u}={\bf B}\Delta t$
扰动矩阵	${\bf C}$	${\bf F}_{\bf w}={\bf C}$
控制协方差	${\bf U}^{c}$	${\bf U}={\bf U}^{c}$
扰动协方差	${\bf W}^{c}$	${\bf W}={\bf W}^{c}\Delta t$

这些结果总结在表 5 中。扩展卡尔曼滤波的预测阶段将根据以下公式传播误差状态的均值与协方差矩阵：

$\displaystyle\hat{\delta{\bf x}}_{n+1}$	$\displaystyle={\bf F}_{\bf x}\hat{\delta{\bf x}}_{n}$	(439)
$\displaystyle{\bf P}_{n+1}$	$\displaystyle={\bf F}_{\bf x}{\bf P}_{n}{\bf F}_{\bf x}^{\top}+{\bf F}_{\bf u}{\bf U}{\bf F}_{\bf u}^{\top}+{\bf F}_{\bf w}{\bf W}{\bf F}_{\bf w}^{\top}$
	$\displaystyle=e^{{\bf A}\Delta t}{\bf P}_{n}(e^{{\bf A}\Delta t})^{\top}+\Delta t^{2}{\bf B}{\bf U}^{c}{\bf B}^{\top}+\Delta t{\bf C}{\bf W}^{c}{\bf C}^{\top}$	(440)

此处观察积分区间 $\Delta t$ 对协方差更新（440）三项的不同影响具有重要启示意义：动态误差项呈指数变化，测量误差项呈二次变化，而扰动误差项呈线性变化。

E.1 噪声与扰动脉冲

人们经常会遇到（例如在重用现有代码或解释其他作者的文档时）比我们这里使用的形式更简单的 EKF 预测方程，即：

{\bf P}_{n+1}={\bf F}_{\bf x}{\bf P}_{n}{\bf F}_{\bf x}^{\top}+{\bf Q}\leavevmode\nobreak\ .

(441)

这对应于一般的离散时间动态系统，

\delta{\bf x}_{n+1}={\bf F}_{\bf x}\delta{\bf x}_{n}+{\bf i}

(442)

其中

{\bf i}\sim\mathcal{N}\{0,{\bf Q}\}

(443)

是一个由随机（白噪声、高斯分布）脉冲组成的向量，这些脉冲在时刻 $t_{n+1}$ 直接叠加到状态向量上。矩阵 ${\bf Q}$ 被视为脉冲的协方差矩阵。根据我们之前的分析，应按下式计算该协方差矩阵：

{\bf Q}=\Delta t^{2}\,{\bf B}\,{\bf U}^{c}\,{\bf B}^{\top}+\Delta t\,{\bf C}\,{\bf W}^{c}\,{\bf C}^{\top}\leavevmode\nobreak\ .

(444)

当脉冲不作用于完整状态时（这种情况很常见），矩阵 ${\bf Q}$ 并非全对角矩阵且可能包含大量零元素。此时可写出等效形式：

\delta{\bf x}_{n+1}={\bf F}_{\bf x}\,\delta{\bf x}_{n}+{\bf F}_{\bf i}\,{\bf i}

(445)

随着

{\bf i}\sim\mathcal{N}\{0,{\bf Q}_{\bf i}\}\leavevmode\nobreak\ ,

(446)

其中矩阵 ${\bf F}_{\bf i}$ 仅将每个独立脉冲映射至其影响的状态向量部分。相应的协方差矩阵 ${\bf Q}_{\bf i}$ 则更小且为完全对角矩阵。具体示例请参阅下一节。在此情况下，误差状态卡尔曼滤波器（ESKF）的时间更新变为

	$\displaystyle\hat{\delta{\bf x}}_{n+1}$	$\displaystyle={\bf F}_{\bf x}\,\hat{\delta{\bf x}}_{n}$		(447)
	$\displaystyle{\bf P}_{n+1}$	$\displaystyle={\bf F}_{\bf x}\,{\bf P}_{n}\,{\bf F}_{\bf x}^{\top}+{\bf F}_{\bf i}\,{\bf Q}_{\bf i}\,{\bf F}_{\bf i}^{\top}\leavevmode\nobreak\ .$		(448)

显然，所有这些形式都是等价的，这可以从以下关于一般扰动 ${\bf Q}$ 的双重恒等式中得到验证

{\bf F}_{\bf i}\,{\bf Q}_{\bf i}\,{\bf F}_{\bf i}^{\top}=\boxed{{\bf Q}}=\Delta t^{2}\,{\bf B}\,{\bf U}^{c}\,{\bf B}^{\top}+\Delta t\,{\bf C}\,{\bf W}^{c}\,{\bf C}^{\top}\leavevmode\nobreak\ .

(449)

E.2 完整 IMU 示例

我们研究了一种用于惯性测量单元(IMU)的误差状态卡尔曼滤波器的构建方法。误差状态系统定义在(238)中，包含名义状态 ${\bf x}$ 、误差状态 $\delta{\bf x}$ 、含噪声的控制信号 ${\bf u}_{m}={\bf u}+\tilde{\bf u}$ 以及由以下公式指定的扰动 ${\bf w}$ ：

{\bf x}=\begin{bmatrix}{\bf p}\\ {\bf v}\\ {\bf q}\\ {\bf a}_{b}\\ {\bm{\omega}}_{b}\\ {\bf g}\end{bmatrix}\quad,\quad\delta{\bf x}=\begin{bmatrix}\delta{\bf p}\\ \delta{\bf v}\\ \delta{\bm{\theta}}\\ \delta{\bf a}_{b}\\ \delta{\bm{\omega}}_{b}\\ \delta{\bf g}\end{bmatrix}\quad,\quad{\bf u}_{m}=\begin{bmatrix}{\bf a}_{m}\\ {\bm{\omega}}_{m}\end{bmatrix}\quad,\quad\tilde{\bf u}=\begin{bmatrix}\tilde{\bf a}\\ \tilde{\bm{\omega}}\end{bmatrix}\quad,\quad{\bf w}=\begin{bmatrix}{\bf a}_{w}\\ {\bm{\omega}}_{w}\end{bmatrix}

(450)

在本文所考虑的 IMU 模型中，控制噪声对应于 IMU 测量中的加性噪声。这些扰动会影响偏置，从而产生其随机游走行为。动态矩阵、控制矩阵和扰动矩阵分别为(参见(428)、(378)和(238))：

{\bf A}=\begin{bmatrix}0&{\bf P}_{\bf v}&0&0&0&0\\ 0&0&{\bf V}_{\bm{\theta}}&{\bf V}_{\bf a}&0&{\bf V}_{\bf g}\\ 0&0&\Theta_{\bm{\theta}}&0&\Theta_{\bm{\omega}}&0\\ 0&0&0&0&0&0\\ 0&0&0&0&0&0\\ 0&0&0&0&0&0\end{bmatrix}\quad,\quad{\bf B}=\begin{bmatrix}0&0\\ -{\bf R}&0\\ 0&-{\bf I}\\ 0&0\\ 0&0\\ 0&0\\ \end{bmatrix}\quad,\quad{\bf C}=\begin{bmatrix}0&0\\ 0&0\\ 0&0\\ {\bf I}&0\\ 0&{\bf I}\\ 0&0\\ \end{bmatrix}

(451)

对于常规配置的三轴同类型加速度计与陀螺仪组合的 IMU 而言，噪声与扰动具有各向同性特性。其标准差按以下标量形式给出：

\sigma_{\tilde{\bf a}}\ [m/s^{2}]\quad,\quad\sigma_{\tilde{\bm{\omega}}}\ [rad/s]\quad,\quad\sigma_{{\bf a}_{w}}\ [m/s^{2}\sqrt{s}]\quad,\quad\sigma_{{\bm{\omega}}_{w}}\ [rad/s\sqrt{s}]

(452)

其协方差矩阵为纯对角矩阵，即

{\bf U}^{c}=\begin{bmatrix}\sigma_{\tilde{\bf a}}^{2}{\bf I}&0\\ 0&\sigma_{\tilde{\bm{\omega}}}^{2}{\bf I}\end{bmatrix}\qquad,\qquad{\bf W}^{c}=\begin{bmatrix}\sigma_{{\bf a}_{w}}^{2}{\bf I}&0\\ 0&\sigma_{{\bm{\omega}}_{w}}^{2}{\bf I}\end{bmatrix}\leavevmode\nobreak\ .

(453)

该系统以 $\Delta t$ 为间隔进行采样测量，遵循(435–438)式演化，其中转移矩阵 ${\bf F}_{\bf x}=\Phi$ 可通过多种方式计算——参见前文附录。

E.2.1 噪声与扰动脉冲

在采用脉冲形式的扰动规范 ${\bf i}$ 的情况下，我们可以如下重新定义系统：

\delta{\bf x}_{n+1}={\bf F}_{\bf x}({\bf x}_{n},{\bf u}_{m})\!\cdot\!\delta{\bf x}_{n}+{\bf F}_{\bf i}\!\cdot\!{\bf i}

(454)

其中名义状态、误差状态、控制及冲量向量分别定义为：

{\bf x}=\begin{bmatrix}{\bf p}\\ {\bf v}\\ {\bf q}\\ {\bf a}_{b}\\ {\bm{\omega}}_{b}\\ {\bf g}\end{bmatrix}\quad,\quad\delta{\bf x}=\begin{bmatrix}\delta{\bf p}\\ \delta{\bf v}\\ \delta{\bm{\theta}}\\ \delta{\bf a}_{b}\\ \delta{\bm{\omega}}_{b}\\ \delta{\bf g}\end{bmatrix}\quad,\quad{\bf u}_{m}=\begin{bmatrix}{\bf a}_{m}\\ {\bm{\omega}}_{m}\end{bmatrix}\quad,\quad{\bf i}=\begin{bmatrix}{\bf v}_{\bf i}\\ {\bm{\theta}}_{\bf i}\\ {\bf a}_{\bf i}\\ {\bm{\omega}}_{\bf i}\end{bmatrix}\leavevmode\nobreak\ ,

(455)

其转移矩阵和扰动矩阵定义为：

{\bf F}_{\bf x}=\Phi=e^{{\bf A}\Delta t}\qquad,\qquad{\bf F}_{\bf i}=\begin{bmatrix}0&0&0&0\\ {\bf I}&0&0&0\\ 0&{\bf I}&0&0\\ 0&0&{\bf I}&0\\ 0&0&0&{\bf I}\\ 0&0&0&0\end{bmatrix}\leavevmode\nobreak\ ,

(456)

以及由以下方差指定的脉冲

{\bf i}\sim\mathcal{N}\{0,{\bf Q}_{\bf i}\}\quad,\quad{\bf Q}_{\bf i}=\begin{bmatrix}\sigma_{\tilde{\bf a}}^{2}\Delta t^{2}{\bf I}&&0&\\ &\sigma_{\tilde{\bm{\omega}}}^{2}\Delta t^{2}{\bf I}&&\\ &&\sigma_{{\bf a}_{w}}^{2}\Delta t{\bf I}&\\ &0&&\sigma_{{\bm{\omega}}_{w}}^{2}\Delta t{\bf I}\end{bmatrix}\leavevmode\nobreak\ .

(457)

考虑到(451)中 ${\bf B}$ 的定义， ${\bf F}_{\bf i}$ 的平凡规范可能显得尤为令人惊讶。实际情况是，误差在 ${\bf Q}_{\bf i}$ 中被定义为各向同性的，因此 $-{\bf R}\sigma^{2}{\bf I}(-{\bf R})^{\top}=\sigma^{2}{\bf I}$ 和 $-{\bf I}\sigma^{2}{\bf I}(-{\bf I})^{\top}=\sigma^{2}{\bf I}$ 成立，从而导出了 ${\bf F}_{\bf i}$ 的表达式。但在考虑非各向同性 IMU 时，这种做法不可行，此时需要配合使用恰当的雅可比矩阵 ${\bf F}_{\bf i}=\begin{bmatrix}{\bf B}&{\bf C}\end{bmatrix}$ 与正确的 ${\bf Q}_{\bf i}$ 规范。

当然，我们也可以使用全状态扰动脉冲，

\delta{\bf x}_{n+1}={\bf F}_{\bf x}({\bf x}_{n},{\bf u}_{m})\!\cdot\!\delta{\bf x}_{n}+{\bf i}

(458)

随着

{\bf i}=\begin{bmatrix}0\\ {\bf v}_{\bf i}\\ {\bm{\theta}}_{\bf i}\\ {\bf a}_{\bf i}\\ {\bm{\omega}}_{\bf i}\\ 0\end{bmatrix}\quad,\quad{\bf i}\sim\mathcal{N}\{0,{\bf Q}\}\quad,\quad{\bf Q}=\begin{bmatrix}0&\\ &\sigma_{\tilde{\bf a}}^{2}\Delta t^{2}{\bf I}&&&0&\\ &&\sigma_{\tilde{\bm{\omega}}}^{2}\Delta t^{2}{\bf I}&&\\ &&&\sigma_{{\bf a}_{w}}^{2}\Delta t{\bf I}&\\ &0&&&\sigma_{{\bm{\omega}}_{w}}^{2}\Delta t{\bf I}\\ &&&&&0\end{bmatrix}\leavevmode\nobreak\ .

(459)

再见

References

Chirikjian, (2012) Chirikjian, G. S. (2012). Stochastic Models, Information Theory, and Lie Groups, volume 2: Analytic Methods and Modern Applications of Applied and Numerical Harmonic Analysis. Birkhäuser,, Basel.
Chou, (1992) Chou, J. (1992). Quaternion kinematic and dynamic differential equations. Robotics and Automation, IEEE Transactions on, 8(1):53–64.
Dam et al., (1998) Dam, E. B., Koch, M., and Lillholm, M. (1998). Quaternions, interpolation and animation. Technical report, Department of Computer Science, University of Copenhagen, Denmark.
Eberly, (2010) Eberly, D. (2010). Quaternion algebra and calculus. Technical report, Geometric Tools, LLC.
Kuipers, (1999) Kuipers, J. B. (1999). Quaternions and rotation sequences: a primer with applications to orbits, aerospace, and virtual reality. Princeton University Press., Princeton, N.J.
Li and Mourikis, (2012) Li, M. and Mourikis, A. (2012). Improving the accuracy of EKF-based visual-inertial odometry. In Robotics and Automation (ICRA), 2012 IEEE International Conference on, pages 828–835.
Li et al., (2014) Li, M., Yu, H., Zheng, X., and Mourikis, A. (2014). High-fidelity sensor modeling and self-calibration in vision-aided inertial navigation. In Robotics and Automation (ICRA), 2014 IEEE International Conference on, pages 409–416.
Lupton and Sukkarieh, (2009) Lupton, T. and Sukkarieh, S. (2009). Efficient integration of inertial observations into visual slam without initialization. In IEEE/RSJ Int. Conf. on Intelligent Robots and Systems.
Madyastha et al., (2011) Madyastha, V., Ravindra, V., Mallikarjunan, S., and Goyal, A. (2011). Extended Kalman Filter vs. Error State Kalman Filter for Aircraft Attitude Estimation. In AIAA Guidance, Navigation, and Control Conference, Portland, Oregon. American Institute of Aeronautics and Astronautics.
Martinelli, (2012) Martinelli, A. (2012). Vision and imu data fusion: Closed-form solutions for attitude, speed, absolute scale, and bias determination. IEEE Transactions on Robotics, 28(1):44 –60.
Piniés et al., (2007) Piniés, P., Lupton, T., Sukkarieh, S., and Tardós, J. D. (2007). Inertial aiding of inverse depth SLAM using a monocular camera. In Int. Conf. on Robotics and Automation.
Roussillon et al., (2011) Roussillon, C., Gonzalez, A., Solà, J., Codol, J. M., Mansard, N., Lacroix, S., and Devy, M. (2011). RT-SLAM: A generic and real-time visual SLAM implementation. In Crowley, J., Draper, B., and Thonnat, M., editors, Computer Vision Systems, volume 6962 of Lecture Notes in Computer Science, pages 31–40. Springer Berlin Heidelberg.
Shoemake, (1985) Shoemake, K. (1985). Animating rotation with quaternion curves. SIGGRAPH Comput. Graph., 19(3):245–254.
Shuster, (1993) Shuster, M. D. (1993). Survey of attitude representations. Journal of the Astronautical Sciences, 41:439–517.
Trawny and Roumeliotis, (2005) Trawny, N. and Roumeliotis, S. I. (2005). Indirect Kalman filter for 3D attitude estimation. Technical Report 2005-002, University of Minnesota, Dept. of Comp. Sci. & Eng.
Yazell, (2009) Yazell, D. (2009). Origins of the unusual space shuttle quaternion definition. In Aerospace Sciences Meetings, pages –. American Institute of Aeronautics and Astronautics.

	$\displaystyle{\bf x}_{\|\|}$	$\displaystyle={\bf u}\,({\left\\|{\bf x}\right\\|}\cos\alpha)={\bf u}\,{\bf u}^{\top}\,{\bf x}$
	$\displaystyle{\bf x}_{\bot}$	$\displaystyle={\bf x}-{\bf x}_{\|\|}={\bf x}-{\bf u}\,{\bf u}^{\top}\,{\bf x}\leavevmode\nobreak\ .$

用于误差状态卡尔曼滤波的四元数运动学

摘要

1 四元数的定义与性质

1.1 四元数的定义

1.1.1 四元数的替代表示法

1.2 四元数主要性质

1.2.1 加法

1.2.2 四元数乘积

1.2.3 单位元

1.2.4 共轭

1.2.5 范数

1.2.6 逆

1.2.7 单位或归一化四元数

1.3 四元数的附加性质

1.3.1 四元数交换子

1.3.2 纯四元数的乘积

1.3.3 纯四元数的自然幂

1.3.4 纯四元数的指数

1.3.5 一般四元数的指数

1.3.6 单位四元数的对数

1.3.7 一般四元数的对数

1.3.8 类型 𝐪tsuperscript{\bf q}^{t} 的指数形式

2 旋转与交叉关系

2.1 三维向量旋转公式

2.2 旋转群 S​O​(3)3SO(3)

2.3 旋转群与旋转矩阵

2.3.1 指数映射

2.3.2 大写指数映射

2.3.3 旋转矩阵与旋转向量：罗德里格斯旋转公式

2.3.4 对数映射

2.3.5 旋转作用

2.4 旋转群与四元数

2.4.1 指数映射

2.4.2 大写指数映射

2.4.3 四元数与旋转向量

2.4.4 对数映射

2.4.5 旋转作用

2.4.6 S​O​(3)3SO(3) 流形的双重覆盖

2.5 旋转矩阵与四元数

2.6 旋转合成

关于符号表示的说明

2.7 球面线性插值(SLERP)

方法一

注：

方法二

方法三

2.8 四元数与等斜旋转：揭秘其神奇之处

ℝ3superscript3{\mathbb{R}}^{3} 中的旋转：

ℝ4superscript4{\mathbb{R}}^{4} 中的旋转：

ℝ4superscript4{\mathbb{R}}^{4} 中的等斜旋转：

ℝ4superscript4{\mathbb{R}}^{4} 与 ℝ3superscript3{\mathbb{R}}^{3} 中的四元数旋转：

3 四元数约定：我的选择

3.1 四元数的不同变体

3.1.1 四元数分量的顺序

3.1.2 四元数代数的规范定义

3.1.3 旋转算子的函数

方向余弦矩阵

3.1.4 旋转算子的方向

4 扰动、导数与积分

4.1 S​O​(3)3SO(3) 中的加性与减性算子

加运算符

减运算符

4.2 四种可能的导数定义

4.2.1 向量空间到向量空间的函数

4.2.2 从 S​O​(3)3SO(3) 到 S​O​(3)3SO(3) 的函数

4.2.3 向量空间到 S​O​(3)3SO(3) 的函数

4.2.4 从 S​O​(3)3SO(3) 到向量空间的函数

4.3 旋转的有用及极其有用的雅可比矩阵

4.3.1 关于向量的雅可比矩阵

4.3.2 关于四元数的雅可比矩阵

4.3.3 S​O​(3)3SO(3) 的右雅可比矩阵

4.3.4 关于旋转向量的雅可比矩阵

4.3.5 旋转组合的雅可比矩阵

4.4 扰动、不确定性与噪声

4.4.1 局部扰动

4.4.2 全局扰动

4.5 时间导数

4.5.1 全局到局部关系

4.5.2 四元数乘积的时间导数

4.5.3 涉及导数的其他实用表达式

1.3.8 类型 ${\bf q}^{t}$ 的指数形式

2.2 旋转群 $SO(3)$

2.4.6 $SO(3)$ 流形的双重覆盖

${\mathbb{R}}^{3}$ 中的旋转：

${\mathbb{R}}^{4}$ 中的旋转：

${\mathbb{R}}^{4}$ 中的等斜旋转：

${\mathbb{R}}^{4}$ 与 ${\mathbb{R}}^{3}$ 中的四元数旋转：

4.1 $SO(3)$ 中的加性与减性算子

4.2.2 从 $SO(3)$ 到 $SO(3)$ 的函数

4.2.3 向量空间到 $SO(3)$ 的函数

4.2.4 从 $SO(3)$ 到向量空间的函数

4.3.3 $SO(3)$ 的右雅可比矩阵