重读经典《Quaternion kinematics for the error-state Kalman filter》

本文将介绍一篇关于 四元数运动学的误差卡尔曼滤波 经典论文。本文结构如下：

• 第1章四元数定义和性质介绍，包括：加法、减法、乘法（矩阵表示）、模、幂数、指数运算等。
• 第2章旋转群定义和性质介绍，包括：旋转矩阵表示旋转运算、四元数表示旋转运算、旋转矩阵和四元数转换。
• 第3章李群定义和性质介绍，包括：李群加减法运算、李群四种导数定义、常用雅可比矩阵、四元数导数。
• 第4章IMU运动学方程介绍，包括：局部坐标系统微分方程、全局坐标系统微分方程、全局坐标系统离散时刻运动学方程。
• 第5章IMU/GPS融合定位介绍，包括：融合定位预测、融合定位更新。

论文英文版链接：https://hal.archives-ouvertes.fr/hal-01122406/document （October 12, 2017）

论文中文版链接：https://blog.csdn.net/sunqin_csdn/article/details/108560582

---

1. 四元数定义和性质

1.1 四元数定义

四元数可以这样进行定义，假设有两个复数 $A=a+bi$ 和 $C=c+di$，则 $Q$ 可以写成 $Q=A+Cj$，并且规定 $k\triangleq ij$，那么则得到四元数 $Q$ 为：
$$Q=a+bi+cj+dk\in \mathbb{H}$$

其中 { a , b , c , d } ∈ R \{a, b, c, d\} \in \mathbb{R} {a,b,c,d}∈R， { i , j , k } \{i,j,k\} {i,j,k} 为三个虚数单位，三者并且满足以下性质：
$$i^2=j^2=k^2=ijk=-1$$

进一步，从中可以得出：
$$ij=-ji=k,jk=-kj=i,ki=-ik=j$$

四元数由实部和虚部组成，如果实部为零，则得到纯四元数，记为 ${\mathbb{H}}_p$：
$$Q=bi+cj+dk\in {\mathbb{H}}_p$$

为了计算方便，常把四元数写成标量和向量组合的形式，例如 $Q=q_w+{\mathbf{q}}_v$，其中 $q_w$ 是实部，${\mathbf{q}}_v=q_{x}i+q_{y}j+q_{z}k$ 为虚部，四元数也可以写成4维向量形式，即：
$$\mathbf{q}\triangleq \left[\begin{array}{l}{q}_w\\ {\mathbf{q}}_v\end{array}\right]=\left[\begin{array}{l}{q}_w\\ q_x\\ q_y\\ q_z\end{array}\right]$$

---

1.2 四元数主要性质

（1）加法：
$$\mathbf{p}\pm \mathbf{q}=\left[\begin{array}{l}{p}_w\\ {\mathbf{p}}_v\end{array}\right]\pm \left[\begin{array}{l}{q}_w\\ {\mathbf{q}}_v\end{array}\right]=\left[\begin{array}{l}{p}_w\pm q_w\\ {\mathbf{p}}_v\pm {\mathbf{q}}_v\end{array}\right]$$

（2）乘积：
$$\mathbf{p}\otimes \mathbf{q}=\left[\begin{array}{l}{p}_w{q}_w-p_x{q}_x-p_y{q}_y-p_z{q}_z\\ p_w{q}_x+p_x{q}_w+p_y{q}_z-p_z{q}_y\\ p_w{q}_y-p_x{q}_z+p_y{q}_w+p_z{q}_x\\ p_w{q}_z+p_x{q}_y-p_y{q}_x+p_z{q}_w\end{array}\right]$$

上式也可以写成标量和向量相乘的形式：

$$\mathbf{p}\otimes \mathbf{q}=\left[\begin{array}{c}{p}_w{q}_w-{\mathbf{p}}_v^{\top }{\mathbf{q}}_v\\ p_w{\mathbf{q}}_v+q_w{\mathbf{p}}_v+{\mathbf{p}}_v\times {\mathbf{q}}_v\end{array}\right]$$
这里需要注意的是，四元数相乘没有交换律，即 $\mathbf{p}\otimes \mathbf{q}\ne \mathbf{q}\otimes \mathbf{p}$。四元数相乘也可以写成矩阵和向量相乘的形式，即：
$${\mathbf{q}}_1\otimes {\mathbf{q}}_2=[{\mathbf{q}}_1{]}_L{\mathbf{q}}_2=[{\mathbf{q}}_2{]}_R{\mathbf{q}}_1$$

其中：
$$[\mathbf{q}{]}_L=\left[\begin{array}{cccc}{q}_w& -q_x& -q_y& -q_z\\ q_x& q_w& -q_z& q_y\\ q_y& q_z& q_w& -q_x\\ q_z& -q_y& q_x& q_w\end{array}\right],[\mathbf{q}{]}_R=\left[\begin{array}{cccc}{q}_w& -q_x& -q_y& -q_z\\ q_x& q_w& q_z& -q_y\\ q_y& -q_z& q_w& q_x\\ q_z& q_y& -q_x& q_w\end{array}\right]$$

或者也可以写成下面的形式：
$$[\mathbf{q}{]}_L=q_w\mathbf{I}+\left[\begin{array}{cc}0& -{\mathbf{q}}_v^{\top }\\ {\mathbf{q}}_v& {\left[{\mathbf{q}}_v\right]}_{\times }\end{array}\right],[\mathbf{q}{]}_R=q_w\mathbf{I}+\left[\begin{array}{cc}0& -{\mathbf{q}}_v^{\top }\\ {\mathbf{q}}_v& -{\left[{\mathbf{q}}_v\right]}_{\times }\end{array}\right]$$

反对称矩阵 $[\bullet {]}_{\times }$ 定义如下，$[{\mathbf{a}}_{\times }{]}^T=-[\mathbf{a}{]}_{\times }$，并且有 $[\mathbf{a}{]}_{\times }\mathbf{b}=\mathbf{a}\times \mathbf{b}$。不过有时反对称矩阵符号也会写成 ${\mathbf{a}}^{\wedge }$ 的形式。
$$[\mathbf{a}{]}_{\times }\triangleq \left[\begin{array}{ccc}0& -a_z& a_y\\ a_z& 0& -a_x\\ -a_y& a_x& 0\end{array}\right]$$

因此，四元数相乘可以写成：
$$\begin{array}{rl}\mathbf{q}\otimes \mathbf{x}\otimes \mathbf{p}& =(\mathbf{q}\otimes \mathbf{x})\otimes \mathbf{p}=[\mathbf{p}{]}_R[\mathbf{q}{]}_L\mathbf{x}\\ & =\mathbf{q}\otimes (\mathbf{x}\otimes \mathbf{p})=[\mathbf{q}{]}_L[\mathbf{p}{]}_R\mathbf{x}\end{array}$$

可以得到;
$$[\mathbf{p}{]}_R[\mathbf{q}{]}_L=[\mathbf{q}{]}_L[\mathbf{p}{]}_R$$

（3）同一性：
$${\mathbf{q}}_1=1=\left[\begin{array}{c}1\\ 0_v\end{array}\right]$$

（4）共轭四元数：
$${\mathbf{q}}^{\ast }\triangleq q_w-{\mathbf{q}}_v=\left[\begin{array}{c}{q}_w\\ -{\mathbf{q}}_v\end{array}\right]$$

可以得到：
$$\mathbf{q}\otimes {\mathbf{q}}^{\ast }={\mathbf{q}}^{\ast }\otimes \mathbf{q}=q_w^2+q_x^2+q_y^2+q_z^2=\left[\begin{array}{c}{q}_w^2+q_x^2+q_y^2+q_z^2\\ 0_v\end{array}\right]$$

同时有 $(\mathbf{p}\otimes \mathbf{q}{)}^{\ast }={\mathbf{q}}^{\ast }\otimes {\mathbf{p}}^{\ast }$。

（5）模：

$$\Vert \mathbf{q}\Vert \triangleq \sqrt{\mathbf{q}\otimes {\mathbf{q}}^{\ast }}=\sqrt{{\mathbf{q}}^{\ast }\otimes \mathbf{q}}=\sqrt{q_w^2+q_x^2+q_y^2+q_z^2}\in \mathbb{R}$$

可以得到：$$\Vert \mathbf{p}\otimes \mathbf{q}\Vert =\Vert \mathbf{q}\otimes \mathbf{p}\Vert =\Vert \mathbf{p}\Vert \Vert \mathbf{q}\Vert$$

（6）逆：
$${\mathbf{q}}^{-1}={\mathbf{q}}^{\ast }/\Vert \mathbf{q}{\Vert }^2$$

---

1.3 四元数其余性质

（1）从上面的公式可以得到：
$${\mathbf{p}}_v\otimes {\mathbf{q}}_v-{\mathbf{q}}_v\otimes {\mathbf{p}}_v=2{\mathbf{p}}_v\times {\mathbf{q}}_v$$

（2）纯四元数相乘：
$${\mathbf{p}}_v\otimes {\mathbf{q}}_v=-{\mathbf{p}}_v^{\top }{\mathbf{q}}_v+{\mathbf{p}}_v\times {\mathbf{q}}_v=\left[\begin{array}{c}-{\mathbf{p}}_v^{\top }{\mathbf{q}}_v\\ {\mathbf{p}}_v\times {\mathbf{q}}_v\end{array}\right]$$

可以得到：
$${\mathbf{q}}_v\otimes {\mathbf{q}}_v=-{\mathbf{q}}_v^{\top }{\mathbf{q}}_v=-{\Vert {\mathbf{q}}_v\Vert }^2$$

如果 $\mathbf{u}$ 是单位四元数，则有;
$$\mathbf{u}\otimes \mathbf{u}=-1$$

（3）纯四元数幂次方：

如果 $\mathbf{v}=\mathbf{u}\theta$ 是纯四元数，且 $\mathbf{u}$ 是单位四元数，$\theta =||\mathbf{v}||$，则 $\mathbf{v}$ 的幂次方形式为：
$${\mathbf{v}}^2=-{\theta }^2,{\mathbf{v}}^3=-\mathbf{u}{\theta }^3,{\mathbf{v}}^4={\theta }^4,{\mathbf{v}}^5=\mathbf{u}{\theta }^5,{\mathbf{v}}^6=-{\theta }^6$$

对于纯单位四元数 $\mathbf{u}$，可以得到：
$${\mathbf{u}}^2=-1,{\mathbf{u}}^3=-\mathbf{u},{\mathbf{u}}^4=1,{\mathbf{u}}^5=\mathbf{u},{\mathbf{u}}^6=-1$$

(4) 纯四元数指数：

四元数指数形式为：
$$e^{\mathbf{q}}\triangleq \sum _{k=0}^{\infty }\frac{1}{k!}{\mathbf{q}}^k\in \mathbb{H}$$

对于纯四元数 $\mathbf{v}=\mathbf{u}\theta$ ，$\theta =||\mathbf{v}||$。根据幂次方形式可以得到其指数形式为：
$$e^{\mathbf{u}\theta }=\left(1-\frac{{\theta }^2}{2!}+\frac{{\theta }^4}{4!}+\cdots \right)+\left(\mathbf{u}\theta -\frac{\mathbf{u}{\theta }^3}{3!}+\frac{\mathbf{u}{\theta }^5}{5!}+\cdots \right)$$

最终，可以得到其表达形式为：
$$e^{\mathbf{v}}=e^{\mathbf{u}\theta }=\cos \theta +\mathbf{u}\sin \theta =\left[\begin{array}{c}\cos \theta \\ \mathbf{u}\sin \theta \end{array}\right]$$

这就是四元数指数形式对应的欧拉方程。同时也可以得到：
$$e^{-\mathbf{v}}={\left(e^{\mathbf{v}}\right)}^{\ast }$$

---

2. 旋转群、旋转矩阵、四元数

2.1 旋转方程与旋转群 $SO(3)$

假设有向量 $\mathbf{x}$ 绕着轴 $\mathbf{u}$ 旋转，角度为 $\varphi$。则可以得到旋转后的向量${\mathbf{x}}^{\prime }$为：
$${\mathbf{x}}^{\prime }={\mathbf{x}}_{\Vert }+{\mathbf{x}}_{\perp }\cos \varphi +(\mathbf{u}\times \mathbf{x})\sin \varphi$$
这就是旋转方程，后面我们会进行证明。

下面给出旋转群 $SO(3)$ 的定义：
$$SO(3):\left\{r:{\mathbb{R}}^3\to {\mathbb{R}}^3/\forall \mathbf{v},\mathbf{w}\in {\mathbb{R}}^3,\Vert r(\mathbf{v})\Vert =\Vert \mathbf{v}\Vert ,r(\mathbf{v})\times r(\mathbf{w})=r(\mathbf{v}\times \mathbf{w})\right\}$$

可以看到，一个向量 $\mathbf{v}$ 旋转后，其模并不改变，向量间的角度也不会改变。

---

2.2 旋转群和旋转矩阵

（1）旋转矩阵基本性质

已知一个旋转矩阵 $\mathbf{R}\in {\mathbb{R}}^{3\times 3}$，定义旋转操作为 $r(\mathbf{v})=\mathbf{R}\mathbf{v}$。因为旋转不改变向量的模，可以得到：
$$(\mathbf{R}\mathbf{v}{)}^{\top }(\mathbf{R}\mathbf{v})={\mathbf{v}}^{\top }{\mathbf{R}}^{\top }\mathbf{R}\mathbf{v}={\mathbf{v}}^{\top }\mathbf{v}$$

得到旋转矩阵的正交性：
$${\mathbf{R}}^{\top }\mathbf{R}=\mathbf{I}=\mathbf{R}{\mathbf{R}}^{\top }$$

从上式可以得到，旋转矩阵的逆是其的转置 ${\mathbf{R}}^{-1}={\mathbf{R}}^{\top }$，旋转矩阵行列式为 $\det (\mathbf{R})=1$。

（2）旋转矩阵指数映射

上式两边求导：

$$\frac{d}{dt}\left({\mathbf{R}}^{\top }\mathbf{R}\right)={\dot{\mathbf{R}}}^{\top }\mathbf{R}+{\mathbf{R}}^{\top }\dot{\mathbf{R}}=0$$

可以得到;

$${\mathbf{R}}^{\top }\dot{\mathbf{R}}=-{\left({\mathbf{R}}^{\top }\dot{\mathbf{R}}\right)}^{\top }$$

可以看出 ${\mathbf{R}}^{\top }\dot{\mathbf{R}}$ 是一个反对称矩阵。反对称的 $3\times 3$ 矩阵可以用符号 $\mathfrak{s}\mathfrak{o}(3)$表示，也称为群 $SO3$ 的李代数。

定义向量 $\mathbf{\omega }\in {\mathbb{R}}^3\leftrightarrow [\mathbf{\omega }{]}_{\times }\in \mathfrak{s}\mathfrak{o}(3)$，则上式可以写为：
$$\dot{\mathbf{R}}=\mathbf{R}[\mathbf{\omega }{]}_{\times }$$

在原点，旋转矩阵 $\mathbf{R}=\mathbf{I}$，上式简化为 $\dot{\mathbf{R}}=[\mathbf{\omega }{]}_{\times }$。表示李代数为旋转矩阵在原点的导数。

如果 $\mathbf{\omega }$ 是常数，上式微分方程的积分形式可以写为：
$$\mathbf{R}(t)=\mathbf{R}(0)e^{[\mathbf{\omega }{]}_{\times }t}=\mathbf{R}(0)e^{[\omega t{]}_{\times }}$$
定义向量 $\varphi \triangleq \omega \Delta t$，则旋转矩阵 $\mathbf{R}$ 可以写成：
$$\mathbf{R}=e^{[\varphi {]}_{\times }}$$

这就是旋转矩阵的指数映射，从李代数 $\mathfrak{s}\mathfrak{o}(3)$ 到群 $SO(3)$：

$$\exp :\mathfrak{s}\mathfrak{o}(3)\to SO(3);[\mathbf{\varphi }{]}_{\times }\mapsto \exp \left([\mathbf{\varphi }{]}_{\times }\right)=e^{[\mathbf{\varphi }{]}_{\times }}$$

也可以写成从实数 ${\mathbb{R}}^3$ 到群 $SO(3)$的映射。
$$\mathrm{Exp}:{\mathbb{R}}^3\to SO(3);\varphi \mapsto \mathrm{Exp}(\varphi )=e^{[\varphi {]}_{\times }}$$

实数、李代数数、旋转群三者之间关系如下图所示：

(3) 旋转矩阵与旋转向量：罗德里格斯公式

已知旋转 $\mathbf{R}$ 为：

且旋转轴 $\mathbf{u}$ 满足 $[\mathbf{u}{]}_{\times }^2=\mathbf{u}{\mathbf{u}}^{\top }-\mathbf{I}，[\mathbf{u}{]}_{\times }^3=-{\mathbf{u}}_{\times }$。最终上式可以写成如下形式，也被称为罗德里格斯公式。
$$\mathbf{R}=\mathbf{I}+\sin \varphi [\mathbf{u}{]}_{\times }+(1-\cos \varphi )[\mathbf{u}{]}_{\times }^2$$

指数映射的逆运算为对数映射，定义为：
$$\log :SO(3)\to \mathfrak{s}\mathfrak{o}(3);\mathbf{R}\mapsto \log (\mathbf{R})=[\mathbf{u}\varphi {]}_{\times }$$
其中：
$$\varphi =\arccos (\frac{trace(\mathbf{R})-1}{2})$$

$$\mathbf{u}=\frac{(\mathbf{R}-{\mathbf{R}}^{\top }{)}^{\vee }}{2\sin \varphi }$$

​其中 $[\bullet {]}^{\vee }$ 是反对称矩阵的逆操作。

(4) 旋转运算

回到最开始的问题， 向量 $\mathbf{x}$ 绕轴 $\mathbf{u}$ 转动角度 $\varphi$ 之后的向量 ${\mathbf{x}}^{\prime }$ 为：
$$\begin{array}{rl}{\mathbf{x}}^{\prime }& =\mathbf{R}\mathbf{x}\\ & =\left(\mathbf{I}+\sin \varphi [\mathbf{u}{]}_{\times }+(1-\cos \varphi )[\mathbf{u}{]}_{\times }^2\right)\mathbf{x}\\ & =\mathbf{x}+\sin \varphi [\mathbf{u}{]}_{\times }\mathbf{x}+(1-\cos \varphi )[\mathbf{u}{]}_{\times }^2\mathbf{x}\\ & =\mathbf{x}+\sin \varphi (\mathbf{u}\times \mathbf{x})+(1-\cos \varphi )\left(\mathbf{u}{\mathbf{u}}^{\top }-\mathbf{I}\right)\mathbf{x}\\ & ={\mathbf{x}}_{\Vert }+{\mathbf{x}}_{\perp }+\sin \varphi (\mathbf{u}\times \mathbf{x})-(1-\cos \varphi ){\mathbf{x}}_{\perp }\\ & ={\mathbf{x}}_{\Vert }+(\mathbf{u}\times \mathbf{x})\sin \varphi +{\mathbf{x}}_{\perp }\cos \varphi \end{array}$$

---

2.3 旋转群与四元数

（1）四元数旋转基本性质

四元数旋转公式为：
$$r(\mathbf{v})=\mathbf{q}\otimes \mathbf{v}\otimes {\mathbf{q}}^{\ast }$$

旋转并不改变向量的长度，两边取模可得：
$$||\mathbf{q}\otimes \mathbf{v}\otimes {\mathbf{q}}^{\ast }||=||\mathbf{q}|{|}^2||\mathbf{v}||=||\mathbf{v}||$$

最终可以得到：
$$\mathbf{q}\otimes {\mathbf{q}}^{\ast }=1={\mathbf{q}}^{\ast }\otimes \mathbf{q}$$

与上一节关于旋转矩阵性质十分相似，即 ${\mathbf{R}}^{\top }\mathbf{R}=\mathbf{I}=\mathbf{R}{\mathbf{R}}^{\top }$。同样地，单位四元数也构成了一个群，用 $S^3$表示。

（2）四元数指数映射

假设有一个四元数 $\mathbf{q}\in S^3$，满足 ${\mathbf{q}}^{\ast }\otimes \mathbf{q}=1$，两边求导可得：
$$\frac{d({\mathbf{q}}^{\ast }\otimes \mathbf{q})}{dt}={\dot{\mathbf{q}}}^{\ast }\otimes \mathbf{q}+{\mathbf{q}}^{\ast }\otimes \dot{\mathbf{q}}=0$$

可以得到：
$${\mathbf{q}}^{\ast }\otimes \dot{\mathbf{q}}=-({\dot{\mathbf{q}}}^{\ast }\otimes \mathbf{q})=-({\mathbf{q}}^{\ast }\otimes \dot{\mathbf{q}}{)}^{\ast }$$

可以证明，${\mathbf{q}}^{\ast }\otimes \dot{\mathbf{q}}$ 是一个纯四元数，使用 $\mathbf{\Omega }$ 表示纯四元数，则有：
$${\mathbf{q}}^{\ast }\otimes \dot{\mathbf{q}}=\mathbf{\Omega }=\left[\begin{array}{c}0\\ \mathbf{\Omega }\end{array}\right]$$

最终可以得到，四元数导数为：
$$\dot{\mathbf{q}}=\mathbf{q}\otimes \mathbf{\Omega }$$

在原点，四元数 $\mathbf{q}=1$。此时得到李代数 $\dot{\mathbf{q}}=\mathbf{\Omega }$。积分可得：
$$\mathbf{q}(t)=\mathbf{q}(0)\otimes e^{\mathbf{\Omega }t}$$

此时，可以得到四元数的指数映射方程为：
$$\exp :{\mathbb{H}}_p\to S^3;\mathbf{V}\mapsto \exp \left(\mathbf{V}\right)=e^{\mathbf{V}}$$

同样地，也可以写成实数 $\mathbb{R}$ 到 群 $S^3$ 的映射。
$$\mathrm{Exp}:{\mathbb{R}}^3\to S^3;\mathbf{\varphi }\mapsto \mathrm{Exp}\left(\mathbf{\varphi }\right)=e^{\mathbf{\varphi }/2}$$

实数、四元数、旋转群三者之间关系为;

有时，为了方便，会引入角速度向量 $\omega =2\mathbf{\Omega }\in {\mathbb{R}}^3$，则四元数及其导数可以写为：
$$\dot{\mathbf{q}}=\frac{1}{2}\mathbf{q}\otimes \omega$$

$$\mathbf{q}=e^{\omega t/2}$$

(3) 四元数与旋转向量

令 $\mathbf{\varphi }=\varphi \mathbf{u}$，其中 $\mathbf{u}$为旋转向量， $\varphi$ 为旋转角度。则四元数与旋转向量方程为：
$$\mathbf{q}=\mathrm{Exp}(\varphi \mathbf{u})=e^{\varphi \mathbf{u}/2}=\left[\begin{array}{c}\cos (\varphi /2)\\ \mathbf{u}\sin (\varphi /2)\end{array}\right]$$

从四元数求旋转向量和角度公式为：

$$\varphi =2\arctan (||{\mathbf{q}}_v||,q_w)$$

$$\mathbf{u}={\mathbf{q}}_v/||{\mathbf{q}}_v||$$

(4) 旋转运算

回到最开始的问题，向量 $\mathbf{x}$ 经过四元数旋转后的向量 ${\mathbf{x}}^{\prime }$为：
$$\begin{array}{rl}{\mathbf{x}}^{\prime }& =\mathbf{q}\otimes \mathbf{x}\otimes {\mathbf{q}}^{\ast }\\ & =\left(\cos \frac{\varphi }{2}+\mathbf{u}\sin \frac{\varphi }{2}\right)\otimes (0+\mathbf{x})\otimes \left(\cos \frac{\varphi }{2}-\mathbf{u}\sin \frac{\varphi }{2}\right)\\ & =\mathbf{x}{\cos }^2\frac{\varphi }{2}+(\mathbf{u}\otimes \mathbf{x}-\mathbf{x}\otimes \mathbf{u})\sin \frac{\varphi }{2}\cos \frac{\varphi }{2}-\mathbf{u}\otimes \mathbf{x}\otimes \mathbf{u}{\sin }^2\frac{\varphi }{2}\\ & =\mathbf{x}{\cos }^2\frac{\varphi }{2}+2(\mathbf{u}\times \mathbf{x})\sin \frac{\varphi }{2}\cos \frac{\varphi }{2}-\left(\mathbf{x}\left({\mathbf{u}}^{\top }\mathbf{u}\right)-2\mathbf{u}\left({\mathbf{u}}^{\top }\mathbf{x}\right)\right){\sin }^2\frac{\varphi }{2}\\ & =\mathbf{x}\left({\cos }^2\frac{\varphi }{2}-{\sin }^2\frac{\varphi }{2}\right)+(\mathbf{u}\times \mathbf{x})\left(2\sin \frac{\varphi }{2}\cos \frac{\varphi }{2}\right)+\mathbf{u}\left({\mathbf{u}}^{\top }\mathbf{x}\right)\left(2{\sin }^2\frac{\varphi }{2}\right)\\ & =\mathbf{x}\cos \varphi +(\mathbf{u}\times \mathbf{x})\sin \varphi +\mathbf{u}\left({\mathbf{u}}^{\top }\mathbf{x}\right)(1-\cos \varphi )\\ & =\left(\mathbf{x}-\mathbf{u}{\mathbf{u}}^{\top }\mathbf{x}\right)\cos \varphi +(\mathbf{u}\times \mathbf{x})\sin \varphi +\mathbf{u}{\mathbf{u}}^{\top }\mathbf{x}\\ & ={\mathbf{x}}_{\perp }\cos \varphi +(\mathbf{u}\times \mathbf{x})\sin \varphi +{\mathbf{x}}_{\Vert }\end{array}$$

---

2.4 旋转矩阵与四元数

无论向量 $\mathbf{x}$ 是用四元数进行旋转，还是旋转矩阵进行旋转，旋转后的结果都是一致的，则有：
$$\mathbf{q}\otimes \mathbf{x}\otimes {\mathbf{q}}^{\ast }=\mathbf{R}\mathbf{x}$$

因此可以得到四元数到旋转矩阵的转换关系为：
$$\mathbf{R}=\left[\begin{array}{ccc}{q}_w^2+q_x^2-q_y^2-q_z^2& 2\left(q_x{q}_y-q_w{q}_z\right)& 2\left(q_x{q}_z+q_w{q}_y\right)\\ 2\left(q_x{q}_y+q_w{q}_z\right)& q_w^2-q_x^2+q_y^2-q_z^2& 2\left(q_y{q}_z-q_w{q}_x\right)\\ 2\left(q_x{q}_z-q_w{q}_y\right)& 2\left(q_y{q}_z+q_w{q}_x\right)& q_w^2-q_x^2-q_y^2+q_z^2\end{array}\right]$$

为了书写方便，旋转矩阵也可以写成：
$$\mathbf{R}=(q_w^2-{\mathbf{q}}_v^{\top }{\mathbf{q}}_v)\mathbf{I}+2{\mathbf{q}}_v{\mathbf{q}}_v^{\top }+2{\mathbf{q}}_w[{\mathbf{q}}_v{]}_{\times }$$

旋转矩阵 $\mathbf{R}$ 和四元数 $\mathbf{q}$ 总结如下表所示。

---

3. 李群扰动导数

3.1 李群加减运算符

在实数空间 ${\mathbb{R}}^n$ 中，加减运算符使用 $+,-$ 表示。但是在李群 $SO(3)$ 上，定义的加减运算符为 $\oplus ,\ominus$。

加法运算为：$$\mathrm{S}=\mathrm{R}\oplus \mathbf{\theta }\triangleq \mathrm{R}\circ \mathrm{Exp}(\mathbf{\theta })\mathrm{R},\mathrm{S}\in SO(3),\mathbf{\theta }\in {\mathbb{R}}^3$$

可以发现运算符满足任意 $SO(3)$ 的表示，对于四元数可旋转矩阵，可以写成：
$$\begin{array}{rl}{\mathbf{q}}_{\mathrm{S}}& ={\mathbf{q}}_{\mathrm{R}}\oplus \mathbf{\theta }={\mathbf{q}}_{\mathrm{R}}\otimes \mathrm{Exp}(\mathbf{\theta })\\ {\mathbf{R}}_{\mathrm{S}}& ={\mathbf{R}}_{\mathrm{R}}\oplus \mathbf{\theta }={\mathbf{R}}_{\mathrm{R}}\cdot \mathrm{Exp}(\mathbf{\theta })\end{array}$$

减法运算为：
$$\mathbf{\theta }=\mathrm{S}\ominus \mathrm{R}\triangleq \log \left({\mathrm{R}}^{-1}\circ \mathrm{S}\right)\mathrm{R},\mathrm{S}\in SO(3),\mathbf{\theta }\in {\mathbb{R}}^3$$

对于旋转矩阵和四元数，其减法运算为：
$$\begin{array}{l}\mathbf{\theta }={\mathbf{q}}_{\mathrm{s}}\ominus {\mathbf{q}}_{\mathrm{R}}=\log \left({\mathbf{q}}_R^{\ast }\otimes {\mathbf{q}}_{\mathrm{S}}\right)\\ \mathbf{\theta }={\mathbf{R}}_{\mathrm{s}}\ominus {\mathbf{R}}_{\mathrm{R}}=\log \left({\mathbf{R}}_{\mathrm{R}}^{\top }{\mathbf{R}}_{\mathrm{S}}\right)\end{array}$$

---

3.2 李群四种导数定义

（1）向量函数与向量的导数

已知函数 $f:{\mathbb{R}}^m\to {\mathbb{R}}^n$，则函数导数为：
$$\frac{\partial f(\mathbf{x})}{\partial \mathbf{x}}\triangleq \underset{\delta \mathbf{x}\to 0}{\lim }\frac{f(\mathbf{x}+\delta \mathbf{x})-f(\mathbf{x})}{\delta \mathbf{x}}\in {\mathbb{R}}^{n\times m}$$

则欧拉积分公式为：
$$f(\mathbf{x}+\Delta \mathbf{x})\approx f(\mathbf{x})+\frac{\partial f(\mathbf{x})}{\partial \mathbf{x}}\Delta \mathbf{x}\in {\mathbb{R}}^n$$

（2）李群函数与李群的导数

已知函数 $f:SO(3)\to SO(3)$，则函数导数为：
$$\begin{array}{rlr}\frac{\partial f(\mathrm{R})}{\partial \mathbf{\theta }}& \triangleq \underset{\delta \mathbf{\theta }\to 0}{\lim }\frac{f(\mathrm{R}\oplus \delta \mathbf{\theta })\ominus f(\mathrm{R})}{\delta \mathbf{\theta }}& \in {\mathbb{R}}^{3\times 3}\\ & =\underset{\delta \mathbf{\theta }\to 0}{\lim }\frac{\log \left(f^{-1}(\mathrm{R})f(\mathrm{R}\mathrm{Exp}(\delta \mathbf{\theta }))\right)}{\delta \mathbf{\theta }}& \end{array}$$

则欧拉积分公式为：
$$f(\mathrm{R}\oplus \Delta \mathbf{\theta })\approx f(\mathrm{R})\oplus \frac{\partial f(\mathrm{R})}{\partial \mathbf{\theta }}\Delta \mathbf{\theta }\triangleq f(\mathrm{R})\mathrm{Exp}\left(\frac{\partial f(\mathrm{R})}{\partial \mathbf{\theta }}\Delta \mathbf{\theta }\right)\in SO(3)$$

（3）李群函数与向量的导数

已知函数 $f:{\mathbb{R}}^m\to SO(3)$，则函数导数为：
$$\begin{array}{rl}\frac{\partial f(\mathbf{x})}{\partial \mathbf{x}}& \triangleq \underset{\delta \mathbf{x}\to 0}{\lim }\frac{f(\mathbf{x}+\delta \mathbf{x})\ominus f(\mathbf{x})}{\delta \mathbf{x}}\in {\mathbb{R}}^{3\times m}\\ & =\underset{\delta \mathbf{x}\to 0}{\lim }\frac{\log \left(f^{-1}(\mathbf{x})f(\mathbf{x}+\delta \mathbf{x})\right)}{\delta \mathbf{x}}\end{array}$$

则欧拉积分公式为：
$$f(\mathbf{x}+\Delta \mathbf{x})\approx f(\mathbf{x})\oplus \frac{\partial f(\mathbf{x})}{\partial \mathbf{x}}\Delta \mathbf{x}\triangleq f(\mathbf{x})\mathrm{Exp}\left(\frac{\partial f(\mathbf{x})}{\partial \mathbf{x}}\Delta \mathbf{x}\right)\in SO(3)$$

（4）向量函数与李群的导数
已知函数 $f:SO(3)\to {\mathbb{R}}^3$，则函数导数为：
$$\begin{array}{rl}\frac{\partial f(\mathrm{R})}{\partial \mathbf{\theta }}& \triangleq \underset{\delta \theta \to 0}{\lim }\frac{f(\mathrm{R}\oplus \delta \mathbf{\theta })-f(\mathrm{R})}{\delta \mathbf{\theta }}\\ & =\underset{\delta \mathbf{\theta }\to 0}{\lim }\frac{f(\mathrm{R}\mathrm{Exp}(\delta \mathbf{\theta }))-f(\mathrm{R})}{\delta \mathbf{\theta }}\end{array}$$

则欧拉积分公式为：
$$f(\mathrm{R}\oplus \delta \mathbf{\theta })\approx f(\mathrm{R})+\frac{\partial f(\mathrm{R})}{\partial \mathbf{\theta }}\Delta \mathbf{\theta }\triangleq f(\mathrm{R})+\mathrm{Exp}\left(\frac{\partial f(\mathrm{R})}{\partial \mathbf{\theta }}\Delta \mathbf{\theta }\right)\in {\mathbb{R}}^3$$

---

3.3 常用旋转雅可比矩阵 (重点)

已知向量 $\mathbf{a}$，旋转角度 $\theta$，旋转轴 $\mathbf{u}$，可以有三种旋转表示： θ = θ u , q = q { θ } , R = R { θ } \boldsymbol{\theta}=\theta\mathbf{u},\mathbf{q}=\mathbf{q}\{\boldsymbol{\theta}\},\mathbf{R}=\mathbf{R}\{\boldsymbol{\theta}\} θ=θu,q=q{θ},R=R{θ}。下面求三者各自对应的雅可比矩阵。

（1）对向量求雅可比
$$\frac{\partial (\mathbf{q}\otimes \mathbf{a}\otimes \mathbf{q}\ast )}{\partial \mathbf{a}}=\frac{\partial (\mathbf{R}\mathbf{a})}{\partial \mathbf{a}}=\mathbf{R}$$

（2）对四元数求雅可比

四元数 $\mathbf{q}=w+\mathbf{v}$，则旋转后向量 ${\mathbf{a}}^{\prime }$为：

$$\begin{array}{rl}{\mathbf{a}}^{\prime }& =\mathbf{q}\otimes \mathbf{a}\otimes {\mathbf{q}}^{\ast }\\ & =(w+\mathbf{v})\otimes \mathbf{a}\otimes (w-\mathbf{v})\\ & =w^2\mathbf{a}+w(\mathbf{v}\otimes \mathbf{a}-\mathbf{a}\otimes \mathbf{v})-\mathbf{v}\otimes \mathbf{a}\otimes \mathbf{v}\\ & =w^2\mathbf{a}+2w(\mathbf{v}\times \mathbf{a})-\left[\left(-{\mathbf{v}}^{\top }\mathbf{a}+\mathbf{v}\times \mathbf{a}\right)\otimes \mathbf{v}\right]\\ & =w^2\mathbf{a}+2w(\mathbf{v}\times \mathbf{a})-\left[\left(-{\mathbf{v}}^{\top }\mathbf{a}\right)\mathbf{v}+(\mathbf{v}\times \mathbf{a})\otimes \mathbf{v}\right]\\ & =w^2\mathbf{a}+2w(\mathbf{v}\times \mathbf{a})-\left[\left(-{\mathbf{v}}^{\top }\mathbf{a}\right)\mathbf{v}-(\mathbf{v}\times \mathbf{a}{)}^{\top }\mathbf{v}+(\mathbf{v}\times \mathbf{a})\times \mathbf{v}\right]\\ & =w^2\mathbf{a}+2w(\mathbf{v}\times \mathbf{a})-\left[\left(-{\mathbf{v}}^{\top }\mathbf{a}\right)\mathbf{v}+\left({\mathbf{v}}^{\top }\mathbf{v}\right)\mathbf{a}-\left({\mathbf{v}}^{\top }\mathbf{a}\right)\mathbf{v}\right]\\ & =w^2\mathbf{a}+2w(\mathbf{v}\times \mathbf{a})+2\left({\mathbf{v}}^{\top }\mathbf{a}\right)\mathbf{v}-\left({\mathbf{v}}^{\top }\mathbf{v}\right)\mathbf{a}\end{array}$$

对实部和虚部依次求偏导为：
$$\begin{array}{rl}\frac{\partial {\mathbf{a}}^{\prime }}{\partial w}& =2(w\mathbf{a}+\mathbf{v}\times \mathbf{a})\\ \frac{\partial {\mathbf{a}}^{\prime }}{\partial \mathbf{v}}& =-2w[\mathbf{a}{]}_{\times }+2\left({\mathbf{v}}^{\top }\mathbf{a}\mathbf{I}+\mathbf{v}{\mathbf{a}}^{\top }\right)-2\mathbf{a}{\mathbf{v}}^{\top }\\ & =2\left({\mathbf{v}}^{\top }\mathbf{a}\mathbf{I}+\mathbf{v}{\mathbf{a}}^{\top }-\mathbf{a}{\mathbf{v}}^{\top }-w[\mathbf{a}{]}_{\times }\right)\end{array}$$

最终雅可比矩阵为：
$$\frac{\partial (\mathbf{q}\otimes \mathbf{a}\otimes \mathbf{q}\ast )}{\partial \mathbf{q}}=2\left[w\mathbf{a}+\mathbf{v}\times \mathbf{a}\mid {\mathbf{v}}^{\top }\mathbf{a}{\mathbf{I}}_3+\mathbf{v}{\mathbf{a}}^{\top }-\mathbf{a}{\mathbf{v}}^{\top }-w[\mathbf{a}{]}_{\times }\right]\in {\mathbb{R}}^{3\times 4}$$

（3）李群的右雅可比

右雅可比的矩阵可以参考下图。

已知：$\mathrm{Exp}(\mathbf{\theta })\oplus \delta \mathbf{\varphi }=\mathrm{Exp}(\mathbf{\theta }+\delta \mathbf{\theta })$，则 $\delta \mathbf{\varphi }=\log \left(\mathrm{Exp}(\mathbf{\theta }{)}^{-1}\circ \mathrm{Exp}(\mathbf{\theta }+\delta \mathbf{\theta })\right)=\mathrm{Exp}(\mathbf{\theta }+\delta \mathbf{\theta })\ominus \mathrm{Exp}(\mathbf{\theta })$。

右雅可比矩阵计算公式为：
$$\begin{array}{rlr}{\mathbf{J}}_r(\mathbf{\theta })& =\underset{\delta \mathbf{\theta }\to 0}{\lim }\frac{\mathrm{Exp}(\mathbf{\theta }+\delta \mathbf{\theta })\ominus \mathrm{Exp}(\mathbf{\theta })}{\delta \mathbf{\theta }}& \\ & =\underset{\delta \mathbf{\theta }\to 0}{\lim }\frac{\log \left(\mathrm{Exp}(\mathbf{\theta }{)}^{\top }\mathrm{Exp}(\mathbf{\theta }+\delta \mathbf{\theta })\right)}{\delta \mathbf{\theta }}& \text{ if using }\mathbf{R}\\ & =\underset{\delta \mathbf{\theta }\to 0}{\lim }\frac{\log \left(\mathrm{Exp}(\mathbf{\theta }{)}^{\ast }\otimes \mathrm{Exp}(\mathbf{\theta }+\delta \mathbf{\theta })\right)}{\delta \mathbf{\theta }}& \text{ if using }\mathbf{q}.\end{array}$$

具体形式为：
$$\begin{array}{rl}{\mathbf{J}}_r(\mathbf{\theta })& =\mathbf{I}-\frac{1-\cos \Vert \mathbf{\theta }\Vert }{\Vert \mathbf{\theta }{\Vert }^2}[\mathbf{\theta }{]}_{\times }+\frac{\Vert \mathbf{\theta }\Vert -\sin \Vert \mathbf{\theta }\Vert }{\Vert \mathbf{\theta }{\Vert }^3}[\mathbf{\theta }{]}_{\times }^2\\ {\mathbf{J}}_r^{-1}(\mathbf{\theta })& =\mathbf{I}+\frac{1}{2}[\mathbf{\theta }{]}_{\times }+\left(\frac{1}{\Vert \mathbf{\theta }{\Vert }^2}-\frac{1+\cos \Vert \mathbf{\theta }\Vert }{2\Vert \mathbf{\theta }\Vert \sin \Vert \mathbf{\theta }\Vert }\right)[\mathbf{\theta }{]}_{\times }^2\end{array}$$

其中，右雅可比矩阵有如下性质：
$$\begin{array}{rl}\mathrm{Exp}(\mathbf{\theta }+\delta \mathbf{\theta })& \approx \mathrm{Exp}(\mathbf{\theta })\mathrm{Exp}\left({\mathbf{J}}_r(\mathbf{\theta })\delta \mathbf{\theta }\right)\\ \mathrm{Exp}(\mathbf{\theta })\mathrm{Exp}(\delta \mathbf{\theta })& \approx \mathrm{Exp}\left(\mathbf{\theta }+{\mathbf{J}}_r^{-1}(\mathbf{\theta })\delta \mathbf{\theta }\right)\\ \log (\mathrm{Exp}(\mathbf{\theta })\mathrm{Exp}(\delta \mathbf{\theta }))& \approx \mathbf{\theta }+{\mathbf{J}}_r^{-1}(\mathbf{\theta })\delta \mathbf{\theta }\end{array}$$

（4）对旋转向量的雅可比
∂ ( q ⊗ a ⊗ q ∗ ) ∂ δ θ = ∂ ( R a ) ∂ δ θ = lim ⁡ δ θ → 0 R { θ + δ θ } a − R { θ } a δ θ = lim ⁡ δ θ → 0 ( R { θ } Exp ⁡ ( J r ( θ ) δ θ ) − R { θ } ) a δ θ = lim ⁡ δ θ → 0 ( R { θ } ( I + [ J r ( θ ) δ θ ] × ) − R { θ } ) a δ θ = lim ⁡ δ θ → 0 R { θ } [ J r ( θ ) δ θ ] × a δ θ = lim ⁡ δ θ → 0 − R { θ } [ a ] × J r ( θ ) δ θ δ θ = − R { θ } [ a ] × J r ( θ ) \begin{aligned} \frac{\partial\left(\mathbf{q} \otimes \mathbf{a} \otimes \mathbf{q}^{*}\right)}{\partial \delta \boldsymbol{\theta}}=\frac{\partial(\mathbf{R} \mathbf{a})}{\partial \delta \boldsymbol{\theta}} &=\lim _{\delta \boldsymbol{\theta} \rightarrow 0} \frac{\mathbf{R}\{\boldsymbol{\theta}+\delta \boldsymbol{\theta}\} \mathbf{a}-\mathbf{R}\{\boldsymbol{\theta}\} \mathbf{a}}{\delta \boldsymbol{\theta}} \\ &=\lim _{\delta \boldsymbol{\theta} \rightarrow 0} \frac{\left(\mathbf{R}\{\boldsymbol{\theta}\} \operatorname{Exp}\left(\mathbf{J}_{r}(\boldsymbol{\theta}) \delta \boldsymbol{\theta}\right)-\mathbf{R}\{\boldsymbol{\theta}\}\right) \mathbf{a}}{\delta \boldsymbol{\theta}} \\ &=\lim _{\delta \boldsymbol{\theta} \rightarrow 0} \frac{\left(\mathbf{R}\{\boldsymbol{\theta}\}\left(\mathbf{I}+\left[\mathbf{J}_{r}(\boldsymbol{\theta}) \delta \boldsymbol{\theta}\right]_{\times}\right)-\mathbf{R}\{\boldsymbol{\theta}\}\right) \mathbf{a}}{\delta \boldsymbol{\theta}} \\ &=\lim _{\delta \boldsymbol{\theta} \rightarrow 0} \frac{\mathbf{R}\{\boldsymbol{\theta}\}\left[\mathbf{J}_{r}(\boldsymbol{\theta}) \delta \boldsymbol{\theta}\right]_{\times} \mathbf{a}}{\delta \boldsymbol{\theta}} \\ &=\lim _{\delta \boldsymbol{\theta} \rightarrow 0}-\frac{\mathbf{R}\{\boldsymbol{\theta}\}[\mathbf{a}]_{\times} \mathbf{J}_{r}(\boldsymbol{\theta}) \delta \boldsymbol{\theta}}{\delta \boldsymbol{\theta}} \\ &=-\mathbf{R}\{\boldsymbol{\theta}\}[\mathbf{a}]_{\times} \mathbf{J}_{r}(\boldsymbol{\theta}) \end{aligned} ∂δθ∂(q⊗a⊗q∗)​=∂δθ∂(Ra)​​=δθ→0lim​δθR{θ+δθ}a−R{θ}a​=δθ→0lim​δθ(R{θ}Exp(Jr​(θ)δθ)−R{θ})a​=δθ→0lim​δθ(R{θ}(I+[Jr​(θ)δθ]×​)−R{θ})a​=δθ→0lim​δθR{θ}[Jr​(θ)δθ]×​a​=δθ→0lim​−δθR{θ}[a]×​Jr​(θ)δθ​=−R{θ}[a]×​Jr​(θ)​

---

3.4 四元数导数

四元数导数为：
$$\begin{array}{rl}\dot{\mathbf{q}}& \triangleq \underset{\Delta t\to 0}{\lim }\frac{\mathbf{q}(t+\Delta t)-\mathbf{q}(t)}{\Delta t}\\ & =\underset{\Delta t\to 0}{\lim }\frac{\mathbf{q}\otimes \Delta {\mathbf{q}}_{\mathcal{L}}-\mathbf{q}}{\Delta t}\\ & =\underset{\Delta t\to 0}{\lim }\frac{\mathbf{q}\otimes \left(\left[\begin{array}{c}1\\ \frac{1}{2}\Delta {\mathbf{\varphi }}_{\mathcal{L}}\end{array}\right]-\left[\begin{array}{l}1\\ 0\end{array}\right]\right)}{\Delta t}\\ & =\underset{\Delta t\to 0}{\lim }\frac{\mathbf{q}\otimes (\left[\begin{array}{c}0\\ \frac{1}{2}\Delta {\mathbf{\varphi }}_{\mathcal{L}}\end{array}\right]}{\Delta t}\\ & =\frac{1}{2}\mathbf{q}\otimes \left[\begin{array}{c}0\\ {\mathbf{\omega }}_{\mathcal{L}}\end{array}\right]\end{array}$$

定义：
$$\mathbf{\Omega }(\mathbf{\omega })\triangleq [\mathbf{\omega }{]}_R=\left[\begin{array}{cc}0& -{\mathbf{\omega }}^{\top }\\ \mathbf{\omega }& -[\mathbf{\omega }{]}_{\times }\end{array}\right]=\left[\begin{array}{cccc}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{array}\right]$$

最终得到四元数和旋转矩阵的导数公式（局部坐标）：
$$\dot{\mathbf{q}}=\frac{1}{2}\mathbf{\Omega }\left({\mathbf{\omega }}_{\mathcal{L}}\right)\mathbf{q}=\frac{1}{2}\mathbf{q}\otimes {\mathbf{\omega }}_{\mathcal{L}},\dot{\mathbf{R}}=\mathbf{R}{\left[{\mathbf{\omega }}_{\mathcal{L}}\right]}_{\times }$$

全局坐标下导数为：
$$\dot{\mathbf{q}}=\frac{1}{2}{\mathbf{\omega }}_{\mathcal{G}}\otimes \mathbf{q},\dot{\mathbf{R}}=[{\mathbf{\omega }}_{\mathcal{G}}{]}_{\times }\mathbf{R}$$

---

4. IMU运动学方程

在文中作者列出了使用误差卡尔曼滤波的原因：

• 方向误差状态极小（和自由度有相同的参数数量），能够避免过度参数化以及由此产生的协方差矩阵奇异的风险。
• 误差状态系统始终在接近原点的位置运行，因此远离可能的参数奇异性，万向节锁定问题等，从而保证了线性化有效性始终不变。
• 误差状态量始终很小，这意味着所有二阶项都可以忽略不计。 这使得 Jacobian 的计算非常容易和快速。
• 误差状态量动态变化缓慢，因为所有的 large-signal 动态变化已经被包含在名义状态里了。因此，在卡尔曼滤波过程中，相较于预测过程，可以以更低的频率来执行更新过程。

在文中作者对误差状态卡尔曼滤波还做了如下解释：

• 在误差状态卡尔曼滤波中，会讨论到三种状态量：真值状态，名义状态和误差状态，真值状态由名义状态和误差状态组合表示（如线性组合，四元数乘积或矩阵乘积方式等）。其思想是将名义状态视为 large-signal（以非线性方式可积分），将误差状态视为 small-signal（线性可积分并适用于线性高斯滤波）。
• 在使用 IMU 进行定位时，高频 IMU 数据 ${\mathbf{u}}_{\mathbf{m}}$ 被积分到名义状态 $\mathbf{x}$ 中。名义状态未考虑噪声 $\mathbf{w}$，结果它会积累误差。误差状态 $\delta \mathbf{x}$ 通过误差状态卡尔曼滤波进行估计，包含了所有的噪声和扰动。误差状态由小幅度的信号组成，其更新函数由线性动态系统定义，其动态、控制和测量矩阵均由名义状态的值计算得出。误差状态卡尔曼修正是在其它传感器信息（例如GPS，视觉等）到达时执行的。

---

4.1 局部坐标系统连续时刻微分方程

下表是误差状态卡尔曼滤波中使用到的所有变量，需要注意的时：这里的方向误差 $\delta \mathbf{q}$ 是定义在局部坐标下的。后面会再给出全局坐标下的方向计算公式。

4.1.1 真值状态微分方程

首先介绍真值状态微分方程：
$$\begin{array}{rl}{\dot{\mathrm{p}}}_t& ={\mathbf{v}}_t\\ {\dot{\mathbf{v}}}_t& ={\mathbf{a}}_t\\ {\dot{\mathbf{q}}}_t& =\frac{1}{2}{\mathbf{q}}_t\otimes {\mathbf{\omega }}_t\\ {\dot{\mathbf{a}}}_{bt}& ={\mathbf{a}}_w\\ {\dot{\mathbf{\omega }}}_{bt}& ={\mathbf{\omega }}_w\\ {\dot{\mathbf{g}}}_t& =0\end{array}$$

其中，加速度真值 ${\mathbf{a}}_t$，角速度真值 ${\omega }_{\mathbf{t}}$ 可以从 IMU 中获得，IMU测量值 ${\mathbf{a}}_m$ 和 ${\omega }_m$ 分别为：
$$\begin{array}{rl}{\mathbf{a}}_m& ={\mathbf{R}}_t^{\top }\left({\mathbf{a}}_t-{\mathbf{g}}_t\right)+{\mathbf{a}}_{bt}+{\mathbf{a}}_n\\ {\mathbf{\omega }}_m& ={\mathbf{\omega }}_t+{\mathbf{\omega }}_{bt}+{\mathbf{\omega }}_n\end{array}$$

对上面两个方程进行移项变换，可以得到真值 ${\mathbf{a}}_t$ 和 ${\omega }_{\mathbf{t}}$ 为：
$$\begin{array}{rl}{\mathbf{a}}_t& ={\mathbf{R}}_t\left({\mathbf{a}}_m-{\mathbf{a}}_{bt}-{\mathbf{a}}_n\right)+{\mathbf{g}}_t\\ {\mathbf{\omega }}_t& ={\mathbf{\omega }}_m-{\mathbf{\omega }}_{bt}-{\mathbf{\omega }}_n\end{array}$$

最终，真值状态微分方程为：
$$\begin{array}{rl}{\dot{\mathbf{p}}}_t& ={\mathbf{v}}_t\\ {\dot{\mathbf{v}}}_t& ={\mathbf{R}}_t\left({\mathbf{a}}_m-{\mathbf{a}}_{bt}-{\mathbf{a}}_n\right)+{\mathbf{g}}_t\\ {\dot{\mathbf{q}}}_t& =\frac{1}{2}{\mathbf{q}}_t\otimes \left({\mathbf{\omega }}_m-{\mathbf{\omega }}_{bt}-{\mathbf{\omega }}_n\right)\\ {\dot{\mathbf{a}}}_{bt}& ={\mathbf{a}}_w\\ {\dot{\mathbf{\omega }}}_{bt}& ={\mathbf{\omega }}_w\\ {\dot{\mathbf{g}}}_t& =0\end{array}$$

---

4.1.2 名义状态微分方程

由于名义状态不包含误差和扰动项，可以直接得到其微分方程为：
$$\begin{array}{rl}\dot{\mathbf{p}}& =\mathbf{v}\\ \dot{\mathbf{v}}& =\mathbf{R}\left({\mathbf{a}}_m-{\mathbf{a}}_b\right)+\mathbf{g}\\ \dot{\mathbf{q}}& =\frac{1}{2}\mathbf{q}\otimes \left({\mathbf{\omega }}_m-{\mathbf{\omega }}_b\right)\\ {\dot{\mathbf{a}}}_b& =0\\ {\dot{\mathbf{\omega }}}_b& =0\\ \dot{\mathrm{g}}& =0\end{array}$$

---

4.1.3 误差状态微分方程

在忽略二阶项后，这里先给出误差状态的线性微分方程。这里重点是速度误差状态量 $\delta \mathbf{v}$ 和方向误差状态量 $\delta \mathbf{\theta }$ 微分方程的推导。
$$\begin{array}{rl}\dot{\delta \mathbf{p}}& =\delta \mathbf{v}\\ \dot{\delta \mathbf{v}}& =-\mathbf{R}{\left[{\mathbf{a}}_m-{\mathbf{a}}_b\right]}_{\times }\delta \mathbf{\theta }-\mathbf{R}\delta {\mathbf{a}}_b+\delta \mathbf{g}-{\mathbf{R}\mathbf{a}}_n\\ \dot{\delta \mathbf{\theta }}& =-{\left[{\mathbf{\omega }}_m-{\mathbf{\omega }}_b\right]}_{\times }\delta \mathbf{\theta }-\delta {\mathbf{\omega }}_b-{\mathbf{\omega }}_n\\ {\dot{\delta \mathbf{a}}}_b& ={\mathbf{a}}_w\\ {\dot{\delta \mathbf{\omega }}}_b& ={\mathbf{\omega }}_w\\ \dot{\delta \mathbf{g}}& =0\end{array}$$

（1）速度误差状态量微分方程

在推导 速度误差状态微分方程 时先给出两个基本方程：
$$\begin{array}{rl}{\mathbf{R}}_t& =\mathbf{R}\left(\mathbf{I}+[\delta \mathbf{\theta }{]}_{\times }\right)+O\left(\Vert \delta \mathbf{\theta }{\Vert }^2\right)\\ \dot{\mathbf{v}}& ={\mathbf{R}\mathbf{a}}_{\mathcal{B}}+\mathbf{g}\end{array}$$

由表3可知：${\mathbf{R}}_t=\mathbf{R}\delta \mathbf{R}$，同时 $\delta \mathbf{R}=e^{[\delta \mathbf{\theta }{]}_{\times }}$，泰勒展开保留二阶项即可得到 ${\mathbf{R}}_t$。这里引入两个变量：$a_{\mathcal{B}}$ 是载体坐标下加速度large-signal值，$\delta {\mathbf{a}}_{\mathcal{B}}$ 为samll-signal值。

$a_{\mathcal{B}}$ 和 $\delta {\mathbf{a}}_{\mathcal{B}}$方程为：
$$\begin{array}{c}{\mathbf{a}}_{\mathcal{B}}\triangleq {\mathbf{a}}_m-{\mathbf{a}}_b\\ \delta {\mathbf{a}}_{\mathcal{B}}\triangleq -\delta {\mathbf{a}}_b-{\mathbf{a}}_n\end{array}$$

代入上式，可得真值状态 ${\mathbf{a}}_t$ 为：
$$\begin{array}{rl}{\mathbf{a}}_t& ={\mathbf{R}}_t\left({\mathbf{a}}_m-{\mathbf{a}}_{bt}-{\mathbf{a}}_n\right)+{\mathbf{g}}_t\\ & ={\mathbf{R}}_t({\mathbf{a}}_m-{\mathbf{a}}_n-({\mathbf{a}}_b+\delta {\mathbf{a}}_b))+{\mathbf{g}}_t\\ & ={\mathbf{R}}_t\left({\mathbf{a}}_{\mathcal{B}}+\delta {\mathbf{a}}_{\mathcal{B}}\right)+\mathbf{g}+\delta \mathbf{g}\end{array}$$

现在用两种形式来表示${\dot{\mathbf{v}}}_t$：
$$\dot{\mathbf{v}}+\dot{\delta \mathbf{v}}={\dot{\mathbf{v}}}_t={\mathbf{R}}_t\left({\mathbf{a}}_m-{\mathbf{a}}_{bt}-{\mathbf{a}}_n\right)+{\mathbf{g}}_t$$

可得：
$$\dot{\mathbf{v}}+\dot{\delta \mathbf{v}}={\dot{\mathbf{v}}}_t=\mathbf{R}\left(\mathbf{I}+[\delta \mathbf{\theta }{]}_{\times }\right)\left({\mathbf{a}}_{\mathcal{B}}+\delta {\mathbf{a}}_{\mathcal{B}}\right)+\mathbf{g}+\delta \mathbf{g}$$

对上式右边进行展开可得：
$${\mathbf{R}\mathbf{a}}_{\mathcal{B}}+\mathbf{g}+\dot{\delta \mathbf{v}}={\dot{\mathbf{v}}}_t={\mathbf{R}\mathbf{a}}_{\mathcal{B}}+\mathbf{R}\delta {\mathbf{a}}_{\mathcal{B}}+\mathbf{R}[\delta \mathbf{\theta }{]}_{\times }{\mathbf{a}}_{\mathcal{B}}+\mathbf{R}[\delta \mathbf{\theta }{]}_{\times }\delta {\mathbf{a}}_{\mathcal{B}}+\mathbf{g}+\delta \mathbf{g}$$

两边消除相同项 ${\mathbf{R}\mathbf{a}}_{\mathcal{B}}+\mathbf{g}$，可得 $\dot{\delta \mathbf{v}}$ 为：
$$\dot{\delta \mathbf{v}}=\mathbf{R}\left(\delta {\mathbf{a}}_{\mathcal{B}}+[\delta \mathbf{\theta }{]}_{\times }{\mathbf{a}}_{\mathcal{B}}\right)+\mathbf{R}[\delta \mathbf{\theta }{]}_{\times }\delta {\mathbf{a}}_{\mathcal{B}}+\delta \mathbf{g}$$

忽略上式中的二阶项，且已知 $[\mathbf{a}{]}_{\times }\mathbf{b}=-[\mathbf{b}{]}_{\times }\mathbf{a}$，可得：
$$\dot{\delta \mathbf{v}}=\mathbf{R}\left(\delta {\mathbf{a}}_{\mathcal{B}}-{\left[{\mathbf{a}}_{\mathcal{B}}\right]}_{\times }\delta \mathbf{\theta }\right)+\delta \mathbf{g}$$

最终可得：
$$\dot{\delta \mathbf{v}}=\mathbf{R}\left(-{\left[{\mathbf{a}}_m-{\mathbf{a}}_b\right]}_{\times }\delta \mathbf{\theta }-\delta {\mathbf{a}}_b-{\mathbf{a}}_n\right)+\delta \mathbf{g}$$

对上式进行整理，最终可得速度误差状态量的微分方程为：
$$\dot{\delta \mathbf{v}}=-\mathbf{R}{\left[{\mathbf{a}}_m-{\mathbf{a}}_b\right]}_{\times }\delta \mathbf{\theta }-\mathbf{R}\delta {\mathbf{a}}_b+\delta \mathbf{g}-\mathbf{R}{\mathbf{a}}_n$$

（2）方向误差状态量微分方程

同样地在推导 方向误差状态量微分方程 时也先给出两个基本方程，下面是真值状态和名义状态四元数的微分方程：
$$\begin{array}{rl}{\dot{\mathbf{q}}}_t& =\frac{1}{2}{\mathbf{q}}_t\otimes {\mathbf{\omega }}_t\\ \dot{\mathbf{q}}& =\frac{1}{2}\mathbf{q}\otimes \mathbf{\omega }\end{array}$$

这里也引入角速度的 large-signal 和 small-signal 值，其表达式为：
$$\begin{array}{c}\mathbf{\omega }\triangleq {\mathbf{\omega }}_m-{\mathbf{\omega }}_b\\ \delta \mathbf{\omega }\triangleq -\delta {\mathbf{\omega }}_b-{\mathbf{\omega }}_n\end{array}$$

因此，角速度真值可以写为：
$${\mathbf{\omega }}_{\mathbf{t}}=\mathbf{\omega }+\delta \mathbf{\omega }$$

也使用两种形式来表示 ${\dot{\mathbf{q}}}_t$：
$$\mathbf{q}\dot{\otimes }\delta \mathbf{q}={\dot{\mathbf{q}}}_t=\frac{1}{2}{\mathbf{q}}_t\otimes {\omega }_t$$

上式两边同时展开可得：
$$\dot{\mathbf{q}}\otimes \delta \mathbf{q}+\mathbf{q}\otimes \dot{\delta \mathbf{q}}={\dot{\mathbf{q}}}_t=\frac{1}{2}\mathbf{q}\otimes \delta \mathbf{q}\otimes {\mathbf{\omega }}_t$$

等式左边进一步展开可得：
$$\frac{1}{2}\mathbf{q}\otimes \mathbf{\omega }\otimes \delta \mathbf{q}+\mathbf{q}\otimes \dot{\delta \mathbf{q}}={\dot{\mathbf{q}}}_t=\frac{1}{2}\mathbf{q}\otimes \delta \mathbf{q}\otimes {\mathbf{\omega }}_t$$

上式移项合并可得：
$$\mathbf{q}\otimes \dot{\delta \mathbf{q}}=\frac{1}{2}\mathbf{q}\otimes \delta \mathbf{q}\otimes {\mathbf{\omega }}_t-\frac{1}{2}\mathbf{q}\otimes \mathbf{\omega }\otimes \delta \mathbf{q}=\frac{1}{2}\mathbf{q}\otimes (\delta \mathbf{q}\otimes {\mathbf{\omega }}_t-\mathbf{\omega }\otimes \delta \mathbf{q})$$

两边消除同类项可得：
$$2\dot{\delta \mathbf{q}}=\delta \mathbf{q}\otimes {\mathbf{\omega }}_t-\mathbf{\omega }\otimes \delta \mathbf{q}$$

且已知 $\delta \mathbf{q}=e^{\delta \mathbf{\theta }/2}$，则 $\left[\begin{array}{c}0\\ \dot{\delta }\mathbf{\theta }\end{array}\right]=2\dot{\delta \mathbf{q}}$，代入上式可得：
$$\left[\begin{array}{c}0\\ \dot{\delta }\mathbf{\theta }\end{array}\right]=2\dot{\delta \mathbf{q}}=\delta \mathbf{q}\otimes {\mathbf{\omega }}_t-\mathbf{\omega }\otimes \delta \mathbf{q}$$

根据四元数矩阵乘法，可得：
$$\left[\begin{array}{c}0\\ \dot{\delta }\mathbf{\theta }\end{array}\right]=[\mathbf{q}{]}_R\left({\mathbf{\omega }}_t\right)\delta \mathbf{q}-[\mathbf{q}{]}_L(\mathbf{\omega })\delta \mathbf{q}=([\mathbf{q}{]}_R\left({\mathbf{\omega }}_t\right)-[\mathbf{q}{]}_L(\mathbf{\omega }))\delta \mathbf{q}$$

将四元数转换为矩阵表示，可知：
$$[\mathbf{q}{]}_R\left({\mathbf{\omega }}_t\right)=\left[\begin{array}{cc}0& -{\left({\mathbf{\omega }}_t\right)}^{\top }\\ ({\mathbf{\omega }}_t)& -{\left[{\mathbf{\omega }}_t\right]}_{\times }\end{array}\right]$$
$$[\mathbf{q}{]}_L\left(\mathbf{\omega }\right)=\left[\begin{array}{cc}0& -{\left(\mathbf{\omega }\right)}^{\top }\\ \left(\mathbf{\omega }\right)& {\left[\mathbf{\omega }\right]}_{\times }\end{array}\right]$$

带入上式可得：
$$\left[\begin{array}{c}0\\ \dot{\delta }\mathbf{\theta }\end{array}\right]=\left[\begin{array}{cc}0& -{\left({\mathbf{\omega }}_t-\mathbf{\omega }\right)}^{\top }\\ \left({\mathbf{\omega }}_t-\mathbf{\omega }\right)& -{\left[{\mathbf{\omega }}_t+\mathbf{\omega }\right]}_{\times }\end{array}\right]\left[\begin{array}{c}1\\ \delta \mathbf{\theta }/2\end{array}\right]+O\left(\Vert \delta \mathbf{\theta }{\Vert }^2\right)$$

根据 ${\omega }_t=\mathbf{\omega }+\delta \mathbf{\omega }$，上式进一步处理为：

$$\left[\begin{array}{c}0\\ \dot{\delta }\mathbf{\theta }\end{array}\right]=\left[\begin{array}{cc}0& -\delta {\mathbf{\omega }}^{\top }\\ \delta \mathbf{\omega }& -[2\mathbf{\omega }+\delta \mathbf{\omega }{]}_{\times }\end{array}\right]\left[\begin{array}{c}1\\ \delta \mathbf{\theta }/2\end{array}\right]+O\left(\Vert \delta \mathbf{\theta }{\Vert }^2\right)$$

最后可得 $\dot{\delta \mathbf{\theta }}=\delta \mathbf{\omega }-[\mathbf{\omega }{]}_{\times }\delta \mathbf{\theta }-\frac{1}{2}[\delta \mathbf{\omega }{]}_{\times }\delta \mathbf{\theta }+O\left(\Vert \delta \mathbf{\theta }{\Vert }^2\right)$，忽略掉二阶项，可得：$\dot{\delta }\mathbf{\theta }=-[\mathbf{\omega }{]}_{\times }\delta \mathbf{\theta }+\delta \mathbf{\omega }$。

最终可得方向误差状态微分方程为：
$$\dot{\delta }\mathbf{\theta }=-{\left[{\mathbf{\omega }}_m-{\mathbf{\omega }}_b\right]}_{\times }\delta \mathbf{\theta }-\delta {\mathbf{\omega }}_b-{\mathbf{\omega }}_n$$

---

4.2 全局坐标系统连续时微分方程

上一节中 方向误差状态量是在局部坐标系下定义的，本节将给出全局坐标系下误差状态微分方程。在全局坐标系下，方向四元数真值为：
$${\mathbf{q}}_t=\delta \mathbf{q}\otimes \mathbf{q}$$
上式证明过程如下：设在局部坐标系下方向误差项为 $\delta {\mathbf{q}}_L$，$\mathbf{q}$ 为局部坐标到全局坐标的旋转四元数，则在全局坐标下的方向误差为：
$$\delta {\mathbf{q}}_G=\mathbf{q}\otimes \delta {\mathbf{q}}_L\otimes {\mathbf{q}}^{\ast }$$已知在局部坐标下方向误差真值为：${\mathbf{q}}_t=\mathbf{q}\otimes \delta {\mathbf{q}}_L$，代入上式可得：${\mathbf{q}}_t=\mathbf{q}\otimes {\mathbf{q}}^{\ast }\otimes \delta {\mathbf{q}}_G\otimes \mathbf{q}=\delta {\mathbf{q}}_G\otimes \mathbf{q}$，证明完毕。

与局部坐标下误差状态量微分方程相比，只有速度误差状态量和方向误差状态量微分方程不同，下面将给出推导过程。

---

（1）速度误差状态量微分方程

在推导速度误差状态微分方程时这里也给出两个基本方程：

$$\begin{array}{rl}{\mathbf{R}}_t& =(\mathbf{I}+[\delta \mathbf{\theta }{]}_{\times })\mathbf{R}+O\left(\Vert \delta \mathbf{\theta }{\Vert }^2\right)\\ \dot{\mathbf{v}}& ={\mathbf{R}\mathbf{a}}_{\mathcal{B}}+\mathbf{g}\end{array}$$

全局坐标下 ${\mathbf{R}}_t=\delta \mathbf{R}\cdot \mathbf{R}$，同时 $\delta \mathbf{R}=e^{[\delta \mathbf{\theta }{]}_{\times }}$。

这里同样引入两个变量：${\mathbf{a}}_{\mathcal{B}}$ 是载体坐标下加速度large-signal值，$\delta {\mathbf{a}}_{\mathcal{B}}$为samll-signal值。

同样的，也用两种形式来表示${\dot{\mathbf{v}}}_t$：
$$\dot{\mathbf{v}}+\dot{\delta \mathbf{v}}={\dot{\mathbf{v}}}_t=\left(\mathbf{I}+[\delta \mathbf{\theta }{]}_{\times }\right)\mathbf{R}\left({\mathbf{a}}_{\mathcal{B}}+\delta {\mathbf{a}}_{\mathcal{B}}\right)+\mathbf{g}+\delta \mathbf{g}$$

同时对上式进行展开可得：
$${\mathbf{R}\mathbf{a}}_{\mathcal{B}}+\mathbf{g}+\dot{\delta \mathbf{v}}=\mathbf{R}{\mathbf{a}}_{\mathcal{B}}+\mathbf{R}\delta {\mathbf{a}}_{\mathcal{B}}+[\delta \mathbf{\theta }{]}_{\times }\mathbf{R}{\mathbf{a}}_{\mathcal{B}}+[\delta \mathbf{\theta }{]}_{\times }\mathbf{R}\delta {\mathbf{a}}_{\mathcal{B}}+\mathbf{g}+\delta \mathbf{g}$$

两边消除相同项 ${\mathbf{R}\mathbf{a}}_{\mathcal{B}}+\mathbf{g}$，可得 $\dot{\delta \mathbf{v}}$ 为：
$$\dot{\delta \mathbf{v}}=\mathbf{R}\delta {\mathbf{a}}_{\mathcal{B}}+[\delta \mathbf{\theta }{]}_{\times }\mathbf{R}{\mathbf{a}}_{\mathcal{B}}+[\delta \mathbf{\theta }{]}_{\times }\mathbf{R}\delta {\mathbf{a}}_{\mathcal{B}}+\delta \mathbf{g}$$

消除上式中的二阶项，且已知 $[\mathbf{a}{]}_{\times }\mathbf{b}=-[\mathbf{b}{]}_{\times }\mathbf{a}$，可得：
$$\dot{\delta \mathbf{v}}=\mathbf{R}\delta {\mathbf{a}}_{\mathcal{B}}-{\left[\mathbf{R}{\mathbf{a}}_{\mathcal{B}}\right]}_{\times }\delta \mathbf{\theta }+\delta \mathbf{g}$$

最终可得全局坐标系下 速度误差状态量的微分方程：
$$\dot{\delta \mathbf{v}}=-{\left[\mathbf{R}\left({\mathbf{a}}_m-{\mathbf{a}}_b\right)\right]}_{\times }\delta \mathbf{\theta }-\mathbf{R}\delta {\mathbf{a}}_b+\delta \mathbf{g}-\mathbf{R}{\mathbf{a}}_n$$

（2）方向误差状态量微分方程

和局部坐标下推导一样，真值状态和名义状态四元数微分方程为：
$$\begin{array}{rl}{\dot{\mathbf{q}}}_t& =\frac{1}{2}{\mathbf{q}}_t\otimes {\mathbf{\omega }}_t\\ \dot{\mathbf{q}}& =\frac{1}{2}\mathbf{q}\otimes \mathbf{\omega }\end{array}$$

现在也使用两种形式来表示${\dot{\mathbf{q}}}_t$，但记住这里使用的是全局坐标下的方向误差：

$$\dot{(\delta \mathbf{q}\otimes \mathbf{q})}={\dot{\mathbf{q}}}_t=\frac{1}{2}{\mathbf{q}}_t\otimes {\omega }_t$$

上式两边同时展开可得：
$$\dot{\delta \mathbf{q}}\otimes \mathbf{q}+\delta \mathbf{q}\otimes \dot{\mathbf{q}}=\frac{1}{2}\delta \mathbf{q}\otimes \mathbf{q}\otimes {\mathbf{\omega }}_t$$

进一步展开可得：

$$\dot{\delta \mathbf{q}}\otimes \mathbf{q}+\frac{1}{2}\delta \mathbf{q}\otimes \mathbf{q}\otimes \mathbf{\omega }=\frac{1}{2}\delta \mathbf{q}\otimes \mathbf{q}\otimes {\mathbf{\omega }}_t$$

根据 ${\mathbf{\omega }}_t=\mathbf{\omega }+\delta \mathbf{\omega }$，移项可得：
$$\dot{\delta \mathbf{q}}\otimes \mathbf{q}=\frac{1}{2}\delta \mathbf{q}\otimes \mathbf{q}\otimes \delta \mathbf{\omega }$$

上式两边右乘${\mathbf{q}}^{\ast }$，可得：
$$\begin{array}{rl}\dot{\delta \mathbf{q}}& =\frac{1}{2}\delta \mathbf{q}\otimes \mathbf{q}\otimes \delta \mathbf{\omega }\otimes {\mathbf{q}}^{\ast }\\ & =\frac{1}{2}\delta \mathbf{q}\otimes (\mathbf{R}\delta \mathbf{\omega })\\ & =\frac{1}{2}\delta \mathbf{q}\otimes \delta {\mathbf{\omega }}_G\end{array}$$

且已知 $\delta \mathbf{q}=e^{\delta \mathbf{\theta }/2}$，则 $\left[\begin{array}{c}0\\ \dot{\delta }\mathbf{\theta }\end{array}\right]=2\dot{\delta \mathbf{q}}$，代入上式可得：
$$\begin{array}{rl}\left[\begin{array}{c}0\\ \dot{\delta \mathbf{\theta }}\end{array}\right]=2\dot{\delta \mathbf{q}}& =\delta \mathbf{q}\otimes \delta {\mathbf{\omega }}_G=\Omega \left(\delta {\mathbf{\omega }}_G\right)\delta \mathbf{q}=\left[\begin{array}{cc}0& -\delta {\mathbf{\omega }}_G^{\top }\\ \delta {\mathbf{\omega }}_G& -{\left[\delta {\mathbf{\omega }}_G\right]}_{\times }\end{array}\right]\left[\begin{array}{c}1\\ \delta \mathbf{\theta }/2\end{array}\right]+O\left(\Vert \delta \mathbf{\theta }{\Vert }^2\right)\end{array}$$

最后可得 $\dot{\delta \mathbf{\theta }}=\delta {\mathbf{\omega }}_G-\frac{1}{2}{\left[\delta {\mathbf{\omega }}_G\right]}_{\times }\delta \mathbf{\theta }+O\left(\Vert \delta \mathbf{\theta }{\Vert }^2\right)$，忽略掉二阶项，可得：$\dot{\delta }\mathbf{\theta }=\delta {\mathbf{\omega }}_G=\mathbf{R}\delta \mathbf{\omega }$。最终全局坐标系下方向误差状态微分方程为：
$$\dot{\delta \mathbf{\theta }}=-\mathbf{R}\delta {\mathbf{\omega }}_b-\mathbf{R}{\mathbf{\omega }}_n$$

---

4.3 全局坐标系统离散时刻运动学

（1）名义状态量运动学方程

根据名义状态量微分方程，可以得到名义状态运动学方程：
p ← p + v Δ t + 1 2 ( R ( a m − a b ) + g ) Δ t 2 v ← v + ( R ( a m − a b ) + g ) Δ t q ← q ⊗ { ( ω m − ω b ) Δ t } a b ← a b ω b ← ω b g ← g \begin{aligned} {\mathbf{p}} &\leftarrow\mathbf{p} +\mathbf{v}\Delta t+\frac{1}{2}(\mathbf{R}(\mathbf{a}_m-\mathbf{a}_b)+\mathbf{g})\Delta t^2\\ {\mathbf{v}} &\leftarrow\mathbf{v} +(\mathbf{R}(\mathbf{a}_m-\mathbf{a}_b)+\mathbf{g})\Delta t \\ {\mathbf{q}} &\leftarrow\mathbf{q} \otimes \{(\boldsymbol{\omega}_m-\boldsymbol{\omega}_b)\Delta t\} \\ \mathbf{a}_b &\leftarrow \mathbf{a}_b \\ \boldsymbol{\omega}_b &\leftarrow \boldsymbol{\omega}_b \\ \mathbf{g}&\leftarrow\mathbf{g} \end{aligned} pvqab​ωb​g​←p+vΔt+21​(R(am​−ab​)+g)Δt2←v+(R(am​−ab​)+g)Δt←q⊗{(ωm​−ωb​)Δt}←ab​←ωb​←g​

（2）误差状态量运动学方程

根据前面两小节的推导，误差状态量运动学方程为：
$$\begin{array}{rl}\delta \mathbf{p}& \leftarrow \mathbf{p}+\delta \mathbf{v}\Delta t\\ \delta \mathbf{v}& \leftarrow \mathbf{v}+(-{\left[\mathbf{R}\left({\mathbf{a}}_m-{\mathbf{a}}_b\right)\right]}_{\times }\delta \mathbf{\theta }-\mathbf{R}\delta {\mathbf{a}}_b+\delta \mathbf{g})\Delta t+{\mathbf{v}}_{\mathbf{i}}\\ \delta \mathbf{\theta }& \leftarrow \delta \mathbf{\theta }-\mathbf{R}\delta {\mathbf{\omega }}_b\Delta t+{\mathbf{\theta }}_{\mathbf{i}}\\ \delta {\mathbf{a}}_b& \leftarrow \delta {\mathbf{a}}_b+{\mathbf{a}}_{\mathbf{i}}\\ \delta {\mathbf{\omega }}_b& \leftarrow \delta {\mathbf{\omega }}_b+{\mathbf{\omega }}_{\mathbf{i}}\\ \delta \mathbf{g}& \leftarrow \delta \mathbf{g}\end{array}$$

其中，${\mathbf{v}}_{\mathbf{i}}，{\mathbf{\theta }}_{\mathbf{i}}，{\mathbf{a}}_{\mathbf{i}}，{\mathbf{\omega }}_{\mathbf{i}}$ 是输入高斯白噪声：
$$\begin{array}{rlr}{\mathbf{V}}_{\mathbf{i}}& ={\sigma }_{{\tilde{\mathbf{a}}}_n}^2\Delta t^2\mathbf{I}& \left[m^2/s^2\right]\\ {\Theta }_{\mathbf{i}}& ={\sigma }_{{\tilde{\omega }}_n}^2\Delta t^2\mathbf{I}& \left[{\mathrm{r}\mathrm{a}\mathrm{d}}^2\right]\\ {\mathbf{A}}_{\mathbf{i}}& ={\sigma }_{{\mathbf{a}}_w}^2\Delta t\mathbf{I}& \left[{\mathrm{m}}^2/{\mathrm{s}}^4\right]\\ {\Omega }_{\mathbf{i}}& ={\sigma }_{{\mathbf{\omega }}_w}^2\Delta t\mathbf{I}& \left[{\mathrm{r}\mathrm{a}\mathrm{d}}^2/{\mathrm{s}}^2\right]\end{array}$$

---

5. IMU/GPS融合定位

5.1 融合定位预测

现在来看一个实例，即IMU/GPS融合定位。关于IMU/GPS融合定位的工作原理可以查看博文：动手学无人驾驶（6）：基于IMU和GPS数据融合的自车定位。

卡尔曼滤波中，首先给出系统的名义状态变量 $\mathbf{x}$，误差状态量 $\delta \mathbf{x}$，输入为 ${\mathbf{u}}_m$，噪声为 $\mathbf{i}$：
$$\mathbf{x}=\left[\begin{array}{c}\mathbf{p}\\ \mathbf{v}\\ \mathbf{q}\\ {\mathbf{a}}_b\\ {\mathbf{\omega }}_b\\ \mathbf{g}\end{array}\right],\delta \mathbf{x}=\left[\begin{array}{c}\delta \mathbf{p}\\ \delta \mathbf{v}\\ \delta \mathbf{\theta }\\ \delta {\mathbf{a}}_b\\ \delta {\mathbf{\omega }}_b\\ \delta \mathbf{g}\end{array}\right],{\mathbf{u}}_m=\left[\begin{array}{c}{\mathbf{a}}_m\\ {\mathbf{\omega }}_m\end{array}\right],\mathbf{i}=\left[\begin{array}{c}{\mathbf{v}}_{\mathbf{i}}\\ {\mathbf{\theta }}_{\mathbf{i}}\\ {\mathbf{a}}_{\mathbf{i}}\\ {\mathbf{\omega }}_{\mathbf{i}}\end{array}\right]$$

则ESKF 预测方程可以写为：
$$\begin{array}{rl}\hat{\delta }\mathbf{x}& \leftarrow {\mathbf{F}}_{\mathbf{x}}\left(\mathbf{x},{\mathbf{u}}_m\right)\cdot \hat{\delta }\mathbf{x}\\ \mathbf{P}& \leftarrow {\mathbf{F}}_{\mathbf{x}}\mathbf{P}{\mathbf{F}}_{\mathbf{x}}^{\top }+{\mathbf{F}}_{\mathbf{i}}{\mathbf{Q}}_{\mathbf{i}}{\mathbf{F}}_{\mathbf{i}}^{\top }\end{array}$$

其中，状态转移矩阵 ${\mathbf{F}}_{\mathbf{x}}$为：
$${\mathbf{F}}_{\mathbf{x}}=\left[\begin{array}{cccccc}\mathbf{I}& \mathbf{I}\Delta t& 0& 0& 0& 0\\ 0& \mathbf{I}& -{\left[\mathbf{R}\left({\mathbf{a}}_m-{\mathbf{a}}_b\right)\right]}_{\times }\Delta t& -\mathbf{R}\Delta t& 0& \mathbf{I}\Delta t\\ 0& 0& \mathbf{I}& 0& -\mathbf{R}\Delta t& 0\\ 0& 0& 0& \mathbf{I}& 0& 0\\ 0& 0& 0& 0& \mathbf{I}& 0\\ 0& 0& 0& 0& 0& \mathbf{I}\end{array}\right].$$

系统输入雅可比矩阵 ${\mathbf{F}}_{\mathbf{i}}$ 为：
$${\mathbf{F}}_{\mathbf{i}}=\left[\begin{array}{llll}0& 0& 0& 0\\ \mathbf{I}& 0& 0& 0\\ 0& \mathbf{I}& 0& 0\\ 0& 0& \mathbf{I}& 0\\ 0& 0& 0& \mathbf{I}\\ 0& 0& 0& 0\end{array}\right]$$

状态转移协方差矩阵 ${\mathbf{Q}}_{\mathbf{i}}$ 为：
$${\mathbf{Q}}_{\mathbf{i}}=\left[\begin{array}{cccc}{\mathbf{V}}_{\mathbf{i}}& 0& 0& 0\\ 0& {\Theta }_{\mathbf{i}}& 0& 0\\ 0& 0& {\mathbf{A}}_{\mathbf{i}}& 0\\ 0& 0& 0& {\mathbf{\Omega }}_{\mathbf{i}}\end{array}\right]$$

需要注意的是，由于 $\delta \mathbf{x}$ 初始化为0，其在预测阶段一直为0，因此使用名义状态量作为真值状态量。

---

5.2 融合定位更新

回顾误差卡尔曼滤波更新步骤：
$$\begin{array}{rl}\mathbf{K}& =\mathbf{P}{\mathbf{H}}^{\top }{\left({\mathbf{H}\mathbf{P}\mathbf{H}}^{\top }+\mathbf{V}\right)}^{-1}\\ \widehat{\delta \mathbf{x}}& \leftarrow \mathbf{K}\left(\mathbf{y}-h\left({\hat{\mathbf{x}}}_t\right)\right)\\ \widehat{{\mathbf{x}}_t}& \leftarrow \mathbf{x}\oplus \widehat{\delta \mathbf{x}}\\ \mathbf{P}& \leftarrow (\mathbf{I}-\mathbf{K}\mathbf{H})\mathbf{P}(\mathbf{I}-\mathbf{K}\mathbf{H}{)}^{\top }+\mathbf{K}\mathbf{V}{\mathbf{K}}^{\top }\end{array}$$

其中，$\mathbf{y}$ 是测量值，$\mathbf{H}$ 是对误差状态量 $\delta \mathbf{x}$ 的测量矩阵，由于误差状态量为0，使用名义状态量作为真值状态量。

测量矩阵$\mathbf{H}$ 为：
$${\mathbf{H}\triangleq \frac{\partial h}{\partial \delta \mathbf{x}}|}_{\mathbf{x}}={{\frac{\partial h}{\partial {\mathbf{x}}_t}|}_{\mathbf{x}}\frac{\partial {\mathbf{x}}_t}{\partial \delta \mathbf{x}}|}_{\mathbf{x}}={\mathbf{H}}_{\mathbf{x}}{\mathbf{X}}_{\delta \mathbf{x}}$$

矩阵 ${\mathbf{H}}_{\mathbf{x}}$为 测量函数对真值状态量的雅可比矩阵，根据使用传感器的不同，其形式也不同。真值状态对误差状态矩阵 ${\mathbf{X}}_{\delta \mathbf{x}}$ 为：
$${\mathbf{X}}_{\delta \mathbf{x}}=\left[\begin{array}{cccccc}\frac{\partial (\mathbf{p}+\delta \mathbf{p})}{\partial \delta \mathbf{p}}& & & & & \\ & \frac{\partial (\mathbf{v}+\delta \mathbf{v})}{\partial \delta \mathbf{v}}& & & 0& \\ & & \frac{\partial (\mathbf{q}\otimes \delta \mathbf{q})}{\partial \delta \mathbf{\theta }}& & & \\ & & & \frac{\partial \left({\mathbf{a}}_b+\delta {\mathbf{a}}_b\right)}{\partial \delta {\mathbf{a}}_b}& & \\ & 0& & & \frac{\partial \left({\mathbf{\omega }}_b+\delta {\mathbf{\omega }}_b\right)}{\partial \delta {\mathbf{\omega }}_b}& \\ & & & & & \frac{\partial (\mathbf{g}+\delta \mathbf{g})}{\partial \delta \mathbf{g}}\end{array}\right]$$

进一步简化，可得：
$${\mathbf{X}}_{\delta \mathbf{x}}=\left[\begin{array}{ccc}{\mathbf{I}}_6& 0& 0\\ 0& {\mathbf{Q}}_{\delta \mathbf{\theta }}& 0\\ 0& 0& {\mathbf{I}}_9\end{array}\right]$$
其中，
$${\mathbf{Q}}_{\delta \mathbf{\theta }}=\frac{1}{2}\left[\begin{array}{ccc}-q_x& -q_y& -q_z\\ q_w& q_z& -q_y\\ -q_z& q_w& q_z\\ q_y& -q_x& q_w\end{array}\right]$$

接下来是更新状态量：
p ← p + δ p v ← v + δ v q ← q { δ θ ^ } ⊗ q a b ← a b + δ a b ω b ← ω b + δ ω b g ← g + δ g \begin{aligned} {\mathbf{p}} &\leftarrow\mathbf{p} + \delta\mathbf{p}\\ {\mathbf{v}} &\leftarrow\mathbf{v} + \delta\mathbf{v} \\ {\mathbf{q}} &\leftarrow\mathbf{q}\{\hat{\delta\boldsymbol{\theta}}\} \otimes \mathbf{q} \\ \mathbf{a}_b &\leftarrow \mathbf{a}_b + \delta\mathbf{a}_b \\ \boldsymbol{\omega}_b &\leftarrow \boldsymbol{\omega}_b +\delta\boldsymbol{\omega}_b\\ \mathbf{g}&\leftarrow\mathbf{g} + \delta\mathbf{g} \end{aligned} pvqab​ωb​g​←p+δp←v+δv←q{δθ^}⊗q←ab​+δab​←ωb​+δωb​←g+δg​

在误差卡尔曼滤波中，还需要对协方差矩阵 $\mathbf{P}$ 进行重置操作：
$$\mathbf{P}\leftarrow \mathbf{G}\mathbf{P}{\mathbf{G}}^{\top }$$
其中，矩阵 $\mathbf{G}$：
$$\mathbf{G}=\left[\begin{array}{ccc}{\mathbf{I}}_6& 0& 0\\ 0& \mathbf{I}+[\widehat{\frac{1}{2}\delta \mathbf{\theta }}{]}_{\times }& 0\\ 0& 0& {\mathbf{I}}_9\end{array}\right]$$

局部坐标和全局坐标方向误差总结如下表4所示：

至此，本文介绍完毕：）。